1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/modules/Stabilization/stabilization.c
2016-06-18 14:08:05 -04:00

417 lines
18 KiB
C

/**
******************************************************************************
* @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 AttitudeState "Attitude State"
* @{
*
* @file stabilization.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
* 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 <pid.h>
#include <manualcontrolcommand.h>
#include <flightmodesettings.h>
#include <stabilizationsettings.h>
#include <stabilizationdesired.h>
#include <stabilizationstatus.h>
#include <stabilizationbank.h>
#include <stabilizationsettingsbank1.h>
#include <stabilizationsettingsbank2.h>
#include <stabilizationsettingsbank3.h>
#include <ratedesired.h>
#include <sin_lookup.h>
#include <stabilization.h>
#include <innerloop.h>
#include <outerloop.h>
#include <altitudeloop.h>
// Public variables
StabilizationData stabSettings;
// Private variables
static int cur_flight_mode = -1;
// Private functions
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
/**
* Module initialization
*/
int32_t StabilizationStart()
{
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
BankUpdatedCb(StabilizationBankHandle());
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationStatusInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
RateDesiredInitialize();
ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
sin_lookup_initalize();
stabilizationOuterloopInit();
stabilizationInnerloopInit();
#ifdef REVOLUTION
stabilizationAltitudeloopInit();
#endif
pid_zero(&stabSettings.outerPids[0]);
pid_zero(&stabSettings.outerPids[1]);
pid_zero(&stabSettings.outerPids[2]);
pid_zero(&stabSettings.innerPids[0]);
pid_zero(&stabSettings.innerPids[1]);
pid_zero(&stabSettings.innerPids[2]);
return 0;
}
MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationStatusData status;
StabilizationDesiredStabilizationModeData mode;
int t;
StabilizationDesiredStabilizationModeGet(&mode);
for (t = 0; t < AXES; t++) {
switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
// roll or pitch
if (t <= 1) {
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
}
// else yaw (other modes don't worry about invalid thrust mode either)
else {
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
}
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT;
break;
#else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// no break, do not reorder this code
// for low power FCs just fall through to Attitude mode
// that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// do not reorder this code
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_ACRO;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break;
}
}
StabilizationStatusSet(&status);
}
static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t fm;
ManualControlCommandFlightModeSwitchPositionGet(&fm);
if (fm == cur_flight_mode) {
return;
}
cur_flight_mode = fm;
SettingsBankUpdatedCb(NULL);
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
return;
}
if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
return;
}
switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
break;
}
StabilizationBankSet(&stabSettings.stabBank);
}
static bool use_tps_for_roll()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
}
static bool use_tps_for_pitch()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
}
static bool use_tps_for_yaw()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
}
static bool use_tps_for_p()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
}
static bool use_tps_for_i()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
}
static bool use_tps_for_d()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankGet(&stabSettings.stabBank);
// Set the roll rate PID constants
pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
stabSettings.stabBank.RollRatePID.Ki,
stabSettings.stabBank.RollRatePID.Kd,
stabSettings.stabBank.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
stabSettings.stabBank.PitchRatePID.Ki,
stabSettings.stabBank.PitchRatePID.Kd,
stabSettings.stabBank.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
stabSettings.stabBank.YawRatePID.Ki,
stabSettings.stabBank.YawRatePID.Kd,
stabSettings.stabBank.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
stabSettings.stabBank.RollPI.Ki,
0,
stabSettings.stabBank.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
stabSettings.stabBank.PitchPI.Ki,
0,
stabSettings.stabBank.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
stabSettings.stabBank.YawPI.Ki,
0,
stabSettings.stabBank.YawPI.ILimit);
bool tps_for_axis[3] = {
use_tps_for_roll(),
use_tps_for_pitch(),
use_tps_for_yaw()
};
bool tps_for_pid[3] = {
use_tps_for_p(),
use_tps_for_i(),
use_tps_for_d()
};
for (int axis = 0; axis < 3; axis++) {
for (int pid = 0; pid < 3; pid++) {
stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
&& tps_for_axis[axis]
&& tps_for_pid[pid];
}
}
for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) {
stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f;
}
stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
StabilizationSettingsGet(&stabSettings.settings);
// Set up the derivative term
pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025f;
if (stabSettings.settings.GyroTau < 0.0001f) {
stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
} else {
stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
}
// force flight mode update
cur_flight_mode = -1;
// Rattitude stick angle where the attitude to rate transition happens
if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
} else {
stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
}
stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
}
/**
* @}
* @}
*/