flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp

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