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     StabilizationSettingsInitialize();
  99     StabilizationStatusInitialize();
 100     StabilizationBankInitialize();
 101     StabilizationSettingsBank1Initialize();
 102     StabilizationSettingsBank2Initialize();
 103     StabilizationSettingsBank3Initialize();
 104     RateDesiredInitialize();
 105     ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
 106     sin_lookup_initalize();
 107
 108     stabilizationOuterloopInit();
 109     stabilizationInnerloopInit();
 110 #ifdef REVOLUTION
 111     stabilizationAltitudeloopInit();
 112 #endif
 113     pid_zero(&stabSettings.outerPids[0]);
 114     pid_zero(&stabSettings.outerPids[1]);
 115     pid_zero(&stabSettings.outerPids[2]);
 116     pid_zero(&stabSettings.innerPids[0]);
 117     pid_zero(&stabSettings.innerPids[1]);
 118     pid_zero(&stabSettings.innerPids[2]);
 119     return 0;
 120 }
 121
 122 MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 123
 124 static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 125 {
 126     StabilizationStatusData status;
 127     StabilizationDesiredStabilizationModeData mode;
 128     int t;
 129
 130     StabilizationDesiredStabilizationModeGet(&mode);
 131     for (t = 0; t < AXES; t++) {
 132         switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
 133         case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
 134             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 135             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
 136             break;
 137         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
 138             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 139             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 140             break;
 141         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
 142             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
 143             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 144             break;
 145         case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
 146 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 147             // roll or pitch
 148             if (t <= 1) {
 149                 StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
 150             }
 151             // else yaw (other modes don't worry about invalid thrust mode either)
 152             else {
 153                 StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 154             }
 155             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT;
 156             break;
 157 #else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 158             // no break, do not reorder this code
 159             // for low power FCs just fall through to Attitude mode
 160             // that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
 161 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 162 // do not reorder this code
 163         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
 164             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
 165             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 166             break;
 167         case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 168             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 169             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
 170             break;
 171         case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 172             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
 173             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 174             break;
 175         case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
 176             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 177             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
 178             break;
 179         case STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO:
 180             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 181             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_ACRO;
 182             break;
 183         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
 184             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
 185             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 186             break;
 187         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
 188             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
 189             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 190             break;
 191         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
 192             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
 193             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 194             break;
 195         case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
 196             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 197             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 198             break;
 199         }
 200     }
 201     StabilizationStatusSet(&status);
 202 }
 203
 204 static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 205 {
 206     uint8_t fm;
 207
 208     ManualControlCommandFlightModeSwitchPositionGet(&fm);
 209
 210     if (fm == cur_flight_mode) {
 211         return;
 212     }
 213     cur_flight_mode = fm;
 214     SettingsBankUpdatedCb(NULL);
 215 }
 216
 217 static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 218 {
 219     if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 220         return;
 221     }
 222     if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
 223                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
 224                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
 225                  stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
 226         return;
 227     }
 228
 229
 230     switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
 231     case 0:
 232         StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
 233         break;
 234
 235     case 1:
 236         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
 237         break;
 238
 239     case 2:
 240         StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
 241         break;
 242     }
 243     StabilizationBankSet(&stabSettings.stabBank);
 244 }
 245
 246 static bool use_tps_for_roll()
 247 {
 248     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 249
 250     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 251            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 252            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 253            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
 254 }
 255
 256 static bool use_tps_for_pitch()
 257 {
 258     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 259
 260     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 261            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 262            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 263            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
 264 }
 265
 266 static bool use_tps_for_yaw()
 267 {
 268     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 269
 270     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 271            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 272            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 273            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
 274 }
 275
 276 static bool use_tps_for_p()
 277 {
 278     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 279
 280     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 281            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 282            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 283            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
 284 }
 285
 286 static bool use_tps_for_i()
 287 {
 288     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 289
 290     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 291            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 292            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 293            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
 294 }
 295
 296 static bool use_tps_for_d()
 297 {
 298     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 299
 300     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 301            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 302            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 303            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
 304 }
 305
 306 static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 307 {
 308     StabilizationBankGet(&stabSettings.stabBank);
 309
 310     // Set the roll rate PID constants
 311     pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
 312                   stabSettings.stabBank.RollRatePID.Ki,
 313                   stabSettings.stabBank.RollRatePID.Kd,
 314                   stabSettings.stabBank.RollRatePID.ILimit);
 315
 316     // Set the pitch rate PID constants
 317     pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
 318                   stabSettings.stabBank.PitchRatePID.Ki,
 319                   stabSettings.stabBank.PitchRatePID.Kd,
 320                   stabSettings.stabBank.PitchRatePID.ILimit);
 321
 322     // Set the yaw rate PID constants
 323     pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
 324                   stabSettings.stabBank.YawRatePID.Ki,
 325                   stabSettings.stabBank.YawRatePID.Kd,
 326                   stabSettings.stabBank.YawRatePID.ILimit);
 327
 328     // Set the roll attitude PI constants
 329     pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
 330                   stabSettings.stabBank.RollPI.Ki,
 331                   0,
 332                   stabSettings.stabBank.RollPI.ILimit);
 333
 334     // Set the pitch attitude PI constants
 335     pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
 336                   stabSettings.stabBank.PitchPI.Ki,
 337                   0,
 338                   stabSettings.stabBank.PitchPI.ILimit);
 339
 340     // Set the yaw attitude PI constants
 341     pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
 342                   stabSettings.stabBank.YawPI.Ki,
 343                   0,
 344                   stabSettings.stabBank.YawPI.ILimit);
 345
 346     bool tps_for_axis[3] = {
 347         use_tps_for_roll(),
 348         use_tps_for_pitch(),
 349         use_tps_for_yaw()
 350     };
 351     bool tps_for_pid[3] = {
 352         use_tps_for_p(),
 353         use_tps_for_i(),
 354         use_tps_for_d()
 355     };
 356     for (int axis = 0; axis < 3; axis++) {
 357         for (int pid = 0; pid < 3; pid++) {
 358             stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
 359                                                                  && tps_for_axis[axis]
 360                                                                  && tps_for_pid[pid];
 361         }
 362     }
 363
 364     for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) {
 365         stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f;
 366     }
 367
 368     stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
 369     stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
 370     stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
 371 }
 372
 373
 374 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 375 {
 376     // needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
 377     StabilizationSettingsGet(&stabSettings.settings);
 378
 379     // Set up the derivative term
 380     pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
 381
 382     // The dT has some jitter iteration to iteration that we don't want to
 383     // make thie result unpredictable.  Still, it's nicer to specify the constant
 384     // based on a time (in ms) rather than a fixed multiplier.  The error between
 385     // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
 386     // calculation
 387     const float fakeDt = 0.0025f;
 388     if (stabSettings.settings.GyroTau < 0.0001f) {
 389         stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
 390     } else {
 391         stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
 392     }
 393
 394     // force flight mode update
 395     cur_flight_mode = -1;
 396
 397     // Rattitude stick angle where the attitude to rate transition happens
 398     if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
 399         stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
 400     } else {
 401         stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
 402     }
 403
 404     stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
 405     stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
 406     stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
 407
 408     stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
 409     stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
 410     stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
 411 }
 412
 413 /**
 414  * @}
 415  * @}
 416  */