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