flight/modules/Receiver/receiver.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup ReceiverModule Manual Control Module
   6  * @brief Provide manual control or allow it alter flight mode.
   7  * @{
   8  *
   9  * Reads in the ManualControlCommand from receiver then
  10  * pass it to ManualControl
  11  *
  12  * @file       receiver.c
  13  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017.
  14  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  15  * @brief      Receiver module. Handles safety R/C link and flight mode.
  16  *
  17  * @see        The GNU Public License (GPL) Version 3
  18  *
  19  *****************************************************************************/
  20 /*
  21  * This program is free software; you can redistribute it and/or modify
  22  * it under the terms of the GNU General Public License as published by
  23  * the Free Software Foundation; either version 3 of the License, or
  24  * (at your option) any later version.
  25  *
  26  * This program is distributed in the hope that it will be useful, but
  27  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  28  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  29  * for more details.
  30  *
  31  * You should have received a copy of the GNU General Public License along
  32  * with this program; if not, write to the Free Software Foundation, Inc.,
  33  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  34  */
  35
  36 #include <openpilot.h>
  37 #include <accessorydesired.h>
  38 #include <manualcontrolsettings.h>
  39 #include <manualcontrolcommand.h>
  40 #include <receiveractivity.h>
  41 #include <receiverstatus.h>
  42 #include <flightstatus.h>
  43 #include <flighttelemetrystats.h>
  44 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  45 #include <stabilizationsettings.h>
  46 #include <vtolpathfollowersettings.h>
  47 #endif
  48 #include <flightmodesettings.h>
  49 #include <systemsettings.h>
  50 #include <taskinfo.h>
  51 #include <sanitycheck.h>
  52
  53
  54 #if defined(PIOS_INCLUDE_USB_RCTX)
  55 #include "pios_usb_rctx.h"
  56 #endif /* PIOS_INCLUDE_USB_RCTX */
  57
  58 // Private constants
  59 #if defined(PIOS_RECEIVER_STACK_SIZE)
  60 #define STACK_SIZE_BYTES                 PIOS_RECEIVER_STACK_SIZE
  61 #else
  62 #define STACK_SIZE_BYTES                 1152
  63 #endif
  64
  65 #define TASK_PRIORITY                    (tskIDLE_PRIORITY + 3) // 3 = flight control
  66 #define UPDATE_PERIOD_MS                 20
  67
  68 // safe band to allow a bit of calibration error or trim offset (in microseconds)
  69 #define CONNECTION_OFFSET                250
  70
  71 #define ASSISTEDCONTROL_DEADBAND_MINIMUM 2 // minimum value for a well behaved Tx, in percent.
  72
  73 // Private types
  74
  75 // Private variables
  76 static xTaskHandle taskHandle;
  77 static portTickType lastSysTime;
  78 static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
  79
  80 #ifdef USE_INPUT_LPF
  81 static portTickType lastSysTimeLPF;
  82 static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
  83 #endif
  84
  85 // Private functions
  86 static void receiverTask(void *parameters);
  87 static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
  88 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
  89 static bool validInputRange(int16_t min, int16_t max, uint16_t value);
  90 static void applyDeadband(float *value, uint8_t deadband);
  91 static void SettingsUpdatedCb(UAVObjEvent *ev);
  92
  93 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  94 static uint8_t isAssistedFlightMode(uint8_t position);
  95 #endif
  96
  97 #ifdef USE_INPUT_LPF
  98 static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT);
  99 #endif
 100
 101 #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 18 // Sbus max channel
 102 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE          15
 103 struct rcvr_activity_fsm {
 104     ManualControlSettingsChannelGroupsOptions group;
 105     uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
 106     uint8_t  sample_count;
 107     uint8_t  quality;
 108 };
 109 static struct rcvr_activity_fsm activity_fsm;
 110
 111 static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
 112 static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
 113 static void resetRcvrStatus(struct rcvr_activity_fsm *fsm);
 114 static bool updateRcvrStatus(
 115     struct rcvr_activity_fsm *fsm,
 116     ManualControlSettingsChannelGroupsOptions group,
 117     int8_t rssiValue);
 118
 119 #define assumptions \
 120     ( \
 121         ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \
 122         ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \
 123         ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \
 124         ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \
 125         ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM))
 126
 127 /**
 128  * Module starting
 129  */
 130 int32_t ReceiverStart()
 131 {
 132     // Start main task
 133     xTaskCreate(receiverTask, "Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 134     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle);
 135 #ifdef PIOS_INCLUDE_WDG
 136     PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
 137 #endif
 138     SettingsUpdatedCb(NULL);
 139
 140     return 0;
 141 }
 142
 143 /**
 144  * Module initialization
 145  */
 146 int32_t ReceiverInitialize()
 147 {
 148     /* Check the assumptions about uavobject enum's are correct */
 149     PIOS_STATIC_ASSERT(assumptions);
 150     AccessoryDesiredInitialize();
 151     ManualControlCommandInitialize();
 152     ReceiverActivityInitialize();
 153     ReceiverStatusInitialize();
 154     ManualControlSettingsInitialize();
 155 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 156     StabilizationSettingsInitialize();
 157     VtolPathFollowerSettingsInitialize();
 158     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 159 #endif
 160     SystemSettingsInitialize();
 161     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 162
 163
 164     return 0;
 165 }
 166 MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
 167
 168 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 169 {
 170     frameType = GetCurrentFrameType();
 171
 172 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 173     uint8_t TreatCustomCraftAs;
 174     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 175
 176
 177     if (frameType == FRAME_TYPE_CUSTOM) {
 178         switch (TreatCustomCraftAs) {
 179         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 180             frameType = FRAME_TYPE_FIXED_WING;
 181             break;
 182         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 183             frameType = FRAME_TYPE_MULTIROTOR;
 184             break;
 185         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 186             frameType = FRAME_TYPE_GROUND;
 187             break;
 188         }
 189     }
 190 #endif
 191 }
 192
 193
 194 /**
 195  * Module task
 196  */
 197 static void receiverTask(__attribute__((unused)) void *parameters)
 198 {
 199     ManualControlSettingsData settings;
 200     ManualControlCommandData cmd;
 201
 202     uint8_t armed;
 203     uint8_t disconnected_count = 0;
 204     uint8_t connected_count    = 0;
 205
 206     // For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
 207     // this includes not even registering it if not used
 208     AccessoryDesiredCreateInstance(); // Accessory1
 209     AccessoryDesiredCreateInstance(); // Accessory2
 210     AccessoryDesiredCreateInstance(); // Accessory3
 211
 212     // Rssi input
 213     AccessoryDesiredCreateInstance();
 214
 215     ManualControlCommandGet(&cmd);
 216
 217     /* Initialize the RcvrActivty FSM */
 218     portTickType lastActivityTime = xTaskGetTickCount();
 219     resetRcvrActivity(&activity_fsm);
 220     resetRcvrStatus(&activity_fsm);
 221
 222     // Main task loop
 223     lastSysTime = xTaskGetTickCount();
 224
 225     float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 };
 226     SystemSettingsThrustControlOptions thrustType;
 227
 228     while (1) {
 229         // Wait until next update
 230         vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
 231 #ifdef PIOS_INCLUDE_WDG
 232         PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
 233 #endif
 234
 235         int8_t rssiValue = -1;
 236
 237         // Read settings
 238         ManualControlSettingsGet(&settings);
 239         SystemSettingsThrustControlGet(&thrustType);
 240
 241         FlightStatusArmedGet(&armed);
 242         /* Update channel activity monitor */
 243         if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
 244             if (updateRcvrActivity(&activity_fsm)) {
 245                 /* Reset the aging timer because activity was detected */
 246                 lastActivityTime = lastSysTime;
 247             }
 248         }
 249
 250         /* Read signal quality from the group used for the throttle */
 251         (void)updateRcvrStatus(&activity_fsm,
 252                                settings.ChannelGroups.Throttle,
 253                                rssiValue);
 254
 255         if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
 256             resetRcvrActivity(&activity_fsm);
 257             resetRcvrStatus(&activity_fsm);
 258             lastActivityTime = lastSysTime;
 259         }
 260
 261         if (ManualControlCommandReadOnly()) {
 262             FlightTelemetryStatsData flightTelemStats;
 263             FlightTelemetryStatsGet(&flightTelemStats);
 264             if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
 265                 /* trying to fly via GCS and lost connection.  fall back to transmitter */
 266                 UAVObjMetadata metadata;
 267                 ManualControlCommandGetMetadata(&metadata);
 268                 UAVObjSetAccess(&metadata, ACCESS_READWRITE);
 269                 ManualControlCommandSetMetadata(&metadata);
 270             }
 271             AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 272             continue;
 273         }
 274
 275         bool valid_input_detected = true;
 276         bool valid_rssi_input     = false;
 277
 278         // Read channel values in us
 279         for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
 280             extern uint32_t pios_rcvr_group_map[];
 281
 282             if (ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 283                 cmd.Channel[n] = PIOS_RCVR_INVALID;
 284             } else {
 285                 cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
 286                                                     ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n]],
 287                                                 ManualControlSettingsChannelNumberToArray(settings.ChannelNumber)[n]);
 288             }
 289
 290             // If a channel has timed out this is not valid data and we shouldn't update anything
 291             // until we decide to go to failsafe
 292             if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
 293                 valid_input_detected = false;
 294             } else {
 295                 int16_t neutralValue = ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n];
 296
 297                 if (n == MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI) {
 298                     // Rssi neutral is ignored and set to min, for 0 - 100% output range
 299                     neutralValue     = ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n];
 300                     valid_rssi_input = true;
 301                 }
 302                 scaledChannel[n] = scaleChannel(cmd.Channel[n],
 303                                                 ManualControlSettingsChannelMaxToArray(settings.ChannelMax)[n],
 304                                                 ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n],
 305                                                 neutralValue);
 306             }
 307         }
 308
 309         if (valid_rssi_input) {
 310             rssiValue = (int8_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI] * 100);
 311         }
 312         /* Read signal quality from the group used for the throttle */
 313         (void)updateRcvrStatus(&activity_fsm,
 314                                settings.ChannelGroups.Throttle,
 315                                rssiValue);
 316
 317         // Sanity Check Throttle and Yaw
 318         if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 319             || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 320             ||
 321             // Check all channel mappings are valid
 322             cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
 323             || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
 324             ||
 325             // Check the driver exists
 326             cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
 327             || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
 328             ||
 329             // Check collective if required
 330             (thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && (
 331                  settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 332                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID
 333                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_NODRIVER))
 334             ||
 335             // Check the FlightModeNumber is valid
 336             settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
 337             ||
 338             // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
 339             ((settings.FlightModeNumber > 1) && (frameType != FRAME_TYPE_GROUND)
 340              && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 341                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
 342                  || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
 343             AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
 344             cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
 345             ManualControlCommandSet(&cmd);
 346
 347             continue;
 348         }
 349
 350
 351         if (frameType != FRAME_TYPE_GROUND) {
 352             // Sanity Check Pitch and Roll
 353             if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 354                 || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
 355                 ||
 356                 // Check all channel mappings are valid
 357                 cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
 358                 || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
 359                 ||
 360                 // Check the driver exists
 361                 cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
 362                 || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER) {
 363                 AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
 364                 cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
 365                 ManualControlCommandSet(&cmd);
 366
 367                 continue;
 368             }
 369         }
 370
 371         // decide if we have valid manual input or not
 372         valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
 373                                                 settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
 374                                 && validInputRange(settings.ChannelMin.Yaw,
 375                                                    settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]);
 376
 377         if (frameType != FRAME_TYPE_GROUND) {
 378             valid_input_detected &= validInputRange(settings.ChannelMin.Roll,
 379                                                     settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
 380                                     && validInputRange(settings.ChannelMin.Pitch,
 381                                                        settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
 382         }
 383
 384         if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 385             valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
 386                                                     settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]);
 387         }
 388         if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 389             valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0,
 390                                                     settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]);
 391         }
 392         if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 393             valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1,
 394                                                     settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]);
 395         }
 396         if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 397             valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2,
 398                                                     settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]);
 399         }
 400         if (settings.ChannelGroups.Accessory3 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 401             valid_input_detected &= validInputRange(settings.ChannelMin.Accessory3,
 402                                                     settings.ChannelMax.Accessory3, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY3]);
 403         }
 404         if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 405             valid_input_detected &= validInputRange(settings.ChannelMin.Rssi,
 406                                                     settings.ChannelMax.Rssi, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI]);
 407         }
 408
 409         // Implement hysteresis loop on connection status
 410         if (valid_input_detected && (++connected_count > 10)) {
 411             cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
 412             connected_count    = 0;
 413             disconnected_count = 0;
 414         } else if (!valid_input_detected && (++disconnected_count > 10)) {
 415             cmd.Connected      = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
 416             connected_count    = 0;
 417             disconnected_count = 0;
 418         }
 419
 420         if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
 421             if (frameType != FRAME_TYPE_GROUND) {
 422                 cmd.Throttle = settings.FailsafeChannel.Throttle;
 423             } else {
 424                 cmd.Throttle = 0.0f;
 425             }
 426             cmd.Roll  = settings.FailsafeChannel.Roll;
 427             cmd.Pitch = settings.FailsafeChannel.Pitch;
 428             cmd.Yaw   = settings.FailsafeChannel.Yaw;
 429             cmd.Collective = settings.FailsafeChannel.Collective;
 430             switch (thrustType) {
 431             case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 432                 cmd.Thrust = cmd.Throttle;
 433                 break;
 434             case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 435                 cmd.Thrust = cmd.Collective;
 436                 break;
 437             default:
 438                 break;
 439             }
 440             if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) {
 441                 cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition;
 442             }
 443             AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 444
 445             AccessoryDesiredData accessory;
 446             // Set Accessory 0
 447             if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 448                 accessory.AccessoryVal = settings.FailsafeChannel.Accessory0;
 449                 if (AccessoryDesiredInstSet(0, &accessory) != 0) {
 450                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 451                 }
 452             }
 453             // Set Accessory 1
 454             if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 455                 accessory.AccessoryVal = settings.FailsafeChannel.Accessory1;
 456                 if (AccessoryDesiredInstSet(1, &accessory) != 0) {
 457                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 458                 }
 459             }
 460             // Set Accessory 2
 461             if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 462                 accessory.AccessoryVal = settings.FailsafeChannel.Accessory2;
 463                 if (AccessoryDesiredInstSet(2, &accessory) != 0) {
 464                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 465                 }
 466             }
 467             // Set Accessory 3
 468             if (settings.ChannelGroups.Accessory3 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 469                 accessory.AccessoryVal = settings.FailsafeChannel.Accessory3;
 470                 if (AccessoryDesiredInstSet(3, &accessory) != 0) {
 471                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 472                 }
 473             }
 474         } else if (valid_input_detected) {
 475             AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER);
 476
 477             // Scale channels to -1 -> +1 range
 478             cmd.Roll     = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
 479             cmd.Pitch    = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
 480             cmd.Yaw      = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
 481             cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
 482             // Convert flightMode value into the switch position in the range [0..N-1]
 483             cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9;
 484             if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) {
 485                 cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1;
 486             }
 487
 488             uint8_t deadband_checked = settings.Deadband;
 489 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 490             // AssistedControl must have deadband set for pitch/roll hence
 491             // we default to a higher value for badly behaved TXs and also enforce a minimum value
 492             // for well behaved TXs
 493             uint8_t assistedMode = isAssistedFlightMode(cmd.FlightModeSwitchPosition);
 494             if (assistedMode) {
 495                 deadband_checked = settings.DeadbandAssistedControl;
 496                 if (deadband_checked < ASSISTEDCONTROL_DEADBAND_MINIMUM) {
 497                     deadband_checked = ASSISTEDCONTROL_DEADBAND_MINIMUM;
 498                 }
 499
 500                 // If user has set settings.Deadband to a higher value...we use that.
 501                 if (deadband_checked < settings.Deadband) {
 502                     deadband_checked = settings.Deadband;
 503                 }
 504             }
 505 #endif // PIOS_EXCLUDE_ADVANCED_FEATURES
 506
 507             // Apply deadband for Roll/Pitch/Yaw stick inputs
 508             if (deadband_checked > 0) {
 509                 applyDeadband(&cmd.Roll, deadband_checked);
 510                 applyDeadband(&cmd.Pitch, deadband_checked);
 511                 applyDeadband(&cmd.Yaw, deadband_checked);
 512                 if (frameType == FRAME_TYPE_GROUND) { // assumes reversible motors
 513                     applyDeadband(&cmd.Throttle, deadband_checked);
 514                 }
 515             }
 516 #ifdef USE_INPUT_LPF
 517             // Apply Low Pass Filter to input channels, time delta between calls in ms
 518             portTickType thisSysTime = xTaskGetTickCount();
 519             float dT = (thisSysTime > lastSysTimeLPF) ?
 520                        (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
 521                        (float)UPDATE_PERIOD_MS;
 522             lastSysTimeLPF = thisSysTime;
 523
 524             applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings.ResponseTime, deadband_checked, dT);
 525             applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings.ResponseTime, deadband_checked, dT);
 526             applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings.ResponseTime, deadband_checked, dT);
 527 #endif // USE_INPUT_LPF
 528             if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
 529                 && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
 530                 && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
 531                 cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
 532                 if (settings.Deadband > 0) {
 533                     applyDeadband(&cmd.Collective, settings.Deadband);
 534                 }
 535 #ifdef USE_INPUT_LPF
 536                 applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings.ResponseTime, settings.Deadband, dT);
 537 #endif // USE_INPUT_LPF
 538             }
 539
 540             switch (thrustType) {
 541             case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 542                 cmd.Thrust = cmd.Throttle;
 543                 break;
 544             case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 545                 cmd.Thrust = cmd.Collective;
 546                 break;
 547             default:
 548                 break;
 549             }
 550
 551             AccessoryDesiredData accessory;
 552             // Set Accessory 0
 553             if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 554                 accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
 555 #ifdef USE_INPUT_LPF
 556                 applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings.ResponseTime, settings.Deadband, dT);
 557 #endif
 558                 if (AccessoryDesiredInstSet(0, &accessory) != 0) {
 559                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 560                 }
 561             }
 562             // Set Accessory 1
 563             if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 564                 accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
 565 #ifdef USE_INPUT_LPF
 566                 applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings.ResponseTime, settings.Deadband, dT);
 567 #endif
 568                 if (AccessoryDesiredInstSet(1, &accessory) != 0) {
 569                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 570                 }
 571             }
 572             // Set Accessory 2
 573             if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 574                 accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
 575 #ifdef USE_INPUT_LPF
 576                 applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings.ResponseTime, settings.Deadband, dT);
 577 #endif
 578
 579                 if (AccessoryDesiredInstSet(2, &accessory) != 0) {
 580                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 581                 }
 582             }
 583             // Set Accessory 3
 584             if (settings.ChannelGroups.Accessory3 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 585                 accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY3];
 586 #ifdef USE_INPUT_LPF
 587                 applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY3, &settings.ResponseTime, settings.Deadband, dT);
 588 #endif
 589
 590                 if (AccessoryDesiredInstSet(3, &accessory) != 0) {
 591                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 592                 }
 593             }
 594             // Set Rssi input
 595             if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 596                 accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI];
 597 #ifdef USE_INPUT_LPF
 598                 // Allow some Rssi filtering without deadband
 599                 applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_RSSI, &settings.ResponseTime, 0.0f, dT);
 600 #endif
 601
 602                 if (AccessoryDesiredInstSet(4, &accessory) != 0) {
 603                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
 604                 }
 605             }
 606         }
 607
 608         // Update cmd object
 609         ManualControlCommandSet(&cmd);
 610
 611
 612 #if defined(PIOS_INCLUDE_USB_RCTX)
 613         if (pios_usb_rctx_id) {
 614             PIOS_USB_RCTX_Update(pios_usb_rctx_id,
 615                                  cmd.Channel,
 616                                  ManualControlSettingsChannelMinToArray(settings.ChannelMin),
 617                                  ManualControlSettingsChannelMaxToArray(settings.ChannelMax),
 618                                  NELEMENTS(cmd.Channel));
 619         }
 620 #endif /* PIOS_INCLUDE_USB_RCTX */
 621     }
 622 }
 623
 624 static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
 625 {
 626     ReceiverActivityData data;
 627     bool updated = false;
 628
 629     /* Clear all channel activity flags */
 630     ReceiverActivityGet(&data);
 631     if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
 632         data.ActiveGroup   = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
 633         data.ActiveChannel = 255;
 634         updated = true;
 635     }
 636     if (updated) {
 637         ReceiverActivitySet(&data);
 638     }
 639
 640     /* Reset the FSM state */
 641     fsm->group = 0;
 642     fsm->sample_count = 0;
 643 }
 644
 645 static void resetRcvrStatus(struct rcvr_activity_fsm *fsm)
 646 {
 647     /* Reset the state */
 648     fsm->quality = 0;
 649 }
 650
 651 static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
 652 {
 653     for (uint8_t channel = 1; channel <= max_channels; channel++) {
 654         // Subtract 1 because channels are 1 indexed
 655         samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
 656     }
 657 }
 658
 659 static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
 660 {
 661     bool activity_updated = false;
 662
 663     ManualControlSettingsChannelGroupsData channelGroups;
 664     ManualControlSettingsChannelNumberData channelNumber;
 665
 666     ManualControlSettingsChannelGroupsGet(&channelGroups);
 667     ManualControlSettingsChannelNumberGet(&channelNumber);
 668
 669     /* Compare the current value to the previous sampled value */
 670     for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
 671         uint16_t delta;
 672         uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
 673         uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
 674         if (curr > prev) {
 675             delta = curr - prev;
 676         } else {
 677             delta = prev - curr;
 678         }
 679
 680         // Ignore activity from this Group/Channel because already used/set for Rssi input
 681         // Without that, the ReceiverActivity will be saturated just with Rssi value activity.
 682         if (channelGroups.Rssi == fsm->group && channelNumber.Rssi == channel) {
 683             delta = 0;
 684         }
 685
 686         if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
 687             /* Mark this channel as active */
 688             ReceiverActivityActiveGroupOptions group;
 689
 690             /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
 691             switch (fsm->group) {
 692             case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
 693                 group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
 694                 break;
 695             case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
 696                 group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
 697                 break;
 698             case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
 699                 group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
 700                 break;
 701             case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
 702                 group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
 703                 break;
 704             case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT:
 705                 group = RECEIVERACTIVITY_ACTIVEGROUP_DSMRCVRPORT;
 706                 break;
 707             case MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS:
 708                 group = RECEIVERACTIVITY_ACTIVEGROUP_EXBUS;
 709                 break;
 710             case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
 711                 group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
 712                 break;
 713             case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT:
 714                 group = RECEIVERACTIVITY_ACTIVEGROUP_HOTT;
 715                 break;
 716             case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL:
 717                 group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL;
 718                 break;
 719             case MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS:
 720                 group = RECEIVERACTIVITY_ACTIVEGROUP_IBUS;
 721                 break;
 722             case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
 723                 group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
 724                 break;
 725             case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
 726                 group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
 727                 break;
 728             case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS:
 729                 group = RECEIVERACTIVITY_ACTIVEGROUP_OPENLRS;
 730                 break;
 731             default:
 732                 PIOS_Assert(0);
 733                 break;
 734             }
 735
 736             ReceiverActivityActiveGroupSet((uint8_t *)&group);
 737             ReceiverActivityActiveChannelSet(&channel);
 738             activity_updated = true;
 739         }
 740     }
 741     return activity_updated;
 742 }
 743
 744 static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
 745 {
 746     bool activity_updated = false;
 747
 748     if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 749         /* We're out of range, reset things */
 750         resetRcvrActivity(fsm);
 751         resetRcvrStatus(fsm);
 752     }
 753
 754     extern uint32_t pios_rcvr_group_map[];
 755     if (!pios_rcvr_group_map[fsm->group]) {
 756         /* Unbound group, skip it */
 757         goto group_completed;
 758     }
 759
 760     if (fsm->sample_count == 0) {
 761         /* Take a sample of each channel in this group */
 762         updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
 763         fsm->sample_count++;
 764         return false;
 765     }
 766
 767     /* Compare with previous sample */
 768     activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
 769
 770 group_completed:
 771     /* Reset the sample counter */
 772     fsm->sample_count = 0;
 773
 774     /* Find the next active group, but limit search so we can't loop forever here */
 775     for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
 776         /* Move to the next group */
 777         fsm->group++;
 778         if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
 779             /* Wrap back to the first group */
 780             fsm->group = 0;
 781         }
 782         if (pios_rcvr_group_map[fsm->group]) {
 783             /*
 784              * Found an active group, take a sample here to avoid an
 785              * extra 20ms delay in the main thread so we can speed up
 786              * this algorithm.
 787              */
 788             updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
 789             fsm->sample_count++;
 790             break;
 791         }
 792     }
 793
 794     return activity_updated;
 795 }
 796
 797 /**
 798  * Read signal quality from the specified group or use Rssi input value if any
 799  */
 800 static bool updateRcvrStatus(
 801     struct rcvr_activity_fsm *fsm,
 802     ManualControlSettingsChannelGroupsOptions group, int8_t rssiValue)
 803 {
 804     extern uint32_t pios_rcvr_group_map[];
 805     bool activity_updated = false;
 806     int8_t quality;
 807
 808     quality = (rssiValue > 0) ? rssiValue : PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
 809
 810     /* If no driver is detected or any other error then return */
 811     if (quality < 0) {
 812         return activity_updated;
 813     }
 814
 815     /* Compare with previous sample */
 816     if (quality != fsm->quality) {
 817         fsm->quality     = quality;
 818         ReceiverStatusQualitySet(&fsm->quality);
 819         activity_updated = true;
 820     }
 821
 822     return activity_updated;
 823 }
 824
 825 /**
 826  * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
 827  */
 828 static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
 829 {
 830     float valueScaled;
 831
 832     // Scale
 833     if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
 834         if (max != neutral) {
 835             valueScaled = (float)(value - neutral) / (float)(max - neutral);
 836         } else {
 837             valueScaled = 0;
 838         }
 839     } else {
 840         if (min != neutral) {
 841             valueScaled = (float)(value - neutral) / (float)(neutral - min);
 842         } else {
 843             valueScaled = 0;
 844         }
 845     }
 846
 847     // Bound
 848     if (valueScaled > 1.0f) {
 849         valueScaled = 1.0f;
 850     } else if (valueScaled < -1.0f) {
 851         valueScaled = -1.0f;
 852     }
 853
 854     return valueScaled;
 855 }
 856
 857 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
 858 {
 859     return (end_time - start_time) * portTICK_RATE_MS;
 860 }
 861
 862
 863 /**
 864  * @brief Determine if the manual input value is within acceptable limits
 865  * @returns return TRUE if so, otherwise return FALSE
 866  */
 867 bool validInputRange(int16_t min, int16_t max, uint16_t value)
 868 {
 869     if (min > max) {
 870         int16_t tmp = min;
 871         min = max;
 872         max = tmp;
 873     }
 874     return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
 875 }
 876
 877 /**
 878  * @brief Apply deadband to Roll/Pitch/Yaw channels
 879  */
 880 static void applyDeadband(float *value, uint8_t deadband)
 881 {
 882     float floatDeadband = ((float)deadband) * 0.01f;
 883
 884     if (fabsf(*value) < floatDeadband) {
 885         *value = 0.0f;
 886     } else if (*value > 0.0f) {
 887         *value = (*value - floatDeadband) / (1.0f - floatDeadband);
 888     } else {
 889         *value = (*value + floatDeadband) / (1.0f - floatDeadband);
 890     }
 891 }
 892
 893 #ifdef USE_INPUT_LPF
 894 /**
 895  * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
 896  */
 897 static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, uint8_t deadband, float dT)
 898 {
 899     float rt = (float)ManualControlSettingsResponseTimeToArray((*responseTime))[channel];
 900
 901     if (rt > 0.0f) {
 902         inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
 903
 904         float floatDeadband = ((float)deadband) * 0.01f;
 905         // avoid a long tail of non-zero data. if we have deadband, once the filtered result reduces to 1/10th
 906         // of deadband revert to 0.  We downstream rely on this to know when sticks are centered.
 907         if (floatDeadband > 0.0f && fabsf(inputFiltered[channel]) < floatDeadband * 0.1f) {
 908             inputFiltered[channel] = 0.0f;
 909         }
 910         *value = inputFiltered[channel];
 911     }
 912 }
 913 #endif // USE_INPUT_LPF
 914
 915 /**
 916  * Check and set modes for gps assisted stablised flight modes
 917  */
 918 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 919 static uint8_t isAssistedFlightMode(uint8_t position)
 920 {
 921     uint8_t isAssistedFlag = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
 922     uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 923
 924     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
 925
 926     if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
 927         isAssistedFlag = FlightModeAssistMap[position];
 928     }
 929
 930     return isAssistedFlag;
 931 }
 932 #endif
 933
 934 /**
 935  * @}
 936  * @}
 937  */