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