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