1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-14 21:23:52 +01:00
LibrePilot/flight/modules/Stabilization/stabilization.c
Stefan Karlsson a7c592b294 LP-115 Fix TPS and Acroplus settings setup
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.
2015-09-09 01:27:20 +02:00

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));
}
/**
* @}
* @}
*/