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