rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Moved non primary Stabilization modules into Branches

Browse Source 
simple		- outdated - won't compile
Experimental	- outdated - won't compile
les		- development version - not stable
les+corvus	- development version - not stable




git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1993 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2010-10-19 19:12:37 +00:00 committed by corvus
parent 4ed3359f6f
commit 50574841bd
4 changed files with 0 additions and 764 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
flight/OpenPilot/Modules/Stabilization/experimental/Stabilization/inc/stabilization.h
View File

@ -1,43 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef STABILIZATION_H
#define STABILIZATION_H
int32_t StabilizationInitialize();
#endif // STABILIZATION_H
/**
* @}
* @}
*/

309
flight/OpenPilot/Modules/Stabilization/experimental/Stabilization/stabilization.c
View File

@ -1,309 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "actuatordesired.h"
#include "attitudedesired.h"
#include "attitudeactual.h"
#include "manualcontrolcommand.h"
#include "systemsettings.h"
// Private constants
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define PITCH_INTEGRAL_LIMIT 0.5
#define ROLL_INTEGRAL_LIMIT 0.5
#define YAW_INTEGRAL_LIMIT 0.5
#define DEG2RAD ( M_PI / 180.0 )
#define FAILSAFE_TIMEOUT_MS 100
// Private types
// Private variables
static xQueueHandle queue;
static xTaskHandle taskHandle;
// Private functions
static void stabilizationTask(void* parameters);
static float bound(float val, float min, float max);
static float angleDifference(float val);
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for AttitudeActual updates.
AttitudeActualConnectQueue(queue);
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
float dT = 1;
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
portTickType thisSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
float yawError, yawErrorLast;
float rollError, rollErrorLast;
float pitchDerivative;
float yawDerivative;
float rollDerivative;
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
float yawPrevious;
float yawChange;
// Initialize
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
yawPrevious = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
// global pitch error
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
pitchErrorGlobal = angleDifference(
bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
}
// global yaw error
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
yawPrevious = attitudeActual.Yaw;
yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
} else { // heading stabilization
yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
}
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
}
// local pitch error
pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
// local roll error (no translation needed - always local)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
rollError = angleDifference(
bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
}
// local yaw error
yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
// pitch last
pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
// yaw last
yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
// global last variables are no longer needed
pitchErrorLastGlobal = pitchErrorGlobal;
yawErrorLastGlobal = yawErrorGlobal;
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
else
{
actuatorDesired.Yaw = 0.0;
}
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
ActuatorDesiredSet(&actuatorDesired);
}
else
{
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if ( val < min )
{
val = min;
}
else if ( val > max )
{
val = max;
}
return val;
}
/**
* Fix result of angular differences
*/
static float angleDifference(float val)
{
while ( val < -180.0 )
{
val += 360.0;
}
while ( val > 180.0 )
{
val -= 360.0;
}
return val;
}
/**
* @}
* @}
*/

43
flight/OpenPilot/Modules/Stabilization/les+corvus/Stabilization/inc/stabilization.h
View File

@ -1,43 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef STABILIZATION_H
#define STABILIZATION_H
int32_t StabilizationInitialize();
#endif // STABILIZATION_H
/**
* @}
* @}
*/

369
flight/OpenPilot/Modules/Stabilization/les+corvus/Stabilization/stabilization.c
View File

