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     } else {
 129         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 130     }
 131
 132     PathStatusSet(pathStatus);
 133 }
 134
 135 /**
 136  * check if situation is unsafe
 137  */
 138 bool FixedWingAutoTakeoffController::isUnsafe(void)
 139 {
 140     bool abort  = false;
 141     float speed = getAirspeed();
 142
 143     if (speed > maxVelocity) {
 144         maxVelocity = speed;
 145     }
 146     // too much total deceleration (crash, insufficient climbing power, ...)
 147     if (speed < maxVelocity - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
 148         abort = true;
 149     }
 150     AttitudeStateData attitude;
 151     AttitudeStateGet(&attitude);
 152     // too much bank angle
 153     if (fabsf(attitude.Roll) > fixedWingSettings->SafetyCutoffLimits.RollDeg) {
 154         abort = true;
 155     }
 156     if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->SafetyCutoffLimits.PitchDeg) {
 157         abort = true;
 158     }
 159     float deltayaw = attitude.Yaw - initYaw;
 160     if (deltayaw > 180.0f) {
 161         deltayaw -= 360.0f;
 162     }
 163     if (deltayaw < -180.0f) {
 164         deltayaw += 360.0f;
 165     }
 166     if (fabsf(deltayaw) > fixedWingSettings->SafetyCutoffLimits.YawDeg) {
 167         abort = true;
 168     }
 169     return abort;
 170 }
 171
 172
 173 // init inactive does nothing
 174 void FixedWingAutoTakeoffController::init_inactive(void) {}
 175
 176 // init launch resets private variables to start values
 177 void FixedWingAutoTakeoffController::init_launch(void)
 178 {
 179     // find out vector direction of *runway* (if any)
 180     // and align, otherwise just stay straight ahead
 181     if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
 182         fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
 183         AttitudeStateYawGet(&initYaw);
 184     } else {
 185         initYaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
 186         if (initYaw < -180.0f) {
 187             initYaw += 360.0f;
 188         }
 189         if (initYaw > 180.0f) {
 190             initYaw -= 360.0f;
 191         }
 192     }
 193
 194     maxVelocity = getAirspeed();
 195 }
 196
 197 // init climb does nothing
 198 void FixedWingAutoTakeoffController::init_climb(void) {}
 199
 200 // init hold does nothing
 201 void FixedWingAutoTakeoffController::init_hold(void) {}
 202
 203 // init abort does nothing
 204 void FixedWingAutoTakeoffController::init_abort(void) {}
 205
 206
 207 // run inactive does nothing
 208 // no state transitions
 209 void FixedWingAutoTakeoffController::run_inactive(void) {}
 210
 211 // run launch tries to takeoff - indicates safe situation with engine power (for hand launch)
 212 // run launch checks for:
 213 // 1. min velocity for climb
 214 void FixedWingAutoTakeoffController::run_launch(void)
 215 {
 216     // state transition
 217     if (maxVelocity > fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
 218         setState(FW_AUTOTAKEOFF_STATE_CLIMB);
 219     }
 220
 221     setAttitude(isUnsafe());
 222 }
 223
 224 // run climb climbs with max power
 225 // run climb checks for:
 226 // 1. min altitude for hold
 227 // 2. critical situation for abort (different than launch)
 228 void FixedWingAutoTakeoffController::run_climb(void)
 229 {
 230     bool unsafe = isUnsafe();
 231     float downPos;
 232
 233     PositionStateDownGet(&downPos);
 234
 235     if (unsafe) {
 236         // state transition 2
 237         setState(FW_AUTOTAKEOFF_STATE_ABORT);
 238     } else if (downPos < pathDesired->End.Down) {
 239         // state transition 1
 240         setState(FW_AUTOTAKEOFF_STATE_HOLD);
 241     }
 242
 243     setAttitude(unsafe);
 244 }
 245
 246 // run hold loiters like in position hold
 247 // no state transitions (FlyController does exception handling)
 248 void FixedWingAutoTakeoffController::run_hold(void)
 249 {
 250     // parent controller will do perfect position hold in autotakeoff mode
 251     FixedWingFlyController::UpdateAutoPilot();
 252 }
 253
 254 // run abort descends with wings level, engine off (like land)
 255 // no state transitions
 256 void FixedWingAutoTakeoffController::run_abort(void)
 257 {
 258     setAttitude(true);
 259 }