flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtolautotakeoffcontroller.cpp
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2018
   6  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   7  * @brief      Vtol auto takeoff controller loop
   8  * @see        The GNU Public License (GPL) Version 3
   9  * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  10  *
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 extern "C" {
  29 #include <openpilot.h>
  30
  31 #include <math.h>
  32 #include <pid.h>
  33 #include <alarms.h>
  34 #include <CoordinateConversions.h>
  35 #include <sin_lookup.h>
  36 #include <pathdesired.h>
  37 #include <paths.h>
  38 #include "plans.h"
  39 #include <sanitycheck.h>
  40
  41 #include <homelocation.h>
  42 #include <accelstate.h>
  43 #include <vtolpathfollowersettings.h>
  44 #include <flightstatus.h>
  45 #include <flightmodesettings.h>
  46 #include <pathstatus.h>
  47 #include <positionstate.h>
  48 #include <velocitystate.h>
  49 #include <velocitydesired.h>
  50 #include <stabilizationdesired.h>
  51 #include <airspeedstate.h>
  52 #include <attitudestate.h>
  53 #include <takeofflocation.h>
  54 #include <poilocation.h>
  55 #include <manualcontrolcommand.h>
  56 #include <systemsettings.h>
  57 #include <stabilizationbank.h>
  58 #include <stabilizationdesired.h>
  59 #include <vtolselftuningstats.h>
  60 #include <statusvtolautotakeoff.h>
  61 #include <pathsummary.h>
  62 }
  63
  64 // C++ includes
  65 #include "vtolautotakeoffcontroller.h"
  66 #include "pathfollowerfsm.h"
  67 #include "vtolautotakeofffsm.h"
  68 #include "pidcontroldown.h"
  69
  70 // Private constants
  71 #define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN             2.0f
  72 #define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX             50.0f
  73 #define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT         0.2f
  74 #define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
  75 #define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT                  0.1f
  76
  77 // pointer to a singleton instance
  78 VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
  79
  80 VtolAutoTakeoffController::VtolAutoTakeoffController()
  81     : fsm(0), vtolPathFollowerSettings(0), mActive(false)
  82 {}
  83
  84 // Called when mode first engaged
  85 void VtolAutoTakeoffController::Activate(void)
  86 {
  87     if (!mActive) {
  88         mActive   = true;
  89         mOverride = true;
  90         SettingsUpdated();
  91         fsm->Activate();
  92         controlDown.Activate();
  93         controlNE.Activate();
  94         autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
  95         // We only allow takeoff if the state transition of disarmed to armed occurs
  96         // whilst in the autotake flight mode
  97         FlightStatusData flightStatus;
  98         FlightStatusGet(&flightStatus);
  99         StabilizationDesiredData stabiDesired;
 100         StabilizationDesiredGet(&stabiDesired);
 101
 102         if (flightStatus.Armed) {
 103             // Are we inflight?
 104             if (stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT || flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 105                 // ok assume already in flight and just enter position hold
 106                 // if we are not actually inflight this will just be a violent autotakeoff
 107                 autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
 108             } else {
 109                 autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
 110                 // Note that if this mode was invoked unintentionally whilst in flight, effectively
 111                 // all inputs get ignored and the vtol continues to fly to its previous
 112                 // stabi command.
 113             }
 114         }
 115         fsm->setControlState(autotakeoffState);
 116     }
 117 }
 118
 119 uint8_t VtolAutoTakeoffController::IsActive(void)
 120 {
 121     return mActive;
 122 }
 123
 124 uint8_t VtolAutoTakeoffController::Mode(void)
 125 {
 126     return PATHDESIRED_MODE_AUTOTAKEOFF;
 127 }
 128
 129 // Objective updated in pathdesired, e.g. same flight mode but new target velocity
 130 void VtolAutoTakeoffController::ObjectiveUpdated(void)
 131 {
 132     if (mOverride) {
 133         // override pathDesired from PathPlanner with current position,
 134         // as we deliberately don't care about the location of the waypoints on the map
 135         float autotakeoff_velocity;
 136         float autotakeoff_height;
 137         PositionStateData positionState;
 138         PositionStateGet(&positionState);
 139         FlightModeSettingsAutoTakeOffVelocityGet(&autotakeoff_velocity);
 140         FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
 141         autotakeoff_height = fabsf(autotakeoff_height);
 142         if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
 143             autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
 144         } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
 145             autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
 146         }
 147         controlDown.UpdateVelocitySetpoint(-autotakeoff_velocity);
 148         controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
 149         controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
 150
 151
 152         controlDown.UpdatePositionSetpoint(positionState.Down - autotakeoff_height);
 153         mOverride = false; // further updates always come from ManualControl and will control horizontal position
 154     } else {
 155         // Set the objective's target velocity
 156
 157         controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
 158         controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
 159                                          pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
 160         controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
 161         controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
 162     }
 163 }
 164
 165 // Controller deactivated
 166 void VtolAutoTakeoffController::Deactivate(void)
 167 {
 168     if (mActive) {
 169         mActive = false;
 170         fsm->Inactive();
 171         controlDown.Deactivate();
 172         controlNE.Deactivate();
 173     }
 174 }
 175
 176 // AutoTakeoff Uses different vertical velocity PID.
 177 void VtolAutoTakeoffController::SettingsUpdated(void)
 178 {
 179     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 180
 181     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
 182                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
 183                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
 184                                vtolPathFollowerSettings->HorizontalVelPID.Beta,
 185                                dT,
 186                                vtolPathFollowerSettings->HorizontalVelMax);
 187
 188
 189     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
 190     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
 191
 192     controlDown.UpdateParameters(vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kp,
 193                                  vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Ki,
 194                                  vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kd,
 195                                  vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Beta,
 196                                  dT,
 197                                  vtolPathFollowerSettings->VerticalVelMax);
 198     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
 199     VtolSelfTuningStatsData vtolSelfTuningStats;
 200     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 201     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
 202     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
 203     fsm->SettingsUpdated();
 204 }
 205
 206 // AutoTakeoff Uses a different FSM to its parent
 207 int32_t VtolAutoTakeoffController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
 208 {
 209     PIOS_Assert(ptr_vtolPathFollowerSettings);
 210     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 211
 212     if (fsm == 0) {
 213         fsm = VtolAutoTakeoffFSM::instance();
 214         VtolAutoTakeoffFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus);
 215         controlDown.Initialize(fsm);
 216     }
 217     return 0;
 218 }
 219
 220
 221 void VtolAutoTakeoffController::UpdateVelocityDesired()
 222 {
 223     VelocityStateData velocityState;
 224
 225     VelocityStateGet(&velocityState);
 226     VelocityDesiredData velocityDesired;
 227     PositionStateData positionState;
 228     PositionStateGet(&positionState);
 229
 230     if (fsm->PositionHoldState()) {
 231         controlDown.UpdatePositionState(positionState.Down);
 232         controlDown.ControlPosition();
 233     }
 234
 235     controlDown.UpdateVelocityState(velocityState.Down);
 236     controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 237
 238     controlNE.UpdatePositionState(positionState.North, positionState.East);
 239     controlNE.ControlPosition();
 240
 241     velocityDesired.Down  = controlDown.GetVelocityDesired();
 242     float north, east;
 243     controlNE.GetVelocityDesired(&north, &east);
 244     velocityDesired.North = north;
 245     velocityDesired.East  = east;
 246
 247     // update pathstatus
 248     pathStatus->error     = 0.0f;
 249     pathStatus->fractional_progress = 0.0f;
 250     if (fsm->PositionHoldState()) {
 251         pathStatus->fractional_progress = 1.0f;
 252         // note if the takeoff waypoint and the launch position are significantly different
 253         // the above check might need to expand to assessment of north and east.
 254     }
 255     pathStatus->path_direction_north = velocityDesired.North;
 256     pathStatus->path_direction_east  = velocityDesired.East;
 257     pathStatus->path_direction_down  = velocityDesired.Down;
 258
 259     pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
 260     pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
 261     pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
 262
 263     VelocityDesiredSet(&velocityDesired);
 264 }
 265
 266 int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
 267 {
 268     uint8_t result = 1;
 269     StabilizationDesiredData stabDesired;
 270     AttitudeStateData attitudeState;
 271     StabilizationBankData stabSettings;
 272     float northCommand;
 273     float eastCommand;
 274
 275     StabilizationDesiredGet(&stabDesired);
 276     AttitudeStateGet(&attitudeState);
 277     StabilizationBankGet(&stabSettings);
 278
 279     controlNE.GetNECommand(&northCommand, &eastCommand);
 280     stabDesired.Thrust = controlDown.GetDownCommand();
 281     switch (autotakeoffState) {
 282     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
 283     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
 284     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
 285     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
 286         stabDesired.Thrust = 0.0f;
 287         break;
 288     default:
 289         break;
 290     }
 291
 292     float angle_radians = DEG2RAD(attitudeState.Yaw);
 293     float cos_angle     = cosf(angle_radians);
 294     float sine_angle    = sinf(angle_radians);
 295     float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
 296     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 297     stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
 298     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 299     stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
 300
 301     ManualControlCommandData manualControl;
 302     ManualControlCommandGet(&manualControl);
 303
 304     stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
 305     stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 306
 307     // default thrust mode to cruise control
 308     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
 309
 310     fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
 311     StabilizationDesiredSet(&stabDesired);
 312
 313     return result;
 314 }
 315
 316 void VtolAutoTakeoffController::UpdateAutoPilot()
 317 {
 318     // state machine updates:
 319     // Vtol AutoTakeoff invocation from flight mode requires the following sequence:
 320     // 1. Arming must be done whilst in the AutoTakeOff flight mode
 321     // 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
 322     // 3. Wait for armed state
 323     // 4. Once the user increases the throttle position to above 30%, then and only then initiate auto-takeoff.
 324     // 5. Whilst the throttle is < 30% before takeoff, all stick inputs are being ignored.
 325     // 6. If during the autotakeoff sequence, at any stage, the throttle stick position reduces to less than 10%, landing is initiated.
 326
 327     switch (autotakeoffState) {
 328     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
 329     {
 330         FlightStatusData flightStatus;
 331         FlightStatusGet(&flightStatus);
 332         if (!flightStatus.Armed) {
 333             autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
 334             fsm->setControlState(autotakeoffState);
 335         }
 336     }
 337     break;
 338     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
 339     {
 340         FlightStatusData flightStatus;
 341         FlightStatusGet(&flightStatus);
 342         if (flightStatus.Armed) {
 343             autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
 344             fsm->setControlState(autotakeoffState);
 345         }
 346     }
 347     break;
 348     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
 349     {
 350         ManualControlCommandData cmd;
 351         ManualControlCommandGet(&cmd);
 352
 353         if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
 354             autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
 355             fsm->setControlState(autotakeoffState);
 356         }
 357     }
 358     break;
 359     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
 360     {
 361         ManualControlCommandData cmd;
 362         ManualControlCommandGet(&cmd);
 363         FlightStatusData flightStatus;
 364         FlightStatusGet(&flightStatus);
 365
 366         // we do not do a takeoff abort in pathplanner mode
 367         if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE &&
 368             cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
 369             autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
 370             fsm->setControlState(autotakeoffState);
 371         }
 372     }
 373     break;
 374     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
 375     {
 376         FlightStatusData flightStatus;
 377         FlightStatusGet(&flightStatus);
 378         if (!flightStatus.Armed) {
 379             autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
 380             fsm->setControlState(autotakeoffState);
 381         }
 382     }
 383     break;
 384     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
 385     // nothing to do. land has been requested. stay here for forever until mode change.
 386     default:
 387         break;
 388     }
 389
 390     fsm->Update();
 391
 392     UpdateVelocityDesired();
 393
 394     int8_t result = UpdateStabilizationDesired();
 395     if (result) {
 396         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 397     } else {
 398         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 399         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 400     }
 401
 402     PathStatusSet(pathStatus);
 403 }