mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-14 21:23:52 +01:00
a7c592b294
Both TPS and Acroplus had settings initialized in SettingsUpdatedCb. These values comes from stabSettings.stabBank, which isn't initialzed when SettingsUpdatedCb is first called. The patch moves the setup of the affected settings values to BankUpdatedCb, where other bank specific settings are updated.
398 lines
16 KiB
C
398 lines
16 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_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));
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|