flight/modules/Stabilization/outerloop.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       outerloop.c
  12  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
  13  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  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 #include <openpilot.h>
  36 #include <pid.h>
  37 #include <callbackinfo.h>
  38 #include <ratedesired.h>
  39 #include <stabilizationdesired.h>
  40 #include <attitudestate.h>
  41 #include <stabilizationstatus.h>
  42 #include <flightstatus.h>
  43 #include <manualcontrolcommand.h>
  44 #include <stabilizationbank.h>
  45
  46
  47 #include <stabilization.h>
  48 #include <cruisecontrol.h>
  49 #include <altitudeloop.h>
  50 #include <CoordinateConversions.h>
  51
  52 // Private constants
  53
  54 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
  55
  56 #define UPDATE_EXPECTED   (1.0f / PIOS_SENSOR_RATE)
  57 #define UPDATE_MIN        1.0e-6f
  58 #define UPDATE_MAX        1.0f
  59 #define UPDATE_ALPHA      1.0e-2f
  60
  61 // Private variables
  62 static DelayedCallbackInfo *callbackHandle;
  63 static AttitudeStateData attitude;
  64
  65 static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
  66 static PiOSDeltatimeConfig timeval;
  67 static bool pitchMin = false;
  68 static bool pitchMax = false;
  69 static bool rollMin  = false;
  70 static bool rollMax  = false;
  71
  72 // Private functions
  73 static void stabilizationOuterloopTask();
  74 static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  75
  76 void stabilizationOuterloopInit()
  77 {
  78     RateDesiredInitialize();
  79     StabilizationDesiredInitialize();
  80     AttitudeStateInitialize();
  81     StabilizationStatusInitialize();
  82     FlightStatusInitialize();
  83     ManualControlCommandInitialize();
  84
  85     PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
  86
  87     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
  88     AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
  89 }
  90
  91
  92 /**
  93  * WARNING! This callback executes with critical flight control priority every
  94  * time a gyroscope update happens do NOT put any time consuming calculations
  95  * in this loop unless they really have to execute with every gyro update
  96  */
  97 static void stabilizationOuterloopTask()
  98 {
  99     AttitudeStateData attitudeState;
 100     RateDesiredData rateDesired;
 101     StabilizationDesiredData stabilizationDesired;
 102     StabilizationStatusOuterLoopData enabled;
 103
 104     AttitudeStateGet(&attitudeState);
 105     StabilizationDesiredGet(&stabilizationDesired);
 106     RateDesiredGet(&rateDesired);
 107     StabilizationStatusOuterLoopGet(&enabled);
 108     float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
 109     float *rateDesiredAxis = &rateDesired.Roll;
 110     int t;
 111     float dT    = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 112     StabilizationStatusOuterLoopOptions newThrustMode = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
 113     bool reinit = (newThrustMode != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
 114
 115 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 116     // Trigger a disable message to the alt hold on reinit to prevent that loop from running when not in use.
 117     if (reinit) {
 118         if (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
 119             previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO) {
 120             if (newThrustMode != STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE && newThrustMode != STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO) {
 121                 // disable the altvario velocity control loop
 122                 stabilizationDisableAltitudeHold();
 123             }
 124         }
 125     }
 126 #endif
 127     // update previous mode
 128     previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = newThrustMode;
 129
 130     // calculate the thrust desired
 131     switch (newThrustMode) {
 132 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 133     case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
 134         rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
 135         break;
 136     case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
 137         rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
 138         break;
 139 #endif
 140     case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 141     case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
 142     default:
 143         rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
 144         break;
 145     }
 146
 147
 148     float local_error[3];
 149     {
 150 #if defined(PIOS_QUATERNION_STABILIZATION)
 151         // Quaternion calculation of error in each axis.  Uses more memory.
 152         float rpy_desired[3];
 153         float q_desired[4];
 154         float q_error[4];
 155
 156         for (t = 0; t < 3; t++) {
 157             switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
 158             case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
 159             case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
 160             case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
 161                 rpy_desired[t] = stabilizationDesiredAxis[t];
 162                 break;
 163             case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
 164             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 165             default:
 166                 rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
 167                 break;
 168             }
 169         }
 170
 171         RPY2Quaternion(rpy_desired, q_desired);
 172         quat_inverse(q_desired);
 173         quat_mult(q_desired, &attitudeState.q1, q_error);
 174         quat_inverse(q_error);
 175         Quaternion2RPY(q_error, local_error);
 176
 177 #else /* if defined(PIOS_QUATERNION_STABILIZATION) */
 178         // Simpler algorithm for CC, less memory
 179         local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
 180         local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
 181         local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw;
 182
 183         // find shortest way
 184         float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
 185         if (modulo < 0) {
 186             local_error[2] = modulo + 180.0f;
 187         } else {
 188             local_error[2] = modulo - 180.0f;
 189         }
 190 #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
 191     }
 192
 193
 194     for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
 195         reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
 196         previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
 197
 198         if (reinit) {
 199             stabSettings.outerPids[t].iAccumulator = 0;
 200         }
 201         switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
 202         case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
 203             rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 204             break;
 205         case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
 206         {
 207             float stickinput[3];
 208             stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
 209             stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
 210             stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
 211             float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
 212             // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
 213             rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
 214                                         -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
 215                                         StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
 216                                         );
 217             // Compute the weighted average rate desired
 218             // Using max() rather than sqrt() for cpu speed;
 219             // - this makes the stick region into a square;
 220             // - this is a feature!
 221             // - hold a roll angle and add just pitch without the stick sensitivity changing
 222             float magnitude = fabsf(stickinput[t]);
 223             if (t < 2) {
 224                 magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
 225             }
 226
 227             // modify magnitude to move the Att to Rate transition to the place
 228             // specified by the user
 229             // we are looking for where the stick angle == transition angle
 230             // and the Att rate equals the Rate rate
 231             // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
 232             // == Rate x StickAngle [Rate pulling up according to stick angle]
 233             // * StickAngle [X Ratt proportion]
 234             // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
 235             // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
 236             // and quadratic formula says that is 0.618033989f
 237             // I tested 14.01 and came up with .61 without even remembering this number
 238             // I thought that moving the P,I, and maxangle terms around would change this value
 239             // and that I would have to take these into account, but varying
 240             // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
 241             // and varying maxangle from 4 to 120 didn't either.
 242             // so for now I'm not taking these into account
 243             // while working with this, it occurred to me that Attitude mode,
 244             // set up with maxangle=190 would be similar to Ratt, and it is.
 245 #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
 246
 247             // the following assumes the transition would otherwise be at 0.618033989f
 248             // and THAT assumes that Att ramps up to max roll rate
 249             // when a small number of degrees off of where it should be
 250
 251             // if below the transition angle (still in attitude mode)
 252             // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
 253             if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
 254                 magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
 255             } else {
 256                 magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
 257                             * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
 258                             / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
 259                             + STICK_VALUE_AT_MODE_TRANSITION;
 260             }
 261             rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
 262         }
 263         break;
 264         case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
 265             // FIXME: local_error[] is rate - attitude for Weak Leveling
 266             // The only ramifications are:
 267             // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
 268             // Changing Rate mode max rate currently requires a change to Kp
 269             // That would be changed to Attitude mode max angle affecting Kp
 270             // Also does not take dT into account
 271         {
 272             float stickinput[3];
 273             stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
 274             stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
 275             stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
 276             float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
 277             float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
 278             weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
 279
 280             // Compute desired rate as input biased towards leveling
 281             rateDesiredAxis[t] = rate_input + weak_leveling;
 282         }
 283         break;
 284         case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
 285             rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
 286             // now test limits for pitch and/or roll
 287             if (t == 1) { // pitch
 288                 if ((attitudeState.Pitch < -stabSettings.stabBank.PitchMax) || pitchMin) {
 289                     pitchMin = true;
 290                     // Attitude exceeds pitch min,
 291                     // Do Attitude stabilisation at min pitch angle while user still maintain negative pitch
 292                     if (stabilizationDesiredAxis[t] < 0.0f) {
 293                         local_error[t]     = -stabSettings.stabBank.PitchMax - attitudeState.Pitch;
 294                         rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 295                     } else {
 296                         // Stop Attitude stabilization and return to Rate
 297                         pitchMin = false;
 298                     }
 299                 } else if ((attitudeState.Pitch > stabSettings.stabBank.PitchMax) || pitchMax) {
 300                     pitchMax = true;
 301                     // Attitude exceeds pitch max
 302                     // Do Attitude stabilisation at max pitch angle while user still maintain positive pitch
 303                     if (stabilizationDesiredAxis[t] > 0.0f) {
 304                         local_error[t]     = stabSettings.stabBank.PitchMax - attitudeState.Pitch;
 305                         rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 306                     } else {
 307                         // Stop Attitude stabilization and return to Rate
 308                         pitchMax = false;
 309                     }
 310                 }
 311             } else if (t == 0) { // roll
 312                 if ((attitudeState.Roll < -stabSettings.stabBank.RollMax) || rollMin) {
 313                     rollMin = true;
 314                     // Attitude exceeds roll min,
 315                     // Do Attitude stabilisation at min roll angle while user still maintain negative roll
 316                     if (stabilizationDesiredAxis[t] < 0.0f) {
 317                         local_error[t]     = -stabSettings.stabBank.RollMax - attitudeState.Roll;
 318                         rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 319                     } else {
 320                         // Stop Attitude stabilization and return to Rate
 321                         rollMin = false;
 322                     }
 323                 } else if ((attitudeState.Roll > stabSettings.stabBank.RollMax) || rollMax) {
 324                     rollMax = true;
 325                     // Attitude exceeds roll max
 326                     // Do Attitude stabilisation at max roll angle while user still maintain positive roll
 327                     if (stabilizationDesiredAxis[t] > 0.0f) {
 328                         local_error[t]     = stabSettings.stabBank.RollMax - attitudeState.Roll;
 329                         rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 330                     } else {
 331                         // Stop Attitude stabilization and return to Rate
 332                         rollMax = false;
 333                     }
 334                 }
 335             }
 336             break;
 337
 338         case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 339         default:
 340             rateDesiredAxis[t] = stabilizationDesiredAxis[t];
 341             break;
 342         }
 343     }
 344
 345     RateDesiredSet(&rateDesired);
 346     {
 347         FlightStatusArmedOptions armed;
 348         FlightStatusArmedGet(&armed);
 349         float throttleDesired;
 350         ManualControlCommandThrottleGet(&throttleDesired);
 351         if (armed != FLIGHTSTATUS_ARMED_ARMED ||
 352             ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
 353             // Force all axes to reinitialize when engaged
 354             for (t = 0; t < AXES; t++) {
 355                 previous_mode[t] = 255;
 356             }
 357         }
 358     }
 359
 360 // update cruisecontrol based on attitude
 361     cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
 362     stabSettings.monitor.rateupdates = 0;
 363 }
 364
 365
 366 static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 367 {
 368 #ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED
 369     // to reduce CPU utilization, outer loop is not executed on every state update
 370     static uint8_t cpusaver = 0;
 371
 372     if ((cpusaver++ % OUTERLOOP_SKIPCOUNT) == 0) {
 373 #endif
 374     // this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
 375     AttitudeStateGet(&attitude);
 376     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 377
 378 #ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED
 379 }
 380 #endif
 381 }
 382
 383 /**
 384  * @}
 385  * @}
 386  */