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     ManualControlSettingsInitialize();
 171     FlightModeSettingsInitialize();
 172     SystemSettingsInitialize();
 173     StabilizationSettingsInitialize();
 174     AccessoryDesiredInitialize();
 175 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 176     SystemAlarmsInitialize();
 177     VtolSelfTuningStatsInitialize();
 178     VtolPathFollowerSettingsInitialize();
 179     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 180     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 181 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 182     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
 183
 184     return 0;
 185 }
 186 MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
 187
 188 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 189 {
 190     frameType = GetCurrentFrameType();
 191 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 192     VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
 193     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 194
 195
 196     if (frameType == FRAME_TYPE_CUSTOM) {
 197         switch (TreatCustomCraftAs) {
 198         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 199             frameType = FRAME_TYPE_FIXED_WING;
 200             break;
 201         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 202             frameType = FRAME_TYPE_MULTIROTOR;
 203             break;
 204         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 205             frameType = FRAME_TYPE_GROUND;
 206             break;
 207         }
 208     }
 209 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 210 }
 211
 212 /**
 213  * Module task
 214  */
 215 static void manualControlTask(void)
 216 {
 217     // Process Arming
 218     armHandler(false, frameType);
 219 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 220     takeOffLocationHandler();
 221 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 222     // Process flight mode
 223     FlightStatusData flightStatus;
 224
 225     FlightStatusGet(&flightStatus);
 226     ManualControlCommandData cmd;
 227     ManualControlCommandGet(&cmd);
 228     AccessoryDesiredData acc;
 229 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 230     VtolPathFollowerSettingsThrustLimitsData thrustLimits;
 231     VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
 232 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 233
 234     FlightModeSettingsData modeSettings;
 235     FlightModeSettingsGet(&modeSettings);
 236
 237     static uint8_t lastPosition      = 0;
 238     uint8_t position = cmd.FlightModeSwitchPosition;
 239     uint8_t newMode = flightStatus.FlightMode;
 240     uint8_t newAlwaysStabilized      = flightStatus.AlwaysStabilizeWhenArmed;
 241     uint8_t newFlightModeAssist      = flightStatus.FlightModeAssist;
 242     uint8_t newAssistedControlState  = flightStatus.AssistedControlState;
 243     uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState;
 244
 245 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 246     HandleBatteryFailsafe(&position, &modeSettings);
 247 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 248
 249     if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
 250         newMode = modeSettings.FlightModePosition[position];
 251     }
 252
 253     // Ignore change to AutoTakeOff and keep last flight mode position
 254     // if vehicle is already armed and maybe in air...
 255     if ((newMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF) && flightStatus.Armed) {
 256         newMode  = flightStatus.FlightMode;
 257         position = lastPosition;
 258     }
 259     // if a mode change occurs we default the assist mode and states here
 260     // to avoid having to add it to all of the below modes that are
 261     // otherwise unrelated
 262     if (newMode != flightStatus.FlightMode) {
 263         // set assist mode to none to avoid an assisted flight mode position
 264         // carrying over and impacting a newly selected non-assisted flight mode pos
 265         newFlightModeAssist      = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 266         // The following are equivalent to none effectively. Code should always
 267         // check the flightmodeassist state.
 268         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 269         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 270     }
 271
 272     // Depending on the mode update the Stabilization or Actuator objects
 273     const controlHandler *handler = &handler_MANUAL;
 274     switch (newMode) {
 275     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
 276         handler = &handler_MANUAL;
 277         break;
 278     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 279     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 280     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 281     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 282     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 283     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 284 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 285     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
 286 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 287         handler = &handler_STABILIZED;
 288
 289 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 290         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
 291
 292         if (newFlightModeAssist != flightStatus.FlightModeAssist) {
 293             // On change of assist mode reinitialise control state.  This is required
 294             // for the scenario where a flight position change reuses a flight mode
 295             // but adds assistedcontrol.
 296             newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 297             newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 298         }
 299
 300         if (newFlightModeAssist) {
 301             // assess roll/pitch state
 302             bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
 303
 304             // assess throttle state
 305             bool throttleNeutral = false;
 306             float neutralThrustOffset = 0.0f;
 307             VtolSelfTuningStatsNeutralThrustOffsetGet(&neutralThrustOffset);
 308
 309
 310             float throttleRangeDelta = (thrustLimits.Neutral + neutralThrustOffset) * ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR;
 311             float throttleNeutralLow = (thrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
 312             float throttleNeutralHi  = (thrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
 313             if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
 314                 throttleNeutral = true;
 315             }
 316
 317             // determine default thrust mode for hold/brake states
 318             uint8_t pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 319             if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
 320                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
 321             }
 322
 323             switch (newAssistedControlState) {
 324             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
 325                 if (!flagRollPitchHasInput) {
 326                     // no stick input on roll/pitch so enter brake state
 327                     newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
 328                     newAssistedThrottleState = pathfollowerthrustmode;
 329                     handler = &handler_PATHFOLLOWER;
 330                     // retain thrust cmd for later comparison with actual in braking
 331                     thrustAtBrakeStart = cmd.Thrust;
 332
 333                     // calculate hi and low value of +-4% as a mini-deadband
 334                     // for use in auto-override in brake sequence
 335                     thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
 336                     thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
 337
 338                     // The purpose for auto throttle assist is to go from a mid to high thrust range to a
 339                     // neutral vertical-holding/maintaining ~50% thrust range.  It is not designed/intended
 340                     // to go from near zero to 50%...we don't want an auto-takeoff feature here!
 341                     // Also for rapid decents a user might have a bit of forward stick and low throttle
 342                     // then stick-off for auto-braking...but if below the vtol min of 20% it will not
 343                     // kick in...the flyer needs to manually manage throttle to slow down decent,
 344                     // and the next time they put in a bit of stick, revert to primary, and then
 345                     // sticks-off it will brake and hold with auto-thrust
 346                     if (thrustAtBrakeStart < thrustLimits.Min) {
 347                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 348                     }
 349                 } else {
 350                     // stick input so stay in primary mode control state
 351                     // newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 352                     newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 353                 }
 354                 break;
 355
 356             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
 357                 if (flagRollPitchHasInput) {
 358                     // stick input during brake sequence allows immediate resumption of control
 359                     newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 360                     newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 361                 } else {
 362                     // no stick input, stay in brake mode
 363                     newAssistedThrottleState = pathfollowerthrustmode;
 364                     handler = &handler_PATHFOLLOWER;
 365
 366                     // if auto thrust and user adjusts thrust outside of a deadband in which case revert to manual
 367                     if ((newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO) &&
 368                         (cmd.Thrust < thrustLo || cmd.Thrust > thrustHi)) {
 369                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 370                     }
 371                 }
 372                 break;
 373
 374             case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
 375
 376                 if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST ||
 377                     (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
 378                      newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
 379                     // for manual or primary throttle states/modes, stick control immediately reverts to primary mode control
 380                     if (flagRollPitchHasInput) {
 381                         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 382                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 383                     } else {
 384                         // otherwise stay in the hold state
 385                         // newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
 386                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 387                         handler = &handler_PATHFOLLOWER;
 388                     }
 389                 } else if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
 390                            newAssistedThrottleState != FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
 391                     // ok auto thrust mode applies in hold unless overridden
 392
 393                     if (flagRollPitchHasInput) {
 394                         // throttle is neutral approximately and stick input present so revert to primary mode control
 395                         newAssistedControlState  = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 396                         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 397                     } else {
 398                         // otherwise hold, autothrust, no stick input...we watch throttle for need to change mode
 399                         switch (newAssistedThrottleState) {
 400                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO:
 401                             // whilst in auto, watch for neutral range, from which we allow override
 402                             if (throttleNeutral) {
 403                                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE;
 404                             } else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO; }
 405                             break;
 406                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE:
 407                             // previously user has set throttle to neutral range, apply a deadband and revert to manual
 408                             // if moving out of deadband.  This allows for landing in hold state.
 409                             if (!throttleNeutral) {
 410                                 pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 411                             } else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE; }
 412                             break;
 413                         case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL:
 414                             pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 415                             break;
 416                         }
 417
 418                         newAssistedThrottleState = pathfollowerthrustmode;
 419                         handler = &handler_PATHFOLLOWER;
 420                     }
 421                 }
 422                 break;
 423             }
 424         }
 425 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 426         break;
 427 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 428
 429     // During development the assistedcontrol implementation is optional and set
 430     // set in stabi settings.  Once if we decide to always have this on, it can
 431     // can be directly set here...i.e. set the flight mode assist as required.
 432     case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 433         newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
 434         newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 435         handler = &handler_PATHFOLLOWER;
 436         break;
 437
 438     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 439     case FLIGHTSTATUS_FLIGHTMODE_LAND:
 440     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 441         newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
 442         if (newFlightModeAssist) {
 443             // Set the default thrust state
 444             switch (newFlightModeAssist) {
 445             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
 446                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
 447                 break;
 448             case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST:
 449                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
 450                 break;
 451             case FLIGHTSTATUS_FLIGHTMODEASSIST_NONE:
 452             default:
 453                 newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
 454                 break;
 455             }
 456         }
 457         handler = &handler_PATHFOLLOWER;
 458         break;
 459
 460     case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
 461     case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
 462     case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
 463     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
 464     case FLIGHTSTATUS_FLIGHTMODE_POI:
 465     case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
 466         handler = &handler_PATHFOLLOWER;
 467         break;
 468     case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
 469         handler = &handler_PATHPLANNER;
 470         break;
 471 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 472         // There is no default, so if a flightmode is forgotten the compiler can throw a warning!
 473     }
 474
 475     bool alwaysStabilizedSwitch = false;
 476
 477     // Check for a AlwaysStabilizeWhenArmed accessory switch
 478     switch (modeSettings.AlwaysStabilizeWhenArmedSwitch) {
 479     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY0:
 480         AccessoryDesiredInstGet(0, &acc);
 481         alwaysStabilizedSwitch = true;
 482         break;
 483     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY1:
 484         AccessoryDesiredInstGet(1, &acc);
 485         alwaysStabilizedSwitch = true;
 486         break;
 487     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY2:
 488         AccessoryDesiredInstGet(2, &acc);
 489         alwaysStabilizedSwitch = true;
 490         break;
 491     case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY3:
 492         AccessoryDesiredInstGet(3, &acc);
 493         alwaysStabilizedSwitch = true;
 494         break;
 495     default:
 496         break;
 497     }
 498
 499     if (alwaysStabilizedSwitch) {
 500         if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) {
 501             newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
 502         } else if (acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) {
 503             newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE;
 504         }
 505     } else {
 506         newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
 507     }
 508
 509     bool newinit = false;
 510
 511     // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
 512     static bool firstRun = true;
 513
 514     if (flightStatus.AlwaysStabilizeWhenArmed != newAlwaysStabilized ||
 515         flightStatus.FlightMode != newMode || firstRun ||
 516         newFlightModeAssist != flightStatus.FlightModeAssist ||
 517         newAssistedControlState != flightStatus.AssistedControlState ||
 518         flightStatus.AssistedThrottleState != newAssistedThrottleState) {
 519         firstRun = false;
 520         flightStatus.ControlChain             = handler->controlChain;
 521         flightStatus.FlightMode               = newMode;
 522         flightStatus.AlwaysStabilizeWhenArmed = newAlwaysStabilized;
 523         flightStatus.FlightModeAssist         = newFlightModeAssist;
 524         flightStatus.AssistedControlState     = newAssistedControlState;
 525         flightStatus.AssistedThrottleState    = newAssistedThrottleState;
 526         FlightStatusSet(&flightStatus);
 527         newinit = true;
 528         lastPosition = position;
 529     }
 530     if (handler->handler) {
 531         handler->handler(newinit);
 532     }
 533 }
 534
 535 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 536 void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings)
 537 {
 538     // static uint8_t lastInputPosition = -1;
 539     typedef enum { BATTERYFAILSAFE_NONE = 0, BATTERYFAILSAFE_WARNING = 1, BATTERYFAILSAFE_CRITICAL = 2 } batteryfailsafemode_t;
 540     static batteryfailsafemode_t lastFailsafeStatus = BATTERYFAILSAFE_NONE;
 541     static bool failsafeOverridden;
 542     static uint8_t lastFlightPosition;
 543     static uint32_t changeTimestamp;
 544     SystemAlarmsAlarmData alarms;
 545     batteryfailsafemode_t failsafeStatus;
 546     FlightStatusArmedOptions armed;
 547     FlightStatusArmedGet(&armed);
 548
 549     // reset the status and do not change anything when not armed
 550     if (armed != FLIGHTSTATUS_ARMED_ARMED) {
 551         lastFailsafeStatus = BATTERYFAILSAFE_NONE;
 552         failsafeOverridden = false;
 553         changeTimestamp    = PIOS_DELAY_GetRaw();
 554         lastFlightPosition = *position;
 555         return;
 556     }
 557
 558     SystemAlarmsAlarmGet(&alarms);
 559
 560     switch (alarms.Battery) {
 561     case SYSTEMALARMS_ALARM_WARNING:
 562         failsafeStatus = BATTERYFAILSAFE_WARNING;
 563         break;
 564     case SYSTEMALARMS_ALARM_CRITICAL:
 565         failsafeStatus = BATTERYFAILSAFE_CRITICAL;
 566         break;
 567     default:
 568         failsafeStatus = BATTERYFAILSAFE_NONE;
 569         break;
 570     }
 571     uint32_t debounceTimerms = PIOS_DELAY_DiffuS(changeTimestamp) / 1000;
 572
 573     if (failsafeStatus == lastFailsafeStatus) {
 574         changeTimestamp = PIOS_DELAY_GetRaw();
 575     } else if ((debounceTimerms < modeSettings->BatteryFailsafeDebounceTimer) || failsafeStatus < lastFailsafeStatus) {
 576         // do not change within the "grace" period and do not "downgrade" the failsafe mode
 577         failsafeStatus = lastFailsafeStatus;
 578     } else {
 579         // a higher failsafe status was met and grace period elapsed. Trigger the new state
 580         lastFailsafeStatus = failsafeStatus;
 581         lastFlightPosition = *position;
 582         failsafeOverridden = false;
 583     }
 584
 585     if ((failsafeStatus == BATTERYFAILSAFE_NONE) || failsafeOverridden) {
 586         return;
 587     }
 588
 589     // failsafe has been triggered. Check for override
 590     if (lastFlightPosition != *position) {
 591         // flag the override and reset the grace period
 592         failsafeOverridden = true;
 593         changeTimestamp    = PIOS_DELAY_GetRaw();
 594         return;
 595     }
 596
 597     switch (failsafeStatus) {
 598     case BATTERYFAILSAFE_CRITICAL:
 599         // if critical is not set, jump to the other case to use the warning setting.
 600         if (modeSettings->BatteryFailsafeSwitchPositions.Critical != -1) {
 601             *position = modeSettings->BatteryFailsafeSwitchPositions.Critical;
 602             break;
 603         }
 604     case BATTERYFAILSAFE_WARNING:
 605         if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) {
 606             *position = modeSettings->BatteryFailsafeSwitchPositions.Warning;
 607         }
 608         break;
 609     default:
 610         break;
 611     }
 612 }
 613
 614 #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
 615
 616 /**
 617  * Called whenever a critical configuration component changes
 618  */
 619 static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 620 {
 621     configuration_check();
 622 }
 623
 624 /**
 625  * Called whenever a critical configuration component changes
 626  */
 627 static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 628 {
 629     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 630 }
 631
 632
 633 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 634 /**
 635  * Check and set modes for gps assisted stablised flight modes
 636  */
 637 static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
 638 {
 639     StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 640
 641     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
 642     if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
 643         || position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
 644         return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 645     }
 646
 647     switch (FlightModeAssistMap[position]) {
 648     case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
 649         break;
 650     case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
 651     {
 652         // default to cruise control.
 653         FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
 654
 655         switch (flightMode) {
 656         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 657             thrustMode = modeSettings->Stabilization1Settings.Thrust;
 658             break;
 659         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 660             thrustMode = modeSettings->Stabilization2Settings.Thrust;
 661             break;
 662         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 663             thrustMode = modeSettings->Stabilization3Settings.Thrust;
 664             break;
 665         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 666             thrustMode = modeSettings->Stabilization4Settings.Thrust;
 667             break;
 668         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 669             thrustMode = modeSettings->Stabilization5Settings.Thrust;
 670             break;
 671         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 672             thrustMode = modeSettings->Stabilization6Settings.Thrust;
 673             break;
 674         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 675         case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 676             // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
 677             // is a more appropriate throttle mode.  "GPSAssist" adds braking
 678             // and a better throttle management to the standard Position Hold.
 679             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
 680             break;
 681         case FLIGHTSTATUS_FLIGHTMODE_LAND:
 682         case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
 683             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
 684             break;
 685
 686             // other modes will use cruisecontrol as default
 687         }
 688
 689
 690         switch (thrustMode) {
 691         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
 692         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
 693             // this is only for use with stabi mods with althold/vario.
 694             return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
 695
 696         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
 697         case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
 698         default:
 699             // this is the default for non stabi modes also
 700             return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
 701         }
 702     }
 703     break;
 704     }
 705
 706     // return isAssistedFlag;
 707     return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
 708 }
 709 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 710
 711 /**
 712  * @}
 713  * @}
 714  */