flight/modules/Stabilization/stabilization.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup StabilizationModule Stabilization Module
   6  * @brief Stabilization PID loops in an airframe type independent manner
   7  * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
   8  * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
   9  * @{
  10  *
  11  * @file       stabilization.c
  12  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  13  * @brief      Attitude stabilization module.
  14  *
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34
  35 #include <openpilot.h>
  36 #include <pid.h>
  37 #include <manualcontrolcommand.h>
  38 #include <flightmodesettings.h>
  39 #include <stabilizationsettings.h>
  40 #include <stabilizationdesired.h>
  41 #include <stabilizationstatus.h>
  42 #include <stabilizationbank.h>
  43 #include <stabilizationsettingsbank1.h>
  44 #include <stabilizationsettingsbank2.h>
  45 #include <stabilizationsettingsbank3.h>
  46 #include <ratedesired.h>
  47 #include <sin_lookup.h>
  48 #include <stabilization.h>
  49 #include <innerloop.h>
  50 #include <outerloop.h>
  51 #include <altitudeloop.h>
  52
  53
  54 // Public variables
  55 StabilizationData stabSettings;
  56
  57 // Private variables
  58 static int cur_flight_mode = -1;
  59
  60 // Private functions
  61 static void SettingsUpdatedCb(UAVObjEvent *ev);
  62 static void BankUpdatedCb(UAVObjEvent *ev);
  63 static void SettingsBankUpdatedCb(UAVObjEvent *ev);
  64 static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
  65 static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
  66
  67 /**
  68  * Module initialization
  69  */
  70 int32_t StabilizationStart()
  71 {
  72     StabilizationSettingsConnectCallback(SettingsUpdatedCb);
  73     ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
  74     StabilizationBankConnectCallback(BankUpdatedCb);
  75     StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
  76     StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
  77     StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
  78     StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
  79     SettingsUpdatedCb(StabilizationSettingsHandle());
  80     StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
  81     FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
  82     BankUpdatedCb(StabilizationBankHandle());
  83
  84 #ifdef PIOS_INCLUDE_WDG
  85     PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
  86 #endif
  87     return 0;
  88 }
  89
  90 /**
  91  * Module initialization
  92  */
  93 int32_t StabilizationInitialize()
  94 {
  95     // Initialize variables
  96     StabilizationDesiredInitialize();
  97     StabilizationSettingsInitialize();
  98     StabilizationStatusInitialize();
  99     StabilizationBankInitialize();
 100     StabilizationSettingsBank1Initialize();
 101     StabilizationSettingsBank2Initialize();
 102     StabilizationSettingsBank3Initialize();
 103     RateDesiredInitialize();
 104     ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
 105     sin_lookup_initalize();
 106
 107     stabilizationOuterloopInit();
 108     stabilizationInnerloopInit();
 109 #ifdef REVOLUTION
 110     stabilizationAltitudeloopInit();
 111 #endif
 112     pid_zero(&stabSettings.outerPids[0]);
 113     pid_zero(&stabSettings.outerPids[1]);
 114     pid_zero(&stabSettings.outerPids[2]);
 115     pid_zero(&stabSettings.innerPids[0]);
 116     pid_zero(&stabSettings.innerPids[1]);
 117     pid_zero(&stabSettings.innerPids[2]);
 118     return 0;
 119 }
 120
 121 MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 122
 123 static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 124 {
 125     StabilizationStatusData status;
 126     StabilizationDesiredStabilizationModeData mode;
 127     int t;
 128
 129     StabilizationDesiredStabilizationModeGet(&mode);
 130     for (t = 0; t < AXES; t++) {
 131         switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
 132         case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
 133             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 134             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
 135             break;
 136         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
 137             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 138             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 139             break;
 140         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
 141             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
 142             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 143             break;
 144         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
 145             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
 146             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 147             break;
 148         case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 149             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 150             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
 151             break;
 152         case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 153             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
 154             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 155             break;
 156         case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
 157             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 158             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
 159             break;
 160         case STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO:
 161             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 162             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_ACRO;
 163             break;
 164         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
 165             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
 166             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 167             break;
 168         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
 169             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
 170             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 171             break;
 172         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
 173             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
 174             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 175             break;
 176         case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
 177             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 178             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 179             break;
 180         }
 181     }
 182     StabilizationStatusSet(&status);
 183 }
 184
 185 static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 186 {
 187     uint8_t fm;
 188
 189     ManualControlCommandFlightModeSwitchPositionGet(&fm);
 190
 191     if (fm == cur_flight_mode) {
 192         return;
 193     }
 194     cur_flight_mode = fm;
 195     SettingsBankUpdatedCb(NULL);
 196 }
 197
 198 static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 199 {
 200     if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 201         return;
 202     }
 203     if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
 204                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
 205                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
 206                  stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
 207         return;
 208     }
 209
 210
 211     switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
 212     case 0:
 213         StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
 214         break;
 215
 216     case 1:
 217         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
 218         break;
 219
 220     case 2:
 221         StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
 222         break;
 223     }
 224     StabilizationBankSet(&stabSettings.stabBank);
 225 }
 226
 227 static bool use_tps_for_roll()
 228 {
 229     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 230
 231     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 232            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 233            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 234            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
 235 }
 236
 237 static bool use_tps_for_pitch()
 238 {
 239     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 240
 241     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 242            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 243            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 244            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
 245 }
 246
 247 static bool use_tps_for_yaw()
 248 {
 249     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 250
 251     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 252            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 253            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 254            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
 255 }
 256
 257 static bool use_tps_for_p()
 258 {
 259     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 260
 261     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 262            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 263            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 264            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
 265 }
 266
 267 static bool use_tps_for_i()
 268 {
 269     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 270
 271     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 272            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 273            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 274            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
 275 }
 276
 277 static bool use_tps_for_d()
 278 {
 279     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 280
 281     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 282            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 283            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 284            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
 285 }
 286
 287 static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 288 {
 289     StabilizationBankGet(&stabSettings.stabBank);
 290
 291     // Set the roll rate PID constants
 292     pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
 293                   stabSettings.stabBank.RollRatePID.Ki,
 294                   stabSettings.stabBank.RollRatePID.Kd,
 295                   stabSettings.stabBank.RollRatePID.ILimit);
 296
 297     // Set the pitch rate PID constants
 298     pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
 299                   stabSettings.stabBank.PitchRatePID.Ki,
 300                   stabSettings.stabBank.PitchRatePID.Kd,
 301                   stabSettings.stabBank.PitchRatePID.ILimit);
 302
 303     // Set the yaw rate PID constants
 304     pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
 305                   stabSettings.stabBank.YawRatePID.Ki,
 306                   stabSettings.stabBank.YawRatePID.Kd,
 307                   stabSettings.stabBank.YawRatePID.ILimit);
 308
 309     // Set the roll attitude PI constants
 310     pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
 311                   stabSettings.stabBank.RollPI.Ki,
 312                   0,
 313                   stabSettings.stabBank.RollPI.ILimit);
 314
 315     // Set the pitch attitude PI constants
 316     pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
 317                   stabSettings.stabBank.PitchPI.Ki,
 318                   0,
 319                   stabSettings.stabBank.PitchPI.ILimit);
 320
 321     // Set the yaw attitude PI constants
 322     pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
 323                   stabSettings.stabBank.YawPI.Ki,
 324                   0,
 325                   stabSettings.stabBank.YawPI.ILimit);
 326
 327     bool tps_for_axis[3] = {
 328         use_tps_for_roll(),
 329         use_tps_for_pitch(),
 330         use_tps_for_yaw()
 331     };
 332     bool tps_for_pid[3] = {
 333         use_tps_for_p(),
 334         use_tps_for_i(),
 335         use_tps_for_d()
 336     };
 337     for (int axis = 0; axis < 3; axis++) {
 338         for (int pid = 0; pid < 3; pid++) {
 339             stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
 340                                                                  && tps_for_axis[axis]
 341                                                                  && tps_for_pid[pid];
 342         }
 343     }
 344 }
 345
 346
 347 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 348 {
 349     // needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
 350     StabilizationSettingsGet(&stabSettings.settings);
 351
 352     // Set up the derivative term
 353     pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
 354
 355     // The dT has some jitter iteration to iteration that we don't want to
 356     // make thie result unpredictable.  Still, it's nicer to specify the constant
 357     // based on a time (in ms) rather than a fixed multiplier.  The error between
 358     // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
 359     // calculation
 360     const float fakeDt = 0.0025f;
 361     if (stabSettings.settings.GyroTau < 0.0001f) {
 362         stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
 363     } else {
 364         stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
 365     }
 366
 367     // force flight mode update
 368     cur_flight_mode = -1;
 369
 370     // Rattitude stick angle where the attitude to rate transition happens
 371     if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
 372         stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
 373     } else {
 374         stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
 375     }
 376
 377     stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
 378     stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
 379     stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
 380
 381     stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
 382     stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
 383     stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
 384
 385     for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) {
 386         stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f;
 387     }
 388     stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
 389     stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
 390     stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
 391 }
 392
 393 /**
 394  * @}
 395  * @}
 396  */