flight/modules/Stabilization/altitudeloop.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       altitudeloop.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
   7  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
   8  * of @ref ManualControlCommand is Auto.
   9  *
  10  * @see        The GNU Public License (GPL) Version 3
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 extern "C" {
  30 #include <openpilot.h>
  31
  32 #include <callbackinfo.h>
  33
  34 #include <pid.h>
  35 #include <altitudeloop.h>
  36 #include <CoordinateConversions.h>
  37 #include <altitudeholdsettings.h>
  38 #include <altitudeholdstatus.h>
  39 #include <velocitystate.h>
  40 #include <positionstate.h>
  41 #include <vtolselftuningstats.h>
  42 #include <stabilization.h>
  43 }
  44
  45 #include <pidcontroldown.h>
  46
  47 // Private constants
  48
  49
  50 #ifdef REVOLUTION
  51
  52 #define UPDATE_EXPECTED   (1.0f / PIOS_SENSOR_RATE)
  53 #define UPDATE_MIN        1.0e-6f
  54 #define UPDATE_MAX        1.0f
  55 #define UPDATE_ALPHA      1.0e-2f
  56
  57 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
  58 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
  59
  60 // Private types
  61
  62 // Private variables
  63 static DelayedCallbackInfo *altitudeHoldCBInfo;
  64 static PIDControlDown controlDown;
  65 static AltitudeHoldSettingsData altitudeHoldSettings;
  66 static ThrustModeType thrustMode;
  67 static float thrustDemand = 0.0f;
  68
  69
  70 // Private functions
  71 static void SettingsUpdatedCb(UAVObjEvent *ev);
  72 static void altitudeHoldTask(void);
  73 static void VelocityStateUpdatedCb(UAVObjEvent *ev);
  74
  75 /**
  76  * Setup mode and setpoint
  77  *
  78  * reinit: true when althold/vario mode selected over a previous alternate thrust mode
  79  */
  80 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
  81 {
  82     static bool newaltitude = true;
  83
  84     if (reinit || !controlDown.IsActive()) {
  85         controlDown.Activate();
  86         newaltitude = true;
  87         // calculate a thrustDemand on reinit only
  88         thrustMode  = mode;
  89         altitudeHoldTask();
  90     }
  91
  92     const float DEADBAND      = 0.20f;
  93     const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
  94     const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
  95
  96
  97     if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
  98         // Cut thrust if desired
  99         controlDown.UpdateVelocitySetpoint(0.0f);
 100         thrustDemand = 0.0f;
 101         thrustMode   = DIRECT;
 102         newaltitude  = true;
 103         return thrustDemand;
 104     } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
 105         // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
 106         // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
 107         controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
 108         thrustMode  = ALTITUDEVARIO;
 109         newaltitude = true;
 110     } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
 111         controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
 112         thrustMode  = ALTITUDEVARIO;
 113         newaltitude = true;
 114     } else if (newaltitude == true) {
 115         controlDown.UpdateVelocitySetpoint(0.0f);
 116         PositionStateData posState;
 117         PositionStateGet(&posState);
 118         controlDown.UpdatePositionSetpoint(posState.Down);
 119         thrustMode  = ALTITUDEHOLD;
 120         newaltitude = false;
 121     }
 122
 123     thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
 124
 125     return thrustDemand;
 126 }
 127
 128 /**
 129  * Disable the alt hold task loop velocity controller to save cpu cycles
 130  */
 131 void stabilizationDisableAltitudeHold(void)
 132 {
 133     controlDown.Deactivate();
 134 }
 135
 136
 137 /**
 138  * Module thread, should not return.
 139  */
 140 static void altitudeHoldTask(void)
 141 {
 142     if (!controlDown.IsActive()) {
 143         return;
 144     }
 145
 146     AltitudeHoldStatusData altitudeHoldStatus;
 147     AltitudeHoldStatusGet(&altitudeHoldStatus);
 148
 149     float velocityStateDown;
 150     VelocityStateDownGet(&velocityStateDown);
 151     controlDown.UpdateVelocityState(velocityStateDown);
 152
 153     float local_thrustDemand = 0.0f;
 154
 155     switch (thrustMode) {
 156     case ALTITUDEHOLD:
 157     {
 158         float positionStateDown;
 159         PositionStateDownGet(&positionStateDown);
 160         controlDown.UpdatePositionState(positionStateDown);
 161         controlDown.ControlPosition();
 162         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
 163         altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
 164         local_thrustDemand = controlDown.GetDownCommand();
 165     }
 166     break;
 167
 168     case ALTITUDEVARIO:
 169         altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
 170         altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
 171         local_thrustDemand = controlDown.GetDownCommand();
 172         break;
 173
 174     case DIRECT:
 175         altitudeHoldStatus.VelocityDesired = 0.0f;
 176         altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
 177         break;
 178     }
 179
 180     thrustDemand = local_thrustDemand;
 181     AltitudeHoldStatusSet(&altitudeHoldStatus);
 182 }
 183
 184 static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 185 {
 186     PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
 187 }
 188
 189 /**
 190  * Initialise the module, called on startup
 191  */
 192 void stabilizationAltitudeloopInit()
 193 {
 194     AltitudeHoldSettingsInitialize();
 195     AltitudeHoldStatusInitialize();
 196     PositionStateInitialize();
 197     VelocityStateInitialize();
 198     VtolSelfTuningStatsInitialize();
 199
 200     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
 201     VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
 202     SettingsUpdatedCb(NULL);
 203
 204     altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
 205     VelocityStateConnectCallback(&VelocityStateUpdatedCb);
 206 }
 207
 208
 209 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 210 {
 211     AltitudeHoldSettingsGet(&altitudeHoldSettings);
 212
 213     controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
 214                                  altitudeHoldSettings.VerticalVelPID.Ki,
 215                                  altitudeHoldSettings.VerticalVelPID.Kd,
 216                                  altitudeHoldSettings.VerticalVelPID.Beta,
 217                                  (float)(UPDATE_EXPECTED),
 218                                  altitudeHoldSettings.ThrustRate);
 219
 220     controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
 221
 222     VtolSelfTuningStatsData vtolSelfTuningStats;
 223     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 224     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
 225
 226     // initialise limits on thrust but note the FSM can override.
 227     controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
 228
 229     // disable neutral thrust calcs which should only be done in a hold mode.
 230     controlDown.DisableNeutralThrustCalc();
 231 }
 232
 233
 234 #endif /* ifdef REVOLUTION */