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 OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  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 #include <openpilot.h>
  35 #include <pios_struct_helper.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 / 666.0f)
  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
  68 // Private functions
  69 static void stabilizationOuterloopTask();
  70 static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  71
  72 void stabilizationOuterloopInit()
  73 {
  74     RateDesiredInitialize();
  75     StabilizationDesiredInitialize();
  76     AttitudeStateInitialize();
  77     StabilizationStatusInitialize();
  78     FlightStatusInitialize();
  79     ManualControlCommandInitialize();
  80
  81     PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
  82
  83     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
  84     AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
  85 }
  86
  87
  88 /**
  89  * WARNING! This callback executes with critical flight control priority every
  90  * time a gyroscope update happens do NOT put any time consuming calculations
  91  * in this loop unless they really have to execute with every gyro update
  92  */
  93 static void stabilizationOuterloopTask()
  94 {
  95     AttitudeStateData attitudeState;
  96     RateDesiredData rateDesired;
  97     StabilizationDesiredData stabilizationDesired;
  98     StabilizationStatusOuterLoopData enabled;
  99
 100     AttitudeStateGet(&attitudeState);
 101     StabilizationDesiredGet(&stabilizationDesired);
 102     RateDesiredGet(&rateDesired);
 103     StabilizationStatusOuterLoopGet(&enabled);
 104     float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
 105     float *rateDesiredAxis = &rateDesired.Roll;
 106     int t;
 107     float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 108
 109     float local_error[3];
 110     {
 111 #if defined(PIOS_QUATERNION_STABILIZATION)
 112         // Quaternion calculation of error in each axis.  Uses more memory.
 113         float rpy_desired[3];
 114         float q_desired[4];
 115         float q_error[4];
 116
 117         for (t = 0; t < 3; t++) {
 118             switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
 119             case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
 120             case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
 121             case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
 122                 rpy_desired[t] = stabilizationDesiredAxis[t];
 123                 break;
 124             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 125             default:
 126                 rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
 127                 break;
 128             }
 129         }
 130
 131         RPY2Quaternion(rpy_desired, q_desired);
 132         quat_inverse(q_desired);
 133         quat_mult(q_desired, &attitudeState.q1, q_error);
 134         quat_inverse(q_error);
 135         Quaternion2RPY(q_error, local_error);
 136
 137 #else /* if defined(PIOS_QUATERNION_STABILIZATION) */
 138         // Simpler algorithm for CC, less memory
 139         local_error[0] = stabilizationDesiredAxis[0] - attitudeState.Roll;
 140         local_error[1] = stabilizationDesiredAxis[1] - attitudeState.Pitch;
 141         local_error[2] = stabilizationDesiredAxis[2] - attitudeState.Yaw;
 142
 143         // find shortest way
 144         float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
 145         if (modulo < 0) {
 146             local_error[2] = modulo + 180.0f;
 147         } else {
 148             local_error[2] = modulo - 180.0f;
 149         }
 150 #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
 151     }
 152     for (t = 0; t < AXES; t++) {
 153         bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
 154         previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
 155
 156         if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
 157             if (reinit) {
 158                 stabSettings.outerPids[t].iAccumulator = 0;
 159             }
 160             switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
 161             case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
 162                 rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
 163                 break;
 164             case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
 165             {
 166                 float stickinput[3];
 167                 stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
 168                 stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
 169                 stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
 170                 float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
 171                 // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
 172                 rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
 173                                             -cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t],
 174                                             cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]
 175                                             );
 176                 // Compute the weighted average rate desired
 177                 // Using max() rather than sqrt() for cpu speed;
 178                 // - this makes the stick region into a square;
 179                 // - this is a feature!
 180                 // - hold a roll angle and add just pitch without the stick sensitivity changing
 181                 float magnitude = fabsf(stickinput[t]);
 182                 if (t < 2) {
 183                     magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
 184                 }
 185
 186                 // modify magnitude to move the Att to Rate transition to the place
 187                 // specified by the user
 188                 // we are looking for where the stick angle == transition angle
 189                 // and the Att rate equals the Rate rate
 190                 // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
 191                 // == Rate x StickAngle [Rate pulling up according to stick angle]
 192                 // * StickAngle [X Ratt proportion]
 193                 // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
 194                 // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
 195                 // and quadratic formula says that is 0.618033989f
 196                 // I tested 14.01 and came up with .61 without even remembering this number
 197                 // I thought that moving the P,I, and maxangle terms around would change this value
 198                 // and that I would have to take these into account, but varying
 199                 // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
 200                 // and varying maxangle from 4 to 120 didn't either.
 201                 // so for now I'm not taking these into account
 202                 // while working with this, it occurred to me that Attitude mode,
 203                 // set up with maxangle=190 would be similar to Ratt, and it is.
 204                 #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
 205
 206                 // the following assumes the transition would otherwise be at 0.618033989f
 207                 // and THAT assumes that Att ramps up to max roll rate
 208                 // when a small number of degrees off of where it should be
 209
 210                 // if below the transition angle (still in attitude mode)
 211                 // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
 212                 if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
 213                     magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
 214                 } else {
 215                     magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
 216                                 * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
 217                                 / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
 218                                 + STICK_VALUE_AT_MODE_TRANSITION;
 219                 }
 220                 rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
 221             }
 222             break;
 223             case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
 224                 // FIXME: local_error[] is rate - attitude for Weak Leveling
 225                 // The only ramifications are:
 226                 // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
 227                 // Changing Rate mode max rate currently requires a change to Kp
 228                 // That would be changed to Attitude mode max angle affecting Kp
 229                 // Also does not take dT into account
 230             {
 231                 float stickinput[3];
 232                 stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
 233                 stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
 234                 stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
 235                 float rate_input    = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
 236                 float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
 237                 weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
 238
 239                 // Compute desired rate as input biased towards leveling
 240                 rateDesiredAxis[t] = rate_input + weak_leveling;
 241             }
 242             break;
 243             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 244             default:
 245                 rateDesiredAxis[t] = stabilizationDesiredAxis[t];
 246                 break;
 247             }
 248         } else {
 249             switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
 250 #ifdef REVOLUTION
 251             case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
 252                 rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
 253                 break;
 254             case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
 255                 rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
 256                 break;
 257 #endif /* REVOLUTION */
 258             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
 259             default:
 260                 rateDesiredAxis[t] = stabilizationDesiredAxis[t];
 261                 break;
 262             }
 263         }
 264     }
 265
 266     RateDesiredSet(&rateDesired);
 267     {
 268         uint8_t armed;
 269         FlightStatusArmedGet(&armed);
 270         float throttleDesired;
 271         ManualControlCommandThrottleGet(&throttleDesired);
 272         if (armed != FLIGHTSTATUS_ARMED_ARMED ||
 273             ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
 274             // Force all axes to reinitialize when engaged
 275             for (t = 0; t < AXES; t++) {
 276                 previous_mode[t] = 255;
 277             }
 278         }
 279     }
 280
 281     // update cruisecontrol based on attitude
 282     cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
 283     stabSettings.monitor.rateupdates = 0;
 284 }
 285
 286
 287 static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 288 {
 289     // to reduce CPU utilisation, outer loop is not executed every state update
 290     static uint8_t cpusafer = 0;
 291
 292     if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) {
 293         // this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
 294         AttitudeStateGet(&attitude);
 295         PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 296     }
 297 }
 298
 299 /**
 300  * @}
 301  * @}
 302  */