mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
416 lines
18 KiB
C
416 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 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));
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|