flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       FixedWingAutoTakeoffController.cpp
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
   6  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   7  * @brief      Fixed wing fly controller implementation
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  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 <pid.h>
  33 #include <sin_lookup.h>
  34 #include <pathdesired.h>
  35 #include <fixedwingpathfollowersettings.h>
  36 #include <flightstatus.h>
  37 #include <pathstatus.h>
  38 #include <stabilizationdesired.h>
  39 #include <velocitystate.h>
  40 #include <positionstate.h>
  41 #include <attitudestate.h>
  42 }
  43
  44 // C++ includes
  45 #include "fixedwingautotakeoffcontroller.h"
  46
  47 // Private constants
  48
  49 // pointer to a singleton instance
  50 FixedWingAutoTakeoffController *FixedWingAutoTakeoffController::p_inst = 0;
  51
  52
  53 // Called when mode first engaged
  54 void FixedWingAutoTakeoffController::Activate(void)
  55 {
  56     if (!mActive) {
  57         setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
  58     }
  59     FixedWingFlyController::Activate();
  60 }
  61
  62 /**
  63  * fixed wing autopilot
  64  * use fixed attitude heading towards destination waypoint
  65  */
  66 void FixedWingAutoTakeoffController::UpdateAutoPilot()
  67 {
  68     if (state < FW_AUTOTAKEOFF_STATE_SIZE) {
  69         (this->*runFunctionTable[state])();
  70     } else {
  71         setState(FW_AUTOTAKEOFF_STATE_LAUNCH);
  72     }
  73 }
  74
  75 /**
  76  * getAirspeed helper function
  77  */
  78 float FixedWingAutoTakeoffController::getAirspeed(void)
  79 {
  80     VelocityStateData v;
  81     float yaw;
  82
  83     VelocityStateGet(&v);
  84     AttitudeStateYawGet(&yaw);
  85
  86     // current ground speed projected in forward direction
  87     float groundspeedProjection = v.North * cos_lookup_deg(yaw) + v.East * sin_lookup_deg(yaw);
  88
  89     // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
  90     // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
  91     // than airspeed and gps sensors alone
  92     return groundspeedProjection + indicatedAirspeedStateBias;
  93 }
  94
  95
  96 /**
  97  * setState - state transition including initialization
  98  */
  99 void FixedWingAutoTakeoffController::setState(FixedWingAutoTakeoffControllerState_T setstate)
 100 {
 101     if (state < FW_AUTOTAKEOFF_STATE_SIZE && setstate != state) {
 102         state = setstate;
 103         (this->*initFunctionTable[state])();
 104     }
 105 }
 106
 107 /**
 108  * setAttitude - output function to steer plane
 109  */
 110 void FixedWingAutoTakeoffController::setAttitude(bool unsafe)
 111 {
 112     StabilizationDesiredData stabDesired;
 113
 114     stabDesired.Roll = 0.0f;
 115     stabDesired.Yaw  = initYaw;
 116     if (unsafe) {
 117         stabDesired.Pitch  = fixedWingSettings->LandingPitch;
 118         stabDesired.Thrust = 0.0f;
 119     } else {
 120         stabDesired.Pitch  = fixedWingSettings->TakeOffPitch;
 121         stabDesired.Thrust = fixedWingSettings->ThrustLimit.Max;
 122     }
 123     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 124     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 125     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 126     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 127
 128     StabilizationDesiredSet(&stabDesired);
 129     if (unsafe) {
 130         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 131         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 132     } else {
 133         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 134     }
 135
 136     // calculate fractional progress based on altitude
 137     float downPos;
 138     PositionStateDownGet(&downPos);
 139     if (fabsf(pathDesired->End.Down - pathDesired->Start.Down) < 1e-3f) {
 140         pathStatus->fractional_progress = 1.0f;
 141         pathStatus->error = 0.0f;
 142     } else {
 143         pathStatus->fractional_progress = (downPos - pathDesired->Start.Down) / (pathDesired->End.Down - pathDesired->Start.Down);
 144     }
 145     pathStatus->error = fabsf(downPos - pathDesired->End.Down);
 146
 147     PathStatusSet(pathStatus);
 148 }
 149
 150 /**
 151  * check if situation is unsafe
 152  */
 153 bool FixedWingAutoTakeoffController::isUnsafe(void)
 154 {
 155     bool abort  = false;
 156     float speed = getAirspeed();
 157
 158     if (speed > maxVelocity) {
 159         maxVelocity = speed;
 160     }
 161     // too much total deceleration (crash, insufficient climbing power, ...)
 162     if (speed < maxVelocity - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
 163         abort = true;
 164     }
 165     AttitudeStateData attitude;
 166     AttitudeStateGet(&attitude);
 167     // too much bank angle
 168     if (fabsf(attitude.Roll) > fixedWingSettings->SafetyCutoffLimits.RollDeg) {
 169         abort = true;
 170     }
 171     if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->SafetyCutoffLimits.PitchDeg) {
 172         abort = true;
 173     }
 174     float deltayaw = attitude.Yaw - initYaw;
 175     if (deltayaw > 180.0f) {
 176         deltayaw -= 360.0f;
 177     }
 178     if (deltayaw < -180.0f) {
 179         deltayaw += 360.0f;
 180     }
 181     if (fabsf(deltayaw) > fixedWingSettings->SafetyCutoffLimits.YawDeg) {
 182         abort = true;
 183     }
 184     return abort;
 185 }
 186
 187
 188 // init inactive does nothing
 189 void FixedWingAutoTakeoffController::init_inactive(void) {}
 190
 191 // init launch resets private variables to start values
 192 void FixedWingAutoTakeoffController::init_launch(void)
 193 {
 194     // find out vector direction of *runway* (if any)
 195     // and align, otherwise just stay straight ahead
 196     pathStatus->path_direction_north = 0.0f;
 197     pathStatus->path_direction_east  = 0.0f;
 198     pathStatus->path_direction_down  = 0.0f;
 199     pathStatus->correction_direction_north = 0.0f;
 200     pathStatus->correction_direction_east  = 0.0f;
 201     pathStatus->correction_direction_down  = 0.0f;
 202     if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
 203         fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
 204         AttitudeStateYawGet(&initYaw);
 205     } else {
 206         initYaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
 207         if (initYaw < -180.0f) {
 208             initYaw += 360.0f;
 209         }
 210         if (initYaw > 180.0f) {
 211             initYaw -= 360.0f;
 212         }
 213     }
 214
 215     maxVelocity = getAirspeed();
 216 }
 217
 218 // init climb does nothing
 219 void FixedWingAutoTakeoffController::init_climb(void) {}
 220
 221 // init hold does nothing
 222 void FixedWingAutoTakeoffController::init_hold(void) {}
 223
 224 // init abort does nothing
 225 void FixedWingAutoTakeoffController::init_abort(void) {}
 226
 227
 228 // run inactive does nothing
 229 // no state transitions
 230 void FixedWingAutoTakeoffController::run_inactive(void) {}
 231
 232 // run launch tries to takeoff - indicates safe situation with engine power (for hand launch)
 233 // run launch checks for:
 234 // 1. min velocity for climb
 235 void FixedWingAutoTakeoffController::run_launch(void)
 236 {
 237     // state transition
 238     if (maxVelocity > fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
 239         setState(FW_AUTOTAKEOFF_STATE_CLIMB);
 240     }
 241
 242     setAttitude(isUnsafe());
 243 }
 244
 245 // run climb climbs with max power
 246 // run climb checks for:
 247 // 1. min altitude for hold
 248 // 2. critical situation for abort (different than launch)
 249 void FixedWingAutoTakeoffController::run_climb(void)
 250 {
 251     bool unsafe = isUnsafe();
 252     float downPos;
 253
 254     PositionStateDownGet(&downPos);
 255
 256     if (unsafe) {
 257         // state transition 2
 258         setState(FW_AUTOTAKEOFF_STATE_ABORT);
 259     } else if (downPos < pathDesired->End.Down) {
 260         // state transition 1
 261         setState(FW_AUTOTAKEOFF_STATE_HOLD);
 262     }
 263
 264     setAttitude(unsafe);
 265 }
 266
 267 // run hold loiters like in position hold
 268 // no state transitions (FlyController does exception handling)
 269 void FixedWingAutoTakeoffController::run_hold(void)
 270 {
 271     // parent controller will do perfect position hold in autotakeoff mode
 272     FixedWingFlyController::UpdateAutoPilot();
 273 }
 274
 275 // run abort descends with wings level, engine off (like land)
 276 // no state transitions
 277 void FixedWingAutoTakeoffController::run_abort(void)
 278 {
 279     setAttitude(true);
 280 }