@ -1,369 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "actuatordesired.h"
#include "attitudedesired.h"
#include "attitudeactual.h"
#include "attituderaw.h"
#include "manualcontrolcommand.h"
#include "systemsettings.h"
#include "ahrssettings.h"
// Private constants
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define FAILSAFE_TIMEOUT_MS 100
#define DEG2RAD ( M_PI / 180.0 )
// Stabilizisation variant
enum {TRANSLATE_NONE, TRANSLATE_ATTITUDE, TRANSLATE_RATES, TRANSLATE_ACTUATORS};
#define TRANSLATE_COORDS TRANSLATE_ATTITUDE
// TRANSLATE_NONE <-- no coordinate translation - old behaviour - no rotation
// TRANSLATE_ATTITUDE <-- suggestion by corvus - rotate attitude error into local reference frame
// TRANSLATE_RATES <-- rotate rate error into local reference frame
// TRANSLATE_ACTUATORS <-- rotate actuator demands into local reference frame
// WARNING: MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE
// will behave differently depending on whether and when translation takes place
// "none" and "attitude" - rates will be stabilized in local reference frame
// "rates" and "actuators" - rates will be stabilized in global reference frame
enum {METHOD_ROLL, METHOD_FULL};
#define TRANSLATE_METHOD METHOD_ROLL
// WARNING: experimental feature "full" is untested!
enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX};
enum {ROLL,PITCH,YAW,MAX_AXES};
// Private types
typedef struct {
float p;
float i;
float d;
float iLim;
float iAccumulator;
float lastErr;
} pid_type;
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float dT = 1;
pid_type pids[PID_MAX];
// Private functions
static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float error);
static float bound(float val);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev);
static void calcDifference(float * values, float * reference, const uint8_t angular);
static void translateValues(float * values, float * reference);
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
ManualControlCommandData manualControl;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids();
while(1) {
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
SystemSettingsGet(&systemSettings);
float *manualAxis = &manualControl.Roll;
float *attitudeDesiredAxis = &attitudeDesired.Roll;
float *attitudeActualAxis = &attitudeActual.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
// calculate attitude errors
calcDifference( attitudeDesiredAxis, attitudeActualAxis, 1);
#if TRANSLATE_COORDS == TRANSLATE_ATTITUDE
//Translate Attitude to local reference frame.
translateValues(attitudeDesiredAxis, attitudeActualAxis);
#endif
//Calculate desired rate
float rates[MAX_AXES]= {0,0,0};
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
switch(manualControl.StabilizationSettings[ct])
{
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
rates[ct] = manualAxis[ct] * settings.ManualRate[ct];
break;
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
rates[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct]);
break;
}
if(fabs(rates[ct]) > settings.MaximumRate[ct])
{
if(rates[ct] > 0)
{
rates[ct] = settings.MaximumRate[ct];
}else
{
rates[ct] = -settings.MaximumRate[ct];
}
}
}
uint8_t shouldUpdate = 0;
ActuatorDesiredGet(&actuatorDesired);
// calculate rate errors
calcDifference( rates, attitudeRaw.gyros_filtered, 0);
#if TRANSLATE_COORDS == TRANSLATE_RATES
//Translate rate errors to local reference frame.
translateValues(rates, attitudeActualAxis);
#endif
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
switch(manualControl.StabilizationSettings[ct])
{
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE:
case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rates[ct]);
actuatorDesiredAxis[ct] = bound(command);
shouldUpdate = 1;
break;
}
}
}
#if TRANSLATE_COORDS == TRANSLATE_ACTUATORS
//Translate Actuator settings to local reference frame.
translateValues(actuatorDesiredAxis,attitudeActualAxis);
#endif
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL)
{
shouldUpdate = 0;
}
if(shouldUpdate)
{
actuatorDesired.Throttle = attitudeDesired.Throttle;
ActuatorDesiredSet(&actuatorDesired);
}
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE ||
!shouldUpdate || (attitudeDesired.Throttle < 0))
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
float ApplyPid(pid_type * pid, const float err)
{
float diff = (err - pid->lastErr);
pid->lastErr = err;
pid->iAccumulator += err * pid->i * dT;
if(fabs(pid->iAccumulator) > pid->iLim) {
if(pid->iAccumulator >0) {
pid->iAccumulator = pid->iLim;
} else {
pid->iAccumulator = -pid->iLim;
}
}
return ((err * pid->p) + pid->iAccumulator + (diff * pid->d / dT));
}
static void ZeroPids(void)
{
for(int8_t ct = 0; ct < PID_MAX; ct++) {
pids[ct].iAccumulator = 0;
pids[ct].lastErr = 0;
}
}
/**
* Bound input value between limits
*/
static float bound(float val)
{
if(val < -1) {
val = -1;
} else if(val > 1) {
val = 1;
}
return val;
}
/**
* calculate difference vector
*/
static void calcDifference(float * values, float * reference, const uint8_t angular)
{
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
values[ct] = values[ct] - reference[ct];
if (angular) {
if (values[ct] > 180.) values[ct] -= 360.;
if (values[ct] < -180.) values[ct] += 360.;
}
}
}
/**
* translate rotational vector into local reference frame
*/
static void translateValues(float * values, float * reference)
{
float tmp[MAX_AXES];
#if TRANSLATE_METHOD == METHOD_FULL && TRANSLATE_COORDS == TRANSLATE_ATTITUDE
// UNTESTED!!! (and also likely unnecessary since neglectible for small values)
// adjust PITCH to corect the (PITCH) difference between a YAW rotation around the
// vertical axis and a YAW rotation around the local vertical axis
// WARNING!!! This only makes sense if values[YAW] is an angle.
// Therefore it cannot work for rates and/or actuatorDemands
values[PITCH] = values[PITCH] + ( reference[PITCH] - cos( DEG2RAD * values[YAW] ) * reference[PITCH] );
#endif
// traditional translation: rotate YAW and PITCH by roll
tmp[PITCH] = cos( DEG2RAD * reference[ROLL] ) * values[PITCH]
+ sin( DEG2RAD * reference[ROLL] ) * values[YAW];
tmp[YAW] = cos( DEG2RAD * reference[ROLL] ) * values[YAW]
+ sin( DEG2RAD * reference[ROLL] ) * values[PITCH];
values[PITCH] = tmp[PITCH];
values[YAW] = tmp[YAW];
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
memset(pids,0,sizeof (pid_type) * PID_MAX);
StabilizationSettingsGet(&settings);
float * data = settings.RollRatePI;
for(int8_t pid=0; pid < PID_MAX; pid++)
{
pids[pid].p = *data++;
pids[pid].i = *data++;
pids[pid].iLim = *data++;
}
}
/**
* @}
* @}
*/