flight/modules/ManualControl/manualcontrol.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup ManualControlModule Manual Control Module
   6  * @brief Provide manual control or allow it alter flight mode.
   7  * @{
   8  *
   9  * Reads in the ManualControlCommand FlightMode setting from receiver then either
  10  * pass the settings straght to ActuatorDesired object (manual mode) or to
  11  * AttitudeDesired object (stabilized mode)
  12  *
  13  * @file       manualcontrol.c
  14  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  15  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  16  * @brief      ManualControl module. Handles safety R/C link and flight mode.
  17  *
  18  * @see        The GNU Public License (GPL) Version 3
  19  *
  20  *****************************************************************************/
  21 /*
  22  * This program is free software; you can redistribute it and/or modify
  23  * it under the terms of the GNU General Public License as published by
  24  * the Free Software Foundation; either version 3 of the License, or
  25  * (at your option) any later version.
  26  *
  27  * This program is distributed in the hope that it will be useful, but
  28  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  29  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  30  * for more details.
  31  *
  32  * You should have received a copy of the GNU General Public License along
  33  * with this program; if not, write to the Free Software Foundation, Inc.,
  34  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  35  */
  36
  37 #include "inc/manualcontrol.h"
  38 #include <sanitycheck.h>
  39 #include <manualcontrolsettings.h>
  40 #include <manualcontrolcommand.h>
  41 #include <accessorydesired.h>
  42 #include <vtolselftuningstats.h>
  43 #include <flightmodesettings.h>
  44 #include <flightstatus.h>
  45 #include <systemsettings.h>
  46 #include <stabilizationdesired.h>
  47 #include <callbackinfo.h>
  48 #include <stabilizationsettings.h>
  49 #include <systemalarms.h>
  50 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  51 #include <vtolpathfollowersettings.h>
  52 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
  53
  54 // Private constants
  55 #if defined(PIOS_MANUAL_STACK_SIZE)
  56 #define STACK_SIZE_BYTES                               PIOS_MANUAL_STACK_SIZE
  57 #else
  58 #define STACK_SIZE_BYTES                               1152
  59 #endif
  60
  61 #define CALLBACK_PRIORITY                              CALLBACK_PRIORITY_REGULAR
  62 #define CBTASK_PRIORITY                                CALLBACK_TASK_FLIGHTCONTROL
  63
  64 #define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR    0.2f
  65 #define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
  66 #define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
  67
  68 #define ALWAYSTABILIZEACCESSORY_THRESHOLD              0.05f
  69
  70 // defined handlers
  71
  72 static const controlHandler handler_MANUAL = {
  73     .controlChain      = {
  74         .Stabilization = false,
  75         .PathFollower  = false,
  76         .PathPlanner   = false,
  77     },
  78     .handler           = &manualHandler,
  79 };
  80 static const controlHandler handler_STABILIZED = {
  81     .controlChain      = {
  82         .Stabilization = true,
  83         .PathFollower  = false,
  84         .PathPlanner   = false,
  85     },
  86     .handler           = &stabilizedHandler,
  87 };
  88
  89
  90 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  91 static const controlHandler handler_PATHFOLLOWER = {
  92     .controlChain      = {
  93         .Stabilization = true,
  94         .PathFollower  = true,
  95         .PathPlanner   = false,
  96     },
  97     .handler           = &pathFollowerHandler,
  98 };
  99
 100 static const controlHandler handler_PATHPLANNER = {
 101     .controlChain      = {
 102         .Stabilization = true,
 103         .PathFollower  = true,
 104         .PathPlanner   = true,
 105     },
 106     .handler           = &pathPlannerHandler,
 107 };
 108 static float thrustAtBrakeStart = 0.0f;
 109 static float thrustLo = 0.0f;
 110 static float thrustHi = 0.0f;
 111
 112 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 113 // Private variables
 114 static DelayedCallbackInfo *callbackHandle;
 115 static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
 116
 117 // Private functions
 118 static void configurationUpdatedCb(UAVObjEvent *ev);
 119 static void commandUpdatedCb(UAVObjEvent *ev);
 120 static void manualControlTask(void);
 121 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 122 static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
 123 static void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings);
 124 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 125 static void SettingsUpdatedCb(UAVObjEvent *ev);
 126 #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
 127
 128 /**
 129  * Module starting
 130  */
 131 int32_t ManualControlStart()
 132 {
 133     // Whenever the configuration changes, make sure it is safe to fly
 134     SystemSettingsConnectCallback(configurationUpdatedCb);
 135     ManualControlSettingsConnectCallback(configurationUpdatedCb);
 136     FlightModeSettingsConnectCallback(configurationUpdatedCb);
 137     ManualControlCommandConnectCallback(commandUpdatedCb);
 138
 139     // Run this initially to make sure the configuration is checked
 140     configuration_check();
 141
 142     // clear alarms
 143     AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
 144
 145     SettingsUpdatedCb(NULL);
 146
 147     // Make sure unarmed on power up
 148     armHandler(true, frameType);
 149
 150 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 151     takeOffLocationHandlerInit();
 152 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 153
 154     // Start main task
 155     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 156
 157     return 0;
 158 }
 159
 160 /**
 161  * Module initialization
 162  */
 163 int32_t ManualControlInitialize()
 164 {
 165     /* Check the assumptions about uavobject enum's are correct */
 166     PIOS_STATIC_ASSERT(assumptions);
 167
 168     ManualControlCommandInitialize();
 169     FlightStatusInitialize();
 170     AccessoryDesiredInitialize();
 171 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 172     SystemAlarmsInitialize();
 173     VtolSelfTuningStatsInitialize();
 174     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 175     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 176 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 177     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
 178
 179     return 0;
 180 }
 181 MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
 182
 183 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 184 {
 185     frameType = GetCurrentFrameType();
 186 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 187     VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
 188     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 189
 190
 191     if (frameType == FRAME_TYPE_CUSTOM) {
 192         switch (TreatCustomCraftAs) {
 193         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 194             frameType = FRAME_TYPE_FIXED_WING;
 195             break;
 196         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 197             frameType = FRAME_TYPE_MULTIROTOR;
 198             break;
 199         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 200             frameType = FRAME_TYPE_GROUND;
 201             break;
 202         }
 203     }
 204 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 205 }
 206
 207 /**
 208  * Module task
 209  */
 210 static void manualControlTask(void)
 211 {
 212     // Process Arming
 213     armHandler(false, frameType);
 214 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 215     takeOffLocationHandler();
 216 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 217     // Process flight mode
 218     FlightStatusData flightStatus;
 219
 220     FlightStatusGet(&flightStatus);
 221     ManualControlCommandData cmd;
 222     ManualControlCommandGet(&cmd);
 223     AccessoryDesiredData acc;
 224 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 225     VtolPathFollowerSettingsThrustLimitsData thrustLimits;
 226     VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
 227 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 228
 229     FlightModeSettingsData modeSettings;
 230     FlightModeSettingsGet(&modeSettings);
 231
 232     static uint8_t lastPosition      = 0;
 233     uint8_t position = cmd.FlightModeSwitchPosition;
 234     uint8_t newMode = flightStatus.FlightMode;
 235     uint8_t newAlwaysStabilized      = flightStatus.AlwaysStabilizeWhenArmed;
 236     uint8_t newFlightModeAssist      = flightStatus.FlightModeAssist;
 237     uint8_t newAssistedControlState  = flightStatus.AssistedControlState;
 238     uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState;
 239
 240 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 241     HandleBatteryFailsafe(&position, &modeSettings);
 242 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 243
 244     if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 245         newMode = modeSettings.FlightModePosition[position];
 246     }
 247
 248     // Ignore change to AutoTakeOff and keep last flight mode position
 249     // if vehicle is already armed and maybe in air...
 250     if ((newMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF) && flightStatus.Armed) {
 251         newMode  = flightStatus.FlightMode;
 252         position = lastPosition;
 253     }
 254     // if a mode change occurs we default the assist mode and states here
 255     // to avoid having to add it to all of the below modes that are
 256     // otherwise unrelated
 257     if (newMode != flightStatus.FlightMode) {
 258         // set assist mode to none to avoid an assisted flight mode position
 259         // carrying over and impacting a newly selected non-assisted flight mode pos
 260         newFlightModeAssist      = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 261         // The following are equivalent to none effectively. Code should always
 262         // check the flightmodeassist state.
 263         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 264         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 265     }
 266
 267     // Depending on the mode update the Stabilization or Actuator objects
 268     const controlHandler *handler = &handler_MANUAL;
 269     switch (newMode) {
 270     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
 271         handler = &handler_MANUAL;
 272         break;
 273     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 274     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 275     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 276     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 277     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 278     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 279 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 280     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
 281 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 282         handler = &handler_STABILIZED;
 283
 284 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 285         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
 286
 287         if (newFlightModeAssist != flightStatus.FlightModeAssist) {
 288             // On change of assist mode reinitialise control state.  This is required
 289             // for the scenario where a flight position change reuses a flight mode
 290             // but adds assistedcontrol.
 291             newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 292             newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 293         }
 294
 295         if (newFlightModeAssist) {
 296             // assess roll/pitch state
 297             bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
 298
 299             // assess throttle state
 300             bool throttleNeutral = false;
 301             float neutralThrustOffset = 0.0f;
 302             VtolSelfTuningStatsNeutralThrustOffsetGet(&neutralThrustOffset);
 303
 304
 305             float throttleRangeDelta = (thrustLimits.Neutral + neutralThrustOffset) * ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR;
 306             float throttleNeutralLow = (thrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
 307             float throttleNeutralHi  = (thrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
 308             if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
 309                 throttleNeutral = true;
 310             }
 311
 312             // determine default thrust mode for hold/brake states
 313             uint8_t pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 314             if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
 315                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
 316             }
 317
 318             switch (newAssistedControlState) {
 319             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
 320                 if (!flagRollPitchHasInput) {
 321                     // no stick input on roll/pitch so enter brake state
 322                     newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
 323                     newAssistedThrottleState = pathfollowerthrustmode;
 324                     handler = &handler_PATHFOLLOWER;
 325                     // retain thrust cmd for later comparison with actual in braking
 326                     thrustAtBrakeStart = cmd.Thrust;
 327
 328                     // calculate hi and low value of +-4% as a mini-deadband
 329                     // for use in auto-override in brake sequence
 330                     thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
 331                     thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
 332
 333                     // The purpose for auto throttle assist is to go from a mid to high thrust range to a
 334                     // neutral vertical-holding/maintaining ~50% thrust range.  It is not designed/intended
 335                     // to go from near zero to 50%...we don't want an auto-takeoff feature here!
 336                     // Also for rapid decents a user might have a bit of forward stick and low throttle
 337                     // then stick-off for auto-braking...but if below the vtol min of 20% it will not
 338                     // kick in...the flyer needs to manually manage throttle to slow down decent,
 339                     // and the next time they put in a bit of stick, revert to primary, and then
 340                     // sticks-off it will brake and hold with auto-thrust
 341                     if (thrustAtBrakeStart < thrustLimits.Min) {
 342                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 343                     }
 344                 } else {
 345                     // stick input so stay in primary mode control state
 346                     // newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 347                     newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 348                 }
 349                 break;
 350
 351             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
 352                 if (flagRollPitchHasInput) {
 353                     // stick input during brake sequence allows immediate resumption of control
 354                     newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 355                     newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 356                 } else {
 357                     // no stick input, stay in brake mode
 358                     newAssistedThrottleState = pathfollowerthrustmode;
 359                     handler = &handler_PATHFOLLOWER;
 360
 361                     // if auto thrust and user adjusts thrust outside of a deadband in which case revert to manual
 362                     if ((newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO) &&
 363                         (cmd.Thrust < thrustLo || cmd.Thrust > thrustHi)) {
 364                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 365                     }
 366                 }
 367                 break;
 368
 369             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
 370
 371                 if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST ||
 372                     (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
 373                      newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
 374                     // for manual or primary throttle states/modes, stick control immediately reverts to primary mode control
 375                     if (flagRollPitchHasInput) {
 376                         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 377                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 378                     } else {
 379                         // otherwise stay in the hold state
 380                         // newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
 381                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 382                         handler = &handler_PATHFOLLOWER;
 383                     }
 384                 } else if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
 385                            newAssistedThrottleState != FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
 386                     // ok auto thrust mode applies in hold unless overridden
 387
 388                     if (flagRollPitchHasInput) {
 389                         // throttle is neutral approximately and stick input present so revert to primary mode control
 390                         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 391                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 392                     } else {
 393                         // otherwise hold, autothrust, no stick input...we watch throttle for need to change mode
 394                         switch (newAssistedThrottleState) {
 395                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO:
 396                             // whilst in auto, watch for neutral range, from which we allow override
 397                             if (throttleNeutral) {
 398                                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE;
 399                             } else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO; }
 400                             break;
 401                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE:
 402                             // previously user has set throttle to neutral range, apply a deadband and revert to manual
 403                             // if moving out of deadband.  This allows for landing in hold state.
 404                             if (!throttleNeutral) {
 405                                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 406                             } else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE; }
 407                             break;
 408                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL:
 409                             pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 410                             break;
 411                         }
 412
 413                         newAssistedThrottleState = pathfollowerthrustmode;
 414                         handler = &handler_PATHFOLLOWER;
 415                     }
 416                 }
 417                 break;
 418             }
 419         }
 420 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 421         break;
 422 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 423
 424     // During development the assistedcontrol implementation is optional and set
 425     // set in stabi settings.  Once if we decide to always have this on, it can
 426     // can be directly set here...i.e. set the flight mode assist as required.
 427     case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 428         newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
 429         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 430         handler = &handler_PATHFOLLOWER;
 431         break;
 432
 433     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 434     case FLIGHTSTATUS_FLIGHTMODE_LAND:
 435     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 436         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
 437         if (newFlightModeAssist) {
 438             // Set the default thrust state
 439             switch (newFlightModeAssist) {
 440             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
 441                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 442                 break;
 443             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST:
 444                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
 445                 break;
 446             case FLIGHTSTATUS_FLIGHTMODEASSIST_NONE:
 447             default:
 448                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 449                 break;
 450             }
 451         }
 452         handler = &handler_PATHFOLLOWER;
 453         break;
 454
 455     case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
 456     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
 457     case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
 458     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
 459     case FLIGHTSTATUS_FLIGHTMODE_POI:
 460     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
 461         handler = &handler_PATHFOLLOWER;
 462         break;
 463     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
 464         handler = &handler_PATHPLANNER;
 465         break;
 466 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 467         // There is no default, so if a flightmode is forgotten the compiler can throw a warning!
 468     }
 469
 470     bool alwaysStabilizedSwitch = false;
 471
 472     // Check for a AlwaysStabilizeWhenArmed accessory switch
 473     switch (modeSettings.AlwaysStabilizeWhenArmedSwitch) {
 474     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY0:
 475         AccessoryDesiredInstGet(0, &acc);
 476         alwaysStabilizedSwitch = true;
 477         break;
 478     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY1:
 479         AccessoryDesiredInstGet(1, &acc);
 480         alwaysStabilizedSwitch = true;
 481         break;
 482     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY2:
 483         AccessoryDesiredInstGet(2, &acc);
 484         alwaysStabilizedSwitch = true;
 485         break;
 486     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY3:
 487         AccessoryDesiredInstGet(3, &acc);
 488         alwaysStabilizedSwitch = true;
 489         break;
 490     default:
 491         break;
 492     }
 493
 494     if (alwaysStabilizedSwitch) {
 495         if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) {
 496             newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
 497         } else if ((acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) &&
 498                    (cmd.Throttle >= modeSettings.AlwaysStabilizeWhenArmedThrottleThreshold)) {
 499             newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE;
 500         }
 501     } else {
 502         newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
 503     }
 504
 505     bool newinit = false;
 506
 507     // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
 508     static bool firstRun = true;
 509
 510     if (flightStatus.AlwaysStabilizeWhenArmed != newAlwaysStabilized ||
 511         flightStatus.FlightMode != newMode || firstRun ||
 512         newFlightModeAssist != flightStatus.FlightModeAssist ||
 513         newAssistedControlState != flightStatus.AssistedControlState ||
 514         flightStatus.AssistedThrottleState != newAssistedThrottleState) {
 515         firstRun = false;
 516         flightStatus.ControlChain             = handler->controlChain;
 517         flightStatus.FlightMode               = newMode;
 518         flightStatus.AlwaysStabilizeWhenArmed = newAlwaysStabilized;
 519         flightStatus.FlightModeAssist         = newFlightModeAssist;
 520         flightStatus.AssistedControlState     = newAssistedControlState;
 521         flightStatus.AssistedThrottleState    = newAssistedThrottleState;
 522         FlightStatusSet(&flightStatus);
 523         newinit = true;
 524         lastPosition = position;
 525     }
 526     if (handler->handler) {
 527         handler->handler(newinit);
 528     }
 529 }
 530
 531 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 532 void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings)
 533 {
 534     // static uint8_t lastInputPosition = -1;
 535     typedef enum { BATTERYFAILSAFE_NONE = 0, BATTERYFAILSAFE_WARNING = 1, BATTERYFAILSAFE_CRITICAL = 2 } batteryfailsafemode_t;
 536     static batteryfailsafemode_t lastFailsafeStatus = BATTERYFAILSAFE_NONE;
 537     static bool failsafeOverridden;
 538     static uint8_t lastFlightPosition;
 539     static uint32_t changeTimestamp;
 540     SystemAlarmsAlarmData alarms;
 541     batteryfailsafemode_t failsafeStatus;
 542     FlightStatusArmedOptions armed;
 543     FlightStatusArmedGet(&armed);
 544
 545     // reset the status and do not change anything when not armed
 546     if (armed != FLIGHTSTATUS_ARMED_ARMED) {
 547         lastFailsafeStatus = BATTERYFAILSAFE_NONE;
 548         failsafeOverridden = false;
 549         changeTimestamp    = PIOS_DELAY_GetRaw();
 550         lastFlightPosition = *position;
 551         return;
 552     }
 553
 554     SystemAlarmsAlarmGet(&alarms);
 555
 556     switch (alarms.Battery) {
 557     case SYSTEMALARMS_ALARM_WARNING:
 558         failsafeStatus = BATTERYFAILSAFE_WARNING;
 559         break;
 560     case SYSTEMALARMS_ALARM_CRITICAL:
 561         failsafeStatus = BATTERYFAILSAFE_CRITICAL;
 562         break;
 563     default:
 564         failsafeStatus = BATTERYFAILSAFE_NONE;
 565         break;
 566     }
 567     uint32_t debounceTimerms = PIOS_DELAY_DiffuS(changeTimestamp) / 1000;
 568
 569     if (failsafeStatus == lastFailsafeStatus) {
 570         changeTimestamp = PIOS_DELAY_GetRaw();
 571     } else if ((debounceTimerms < modeSettings->BatteryFailsafeDebounceTimer) || failsafeStatus < lastFailsafeStatus) {
 572         // do not change within the "grace" period and do not "downgrade" the failsafe mode
 573         failsafeStatus = lastFailsafeStatus;
 574     } else {
 575         // a higher failsafe status was met and grace period elapsed. Trigger the new state
 576         lastFailsafeStatus = failsafeStatus;
 577         lastFlightPosition = *position;
 578         failsafeOverridden = false;
 579     }
 580
 581     if ((failsafeStatus == BATTERYFAILSAFE_NONE) || failsafeOverridden) {
 582         return;
 583     }
 584
 585     // failsafe has been triggered. Check for override
 586     if (lastFlightPosition != *position) {
 587         // flag the override and reset the grace period
 588         failsafeOverridden = true;
 589         changeTimestamp    = PIOS_DELAY_GetRaw();
 590         return;
 591     }
 592
 593     switch (failsafeStatus) {
 594     case BATTERYFAILSAFE_CRITICAL:
 595         // if critical is not set, jump to the other case to use the warning setting.
 596         if (modeSettings->BatteryFailsafeSwitchPositions.Critical != -1) {
 597             *position = modeSettings->BatteryFailsafeSwitchPositions.Critical;
 598             break;
 599         }
 600     case BATTERYFAILSAFE_WARNING:
 601         if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) {
 602             *position = modeSettings->BatteryFailsafeSwitchPositions.Warning;
 603         }
 604         break;
 605     default:
 606         break;
 607     }
 608 }
 609
 610 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 611
 612 /**
 613  * Called whenever a critical configuration component changes
 614  */
 615 static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 616 {
 617     configuration_check();
 618 }
 619
 620 /**
 621  * Called whenever a critical configuration component changes
 622  */
 623 static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 624 {
 625     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 626 }
 627
 628
 629 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 630 /**
 631  * Check and set modes for gps assisted stablised flight modes
 632  */
 633 static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
 634 {
 635     StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 636
 637     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
 638     if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
 639         || position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
 640         return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 641     }
 642
 643     switch (FlightModeAssistMap[position]) {
 644     case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
 645         break;
 646     case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
 647     {
 648         // default to cruise control.
 649         FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
 650
 651         switch (flightMode) {
 652         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 653             thrustMode = modeSettings->Stabilization1Settings.Thrust;
 654             break;
 655         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 656             thrustMode = modeSettings->Stabilization2Settings.Thrust;
 657             break;
 658         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 659             thrustMode = modeSettings->Stabilization3Settings.Thrust;
 660             break;
 661         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 662             thrustMode = modeSettings->Stabilization4Settings.Thrust;
 663             break;
 664         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 665             thrustMode = modeSettings->Stabilization5Settings.Thrust;
 666             break;
 667         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 668             thrustMode = modeSettings->Stabilization6Settings.Thrust;
 669             break;
 670         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 671         case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 672             // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
 673             // is a more appropriate throttle mode.  "GPSAssist" adds braking
 674             // and a better throttle management to the standard Position Hold.
 675             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
 676             break;
 677         case FLIGHTSTATUS_FLIGHTMODE_LAND:
 678         case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 679             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
 680             break;
 681
 682             // other modes will use cruisecontrol as default
 683         }
 684
 685
 686         switch (thrustMode) {
 687         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
 688         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
 689             // this is only for use with stabi mods with althold/vario.
 690             return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
 691
 692         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
 693         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
 694         default:
 695             // this is the default for non stabi modes also
 696             return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
 697         }
 698     }
 699     break;
 700     }
 701
 702     // return isAssistedFlag;
 703     return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 704 }
 705 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 706
 707 /**
 708  * @}
 709  * @}
 710  */