flight/modules/ManualControl/armhandler.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup ManualControl
   6  * @brief Interpretes the control input in ManualControlCommand
   7  * @{
   8  *
   9  * @file       armhandler.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  ******************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "inc/manualcontrol.h"
  33 #include <sanitycheck.h>
  34 #include <manualcontrolcommand.h>
  35 #include <accessorydesired.h>
  36 #include <flightstatus.h>
  37 #include <flightmodesettings.h>
  38 #include <stabilizationdesired.h>
  39 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  40 #include <statusvtolland.h>
  41 #endif
  42
  43 // Private constants
  44 #define ARMED_THRESHOLD_SWITCH 0.20f
  45 #define ARMED_THRESHOLD_STICK  0.80f
  46 #define GROUND_LOW_THROTTLE    0.01f
  47
  48 // Private types
  49 typedef enum {
  50     ARM_STATE_DISARMED,
  51     ARM_STATE_ARMING_MANUAL,
  52     ARM_STATE_ARMED,
  53     ARM_STATE_DISARMING_MANUAL,
  54     ARM_STATE_DISARMING_TIMEOUT
  55 } ArmState_t;
  56
  57 // Private variables
  58
  59 // Private functions
  60 static void setArmedIfChanged(uint8_t val);
  61 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
  62 static bool okToArm(void);
  63 static bool forcedDisArm(void);
  64
  65
  66 /**
  67  * @brief Handler to interprete Command inputs in regards to arming/disarming
  68  * @input: ManualControlCommand, AccessoryDesired
  69  * @output: FlightStatus.Arming
  70  */
  71 void armHandler(bool newinit, FrameType_t frameType)
  72 {
  73     static ArmState_t armState;
  74
  75     if (newinit) {
  76         AccessoryDesiredInitialize();
  77         setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
  78         armState = ARM_STATE_DISARMED;
  79     }
  80
  81     portTickType sysTime = xTaskGetTickCount();
  82
  83     FlightModeSettingsData settings;
  84     FlightModeSettingsGet(&settings);
  85     ManualControlCommandData cmd;
  86     ManualControlCommandGet(&cmd);
  87     AccessoryDesiredData acc;
  88
  89     bool lowThrottle = cmd.Throttle < 0;
  90
  91     if (frameType == FRAME_TYPE_GROUND) {
  92         // Deadbanding applied in receiver.c typically at 2% but we don't assume its enabled.
  93         lowThrottle = fabsf(cmd.Throttle) < GROUND_LOW_THROTTLE;
  94     }
  95
  96     bool armSwitch = false;
  97
  98     switch (settings.Arming) {
  99     case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
 100         AccessoryDesiredInstGet(0, &acc);
 101         armSwitch = true;
 102         break;
 103     case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
 104         AccessoryDesiredInstGet(1, &acc);
 105         armSwitch = true;
 106         break;
 107     case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
 108         AccessoryDesiredInstGet(2, &acc);
 109         armSwitch = true;
 110         break;
 111     case FLIGHTMODESETTINGS_ARMING_ACCESSORY3:
 112         AccessoryDesiredInstGet(3, &acc);
 113         armSwitch = true;
 114         break;
 115     default:
 116         break;
 117     }
 118
 119     // immediate disarm on switch
 120     if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD_SWITCH) {
 121         lowThrottle = true;
 122     }
 123
 124     if (forcedDisArm()) {
 125         // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
 126         armState = ARM_STATE_DISARMED;
 127         setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
 128         return;
 129     }
 130
 131     if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
 132         // In this configuration we always disarm
 133         setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
 134         return;
 135     }
 136
 137
 138     // The throttle is not low, in case we where arming or disarming, abort
 139     if (!lowThrottle) {
 140         switch (armState) {
 141         case ARM_STATE_DISARMING_MANUAL:
 142         case ARM_STATE_DISARMING_TIMEOUT:
 143             armState = ARM_STATE_ARMED;
 144             break;
 145         case ARM_STATE_ARMING_MANUAL:
 146             armState = ARM_STATE_DISARMED;
 147             break;
 148         default:
 149             // Nothing needs to be done in the other states
 150             break;
 151         }
 152         return;
 153     }
 154
 155     // The rest of these cases throttle is low
 156     if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) {
 157         // In this configuration, we go into armed state as soon as the throttle is low, never disarm
 158         setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
 159         return;
 160     }
 161
 162     // When the configuration is not "Always armed" and no "Always disarmed",
 163     // the state will not be changed when the throttle is not low
 164     static portTickType armedDisarmStart;
 165     float armingInputLevel = 0;
 166
 167     // Calc channel see assumptions7
 168     switch (settings.Arming) {
 169     case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
 170         armingInputLevel = 1.0f * cmd.Roll;
 171         break;
 172     case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
 173         armingInputLevel = -1.0f * cmd.Roll;
 174         break;
 175     case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
 176         armingInputLevel = 1.0f * cmd.Pitch;
 177         break;
 178     case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
 179         armingInputLevel = -1.0f * cmd.Pitch;
 180         break;
 181     case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
 182         armingInputLevel = 1.0f * cmd.Yaw;
 183         break;
 184     case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
 185         armingInputLevel = -1.0f * cmd.Yaw;
 186         break;
 187     case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
 188     case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
 189     case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
 190     case FLIGHTMODESETTINGS_ARMING_ACCESSORY3:
 191         armingInputLevel = -1.0f * acc.AccessoryVal;
 192         break;
 193     default:
 194         break;
 195     }
 196
 197     bool manualArm    = false;
 198     bool manualDisarm = false;
 199
 200     static FlightModeSettingsArmingOptions previousArmingSettings = -1;
 201     static float previousArmingInputLevel = 0.0f;
 202
 203     if (previousArmingSettings != settings.Arming) {
 204         previousArmingSettings   = settings.Arming;
 205         previousArmingInputLevel = 0.0f;
 206     }
 207
 208     // ignore previous arming input level if not transitioning from fully ARMED/DISARMED states.
 209     if ((armState != ARM_STATE_DISARMED) && (armState != ARM_STATE_ARMED)) {
 210         previousArmingInputLevel = 0.0f;
 211     }
 212
 213     float armedThreshold = armSwitch ? ARMED_THRESHOLD_SWITCH : ARMED_THRESHOLD_STICK;
 214
 215     if ((armingInputLevel <= -armedThreshold) && (previousArmingInputLevel > -armedThreshold)) {
 216         manualArm = true;
 217     } else if ((armingInputLevel >= +armedThreshold) && (previousArmingInputLevel < +armedThreshold)) {
 218         manualDisarm = true;
 219     }
 220
 221     previousArmingInputLevel = armingInputLevel;
 222
 223     switch (armState) {
 224     case ARM_STATE_DISARMED:
 225         setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
 226
 227         // only allow arming if it's OK too
 228         if (manualArm && okToArm()) {
 229             armedDisarmStart = sysTime;
 230             armState = ARM_STATE_ARMING_MANUAL;
 231         }
 232         break;
 233
 234     case ARM_STATE_ARMING_MANUAL:
 235         setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
 236
 237         if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) {
 238             armState = ARM_STATE_ARMED;
 239         } else if (!manualArm) {
 240             armState = ARM_STATE_DISARMED;
 241         }
 242         break;
 243
 244     case ARM_STATE_ARMED:
 245         // When we get here, the throttle is low,
 246         // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
 247         armedDisarmStart = sysTime;
 248         armState = ARM_STATE_DISARMING_TIMEOUT;
 249         setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
 250         break;
 251
 252     case ARM_STATE_DISARMING_TIMEOUT:
 253     {
 254         // we should never reach the disarming timeout if the pathfollower is engaged - reset timeout
 255         FlightStatusControlChainData cc;
 256         FlightStatusControlChainGet(&cc);
 257         if (cc.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 258             armedDisarmStart = sysTime;
 259         }
 260     }
 261
 262         // We get here when armed while throttle low, even when the arming timeout is not enabled
 263         if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) {
 264             armState = ARM_STATE_DISARMED;
 265         }
 266
 267         // Switch to disarming due to manual control when needed
 268         if (manualDisarm) {
 269             armedDisarmStart = sysTime;
 270             armState = ARM_STATE_DISARMING_MANUAL;
 271         }
 272         break;
 273
 274     case ARM_STATE_DISARMING_MANUAL:
 275         // arming switch disarms immediately,
 276         if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) {
 277             armState = ARM_STATE_DISARMED;
 278         } else if (!manualDisarm) {
 279             armState = ARM_STATE_ARMED;
 280         }
 281         break;
 282     } // End Switch
 283 }
 284
 285 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
 286 {
 287     return (end_time - start_time) * portTICK_RATE_MS;
 288 }
 289
 290 /**
 291  * @brief Determine if the aircraft is safe to arm
 292  * @returns True if safe to arm, false otherwise
 293  */
 294 static bool okToArm(void)
 295 {
 296     // update checks
 297     configuration_check();
 298
 299     // read alarms
 300     SystemAlarmsData alarms;
 301
 302     SystemAlarmsGet(&alarms);
 303
 304     // Check each alarm
 305     for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
 306         if (SystemAlarmsAlarmToArray(alarms.Alarm)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
 307             if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
 308                 continue;
 309             }
 310
 311             return false;
 312         }
 313     }
 314
 315     StabilizationDesiredStabilizationModeData stabDesired;
 316
 317     FlightStatusData flightStatus;
 318     FlightStatusGet(&flightStatus);
 319     switch (flightStatus.FlightMode) {
 320     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
 321     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 322     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 323     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 324     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 325     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 326     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 327         // Prevent arming if unsafe due to the current Thrust Mode
 328         StabilizationDesiredStabilizationModeGet(&stabDesired);
 329         if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ||
 330             stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) {
 331             return false;
 332         }
 333
 334         // avoid assisted control with auto throttle.  As it sits waiting to launch,
 335         // it will move to hold, and auto thrust will auto launch otherwise!
 336         if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
 337             return false;
 338         }
 339
 340         // Avoid arming while AlwaysStabilizeWhenArmed is active
 341         if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) {
 342             return false;
 343         }
 344
 345         return true;
 346
 347         break;
 348     case FLIGHTSTATUS_FLIGHTMODE_LAND:
 349 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 350     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
 351 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 352         return false;
 353
 354     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 355     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
 356         // Avoid arming while AlwaysStabilizeWhenArmed is active
 357         if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) {
 358             return false;
 359         }
 360         return true;
 361
 362     default:
 363         return false;
 364
 365         break;
 366     }
 367 }
 368 /**
 369  * @brief Determine if the aircraft is forced to disarm by an explicit alarm
 370  * @returns True if safe to arm, false otherwise
 371  */
 372 static bool forcedDisArm(void)
 373 {
 374     // read alarms
 375     SystemAlarmsAlarmData alarms;
 376
 377     SystemAlarmsAlarmGet(&alarms);
 378
 379     if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
 380         return true;
 381     }
 382     if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
 383         return true;
 384     }
 385
 386     return false;
 387 }
 388
 389 /**
 390  * @brief Update the flightStatus object only if value changed.  Reduces callbacks
 391  * @param[in] val The new value
 392  */
 393 static void setArmedIfChanged(uint8_t val)
 394 {
 395     FlightStatusData flightStatus;
 396
 397     FlightStatusGet(&flightStatus);
 398
 399     if (flightStatus.Armed != val) {
 400         flightStatus.Armed = val;
 401         FlightStatusSet(&flightStatus);
 402     }
 403 }
 404
 405 /**
 406  * @}
 407  * @}
 408  */