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 <relaytuning.h>
  47 #include <relaytuningsettings.h>
  48 #include <ratedesired.h>
  49 #include <sin_lookup.h>
  50 #include <stabilization.h>
  51 #include <innerloop.h>
  52 #include <outerloop.h>
  53 #include <altitudeloop.h>
  54
  55
  56 // Public variables
  57 StabilizationData stabSettings;
  58
  59 // Private variables
  60 static int cur_flight_mode = -1;
  61
  62 // Private functions
  63 static void SettingsUpdatedCb(UAVObjEvent *ev);
  64 static void BankUpdatedCb(UAVObjEvent *ev);
  65 static void SettingsBankUpdatedCb(UAVObjEvent *ev);
  66 static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
  67 static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
  68
  69 /**
  70  * Module initialization
  71  */
  72 int32_t StabilizationStart()
  73 {
  74     StabilizationSettingsConnectCallback(SettingsUpdatedCb);
  75     ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
  76     StabilizationBankConnectCallback(BankUpdatedCb);
  77     StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
  78     StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
  79     StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
  80     StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
  81     SettingsUpdatedCb(StabilizationSettingsHandle());
  82     StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
  83     FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
  84     BankUpdatedCb(StabilizationBankHandle());
  85
  86 #ifdef PIOS_INCLUDE_WDG
  87     PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
  88 #endif
  89     return 0;
  90 }
  91
  92 /**
  93  * Module initialization
  94  */
  95 int32_t StabilizationInitialize()
  96 {
  97     // Initialize variables
  98     StabilizationDesiredInitialize();
  99     StabilizationSettingsInitialize();
 100     StabilizationStatusInitialize();
 101     StabilizationBankInitialize();
 102     StabilizationSettingsBank1Initialize();
 103     StabilizationSettingsBank2Initialize();
 104     StabilizationSettingsBank3Initialize();
 105     RateDesiredInitialize();
 106     ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
 107     // Code required for relay tuning
 108     sin_lookup_initalize();
 109     RelayTuningSettingsInitialize();
 110     RelayTuningInitialize();
 111
 112     stabilizationOuterloopInit();
 113     stabilizationInnerloopInit();
 114 #ifdef REVOLUTION
 115     stabilizationAltitudeloopInit();
 116 #endif
 117     pid_zero(&stabSettings.outerPids[0]);
 118     pid_zero(&stabSettings.outerPids[1]);
 119     pid_zero(&stabSettings.outerPids[2]);
 120     pid_zero(&stabSettings.innerPids[0]);
 121     pid_zero(&stabSettings.innerPids[1]);
 122     pid_zero(&stabSettings.innerPids[2]);
 123     return 0;
 124 }
 125
 126 MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
 127
 128 static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 129 {
 130     StabilizationStatusData status;
 131     StabilizationDesiredStabilizationModeData mode;
 132     int t;
 133
 134     StabilizationDesiredStabilizationModeGet(&mode);
 135     for (t = 0; t < AXES; t++) {
 136         switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
 137         case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
 138             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 139             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
 140             break;
 141         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
 142             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 143             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 144             break;
 145         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
 146             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
 147             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 148             break;
 149         case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 150             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 151             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
 152             break;
 153         case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 154             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
 155             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 156             break;
 157         case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
 158             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 159             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
 160             break;
 161         case STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO:
 162             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 163             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_ACRO;
 164             break;
 165         case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
 166             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
 167             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
 168             break;
 169         case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
 170             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 171             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
 172             break;
 173         case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
 174             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
 175             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
 176             break;
 177         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
 178             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
 179             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 180             break;
 181         case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
 182             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
 183             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 184             break;
 185         case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
 186             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
 187             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
 188             break;
 189         }
 190     }
 191     StabilizationStatusSet(&status);
 192 }
 193
 194 static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 195 {
 196     uint8_t fm;
 197
 198     ManualControlCommandFlightModeSwitchPositionGet(&fm);
 199
 200     if (fm == cur_flight_mode) {
 201         return;
 202     }
 203     cur_flight_mode = fm;
 204     SettingsBankUpdatedCb(NULL);
 205 }
 206
 207 static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 208 {
 209     if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 210         return;
 211     }
 212     if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
 213                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
 214                  (stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
 215                  stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
 216         return;
 217     }
 218
 219
 220     switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
 221     case 0:
 222         StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
 223         break;
 224
 225     case 1:
 226         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
 227         break;
 228
 229     case 2:
 230         StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
 231         break;
 232     }
 233     StabilizationBankSet(&stabSettings.stabBank);
 234 }
 235
 236 static bool use_tps_for_roll()
 237 {
 238     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 239
 240     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 241            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 242            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 243            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
 244 }
 245
 246 static bool use_tps_for_pitch()
 247 {
 248     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 249
 250     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 251            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
 252            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 253            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
 254 }
 255
 256 static bool use_tps_for_yaw()
 257 {
 258     uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
 259
 260     return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
 261            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
 262            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
 263            axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
 264 }
 265
 266 static bool use_tps_for_p()
 267 {
 268     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 269
 270     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 271            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 272            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 273            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
 274 }
 275
 276 static bool use_tps_for_i()
 277 {
 278     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 279
 280     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 281            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
 282            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 283            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
 284 }
 285
 286 static bool use_tps_for_d()
 287 {
 288     uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
 289
 290     return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
 291            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
 292            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
 293            target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
 294 }
 295
 296 static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 297 {
 298     StabilizationBankGet(&stabSettings.stabBank);
 299
 300     // Set the roll rate PID constants
 301     pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
 302                   stabSettings.stabBank.RollRatePID.Ki,
 303                   stabSettings.stabBank.RollRatePID.Kd,
 304                   stabSettings.stabBank.RollRatePID.ILimit);
 305
 306     // Set the pitch rate PID constants
 307     pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
 308                   stabSettings.stabBank.PitchRatePID.Ki,
 309                   stabSettings.stabBank.PitchRatePID.Kd,
 310                   stabSettings.stabBank.PitchRatePID.ILimit);
 311
 312     // Set the yaw rate PID constants
 313     pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
 314                   stabSettings.stabBank.YawRatePID.Ki,
 315                   stabSettings.stabBank.YawRatePID.Kd,
 316                   stabSettings.stabBank.YawRatePID.ILimit);
 317
 318     // Set the roll attitude PI constants
 319     pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
 320                   stabSettings.stabBank.RollPI.Ki,
 321                   0,
 322                   stabSettings.stabBank.RollPI.ILimit);
 323
 324     // Set the pitch attitude PI constants
 325     pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
 326                   stabSettings.stabBank.PitchPI.Ki,
 327                   0,
 328                   stabSettings.stabBank.PitchPI.ILimit);
 329
 330     // Set the yaw attitude PI constants
 331     pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
 332                   stabSettings.stabBank.YawPI.Ki,
 333                   0,
 334                   stabSettings.stabBank.YawPI.ILimit);
 335
 336     bool tps_for_axis[3] = {
 337         use_tps_for_roll(),
 338         use_tps_for_pitch(),
 339         use_tps_for_yaw()
 340     };
 341     bool tps_for_pid[3] = {
 342         use_tps_for_p(),
 343         use_tps_for_i(),
 344         use_tps_for_d()
 345     };
 346     for (int axis = 0; axis < 3; axis++) {
 347         for (int pid = 0; pid < 3; pid++) {
 348             stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
 349                                                                  && tps_for_axis[axis]
 350                                                                  && tps_for_pid[pid];
 351         }
 352     }
 353 }
 354
 355
 356 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 357 {
 358     // needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
 359     StabilizationSettingsGet(&stabSettings.settings);
 360
 361     // Set up the derivative term
 362     pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
 363
 364     // The dT has some jitter iteration to iteration that we don't want to
 365     // make thie result unpredictable.  Still, it's nicer to specify the constant
 366     // based on a time (in ms) rather than a fixed multiplier.  The error between
 367     // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
 368     // calculation
 369     const float fakeDt = 0.0025f;
 370     if (stabSettings.settings.GyroTau < 0.0001f) {
 371         stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
 372     } else {
 373         stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
 374     }
 375
 376     // force flight mode update
 377     cur_flight_mode = -1;
 378
 379     // Rattitude stick angle where the attitude to rate transition happens
 380     if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
 381         stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
 382     } else {
 383         stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
 384     }
 385
 386     stabSettings.cruiseControl.min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
 387     stabSettings.cruiseControl.max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
 388     stabSettings.cruiseControl.thrust_difference = stabSettings.cruiseControl.max_thrust - stabSettings.cruiseControl.min_thrust;
 389
 390     stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
 391     stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f;
 392     stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor));
 393 }
 394
 395 /**
 396  * @}
 397  * @}
 398  */