flight/modules/Actuator/actuator.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup ActuatorModule Actuator Module
   6  * @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type.
   7  * This is where all the mixing of channels is computed.
   8  * @{
   9  *
  10  * @file       actuator.c
  11  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
  12  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  13  * @brief      Actuator module. Drives the actuators (servos, motors etc).
  14  *
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  *****************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34
  35 #include <openpilot.h>
  36
  37 #include "accessorydesired.h"
  38 #include "actuator.h"
  39 #include "actuatorsettings.h"
  40 #include "systemsettings.h"
  41 #include "actuatordesired.h"
  42 #include "actuatorcommand.h"
  43 #include "flightstatus.h"
  44 #include <flightmodesettings.h>
  45 #include "mixersettings.h"
  46 #include "mixerstatus.h"
  47 #include "cameradesired.h"
  48 #include "manualcontrolcommand.h"
  49 #include "taskinfo.h"
  50 #include <systemsettings.h>
  51 #include <sanitycheck.h>
  52 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
  53 #include <vtolpathfollowersettings.h>
  54 #endif
  55 #undef PIOS_INCLUDE_INSTRUMENTATION
  56 #ifdef PIOS_INCLUDE_INSTRUMENTATION
  57 #include <pios_instrumentation.h>
  58 static int8_t counter;
  59 // Counter 0xAC700001 total Actuator body execution time(excluding queue waits etc).
  60 #endif
  61
  62 // Private constants
  63 #define MAX_QUEUE_SIZE                  2
  64
  65 #if defined(PIOS_ACTUATOR_STACK_SIZE)
  66 #define STACK_SIZE_BYTES                PIOS_ACTUATOR_STACK_SIZE
  67 #else
  68 #define STACK_SIZE_BYTES                1312
  69 #endif
  70
  71 #define TASK_PRIORITY                   (tskIDLE_PRIORITY + 4) // device driver
  72 #define FAILSAFE_TIMEOUT_MS             100
  73 #define MAX_MIX_ACTUATORS               ACTUATORCOMMAND_CHANNEL_NUMELEM
  74
  75 #define CAMERA_BOOT_DELAY_MS            7000
  76
  77 #define ACTUATOR_ONESHOT125_CLOCK       2000000
  78 #define ACTUATOR_ONESHOT125_PULSE_SCALE 4
  79 #define ACTUATOR_PWM_CLOCK              1000000
  80 // Private types
  81
  82
  83 // Private variables
  84 static xQueueHandle queue;
  85 static xTaskHandle taskHandle;
  86 static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
  87 static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
  88
  89 static uint8_t pinsMode[MAX_MIX_ACTUATORS];
  90 // used to inform the actuator thread that actuator update rate is changed
  91 static ActuatorSettingsData actuatorSettings;
  92 static bool spinWhileArmed;
  93
  94 // used to inform the actuator thread that mixer settings are changed
  95 static MixerSettingsData mixerSettings;
  96 static int mixer_settings_count = 2;
  97
  98 // Private functions
  99 static void actuatorTask(void *parameters);
 100 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
 101 static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired);
 102 static void setFailsafe();
 103 static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor);
 104 static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor);
 105 static bool set_channel(uint8_t mixer_channel, uint16_t value);
 106 static void actuator_update_rate_if_changed(bool force_update);
 107 static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
 108 static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
 109 static void SettingsUpdatedCb(UAVObjEvent *ev);
 110 float ProcessMixer(const int index, const float curve1, const float curve2,
 111                    ActuatorDesiredData *desired,
 112                    bool multirotor, bool fixedwing);
 113
 114 // this structure is equivalent to the UAVObjects for one mixer.
 115 typedef struct {
 116     uint8_t type;
 117     int8_t  matrix[5];
 118 } __attribute__((packed)) Mixer_t;
 119
 120 /**
 121  * @brief Module initialization
 122  * @return 0
 123  */
 124 int32_t ActuatorStart()
 125 {
 126     // Start main task
 127     xTaskCreate(actuatorTask, "Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 128     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle);
 129 #ifdef PIOS_INCLUDE_WDG
 130     PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
 131 #endif
 132     SettingsUpdatedCb(NULL);
 133     MixerSettingsUpdatedCb(NULL);
 134     ActuatorSettingsUpdatedCb(NULL);
 135     return 0;
 136 }
 137
 138 /**
 139  * @brief Module initialization
 140  * @return 0
 141  */
 142 int32_t ActuatorInitialize()
 143 {
 144     // Register for notification of changes to ActuatorSettings
 145     ActuatorSettingsInitialize();
 146     ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
 147
 148     // Register for notification of changes to MixerSettings
 149     MixerSettingsInitialize();
 150     MixerSettingsConnectCallback(MixerSettingsUpdatedCb);
 151
 152     // Listen for ActuatorDesired updates (Primary input to this module)
 153     ActuatorDesiredInitialize();
 154     queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 155     ActuatorDesiredConnectQueue(queue);
 156
 157     // Register AccessoryDesired (Secondary input to this module)
 158     AccessoryDesiredInitialize();
 159
 160     // Primary output of this module
 161     ActuatorCommandInitialize();
 162
 163 #ifdef DIAG_MIXERSTATUS
 164     // UAVO only used for inspecting the internal status of the mixer during debug
 165     MixerStatusInitialize();
 166 #endif
 167
 168 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
 169     VtolPathFollowerSettingsInitialize();
 170     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 171 #endif
 172     SystemSettingsInitialize();
 173     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 174
 175     return 0;
 176 }
 177 MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
 178
 179 /**
 180  * @brief Main Actuator module task
 181  *
 182  * Universal matrix based mixer for VTOL, helis and fixed wing.
 183  * Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.
 184  *
 185  * Because of how the Throttle ranges from 0 to 1, the motors should too!
 186  *
 187  * Note this code depends on the UAVObjects for the mixers being all being the same
 188  * and in sequence. If you change the object definition, make sure you check the code!
 189  *
 190  * @return -1 if error, 0 if success
 191  */
 192 static void actuatorTask(__attribute__((unused)) void *parameters)
 193 {
 194     UAVObjEvent ev;
 195     portTickType lastSysTime;
 196     portTickType thisSysTime;
 197     uint32_t dTMilliseconds;
 198
 199     ActuatorCommandData command;
 200     ActuatorDesiredData desired;
 201     MixerStatusData mixerStatus;
 202     FlightModeSettingsData settings;
 203     FlightStatusData flightStatus;
 204     float throttleDesired;
 205     float collectiveDesired;
 206
 207 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 208     counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
 209 #endif
 210     /* Read initial values of ActuatorSettings */
 211
 212     ActuatorSettingsGet(&actuatorSettings);
 213
 214     /* Read initial values of MixerSettings */
 215     MixerSettingsGet(&mixerSettings);
 216
 217     /* Force an initial configuration of the actuator update rates */
 218     actuator_update_rate_if_changed(true);
 219
 220     // Go to the neutral (failsafe) values until an ActuatorDesired update is received
 221     setFailsafe();
 222
 223     // Main task loop
 224     lastSysTime = xTaskGetTickCount();
 225     while (1) {
 226 #ifdef PIOS_INCLUDE_WDG
 227         PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
 228 #endif
 229
 230         // Wait until the ActuatorDesired object is updated
 231         uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS);
 232 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 233         PIOS_Instrumentation_TimeStart(counter);
 234 #endif
 235
 236         if (rc != pdTRUE) {
 237             /* Update of ActuatorDesired timed out.  Go to failsafe */
 238             setFailsafe();
 239             continue;
 240         }
 241
 242         // Check how long since last update
 243         thisSysTime    = xTaskGetTickCount();
 244         dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS;
 245         lastSysTime    = thisSysTime;
 246
 247         FlightStatusGet(&flightStatus);
 248         FlightModeSettingsGet(&settings);
 249         ActuatorDesiredGet(&desired);
 250         ActuatorCommandGet(&command);
 251
 252         // read in throttle and collective -demultiplex thrust
 253         switch (thrustType) {
 254         case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 255             throttleDesired = desired.Thrust;
 256             ManualControlCommandCollectiveGet(&collectiveDesired);
 257             break;
 258         case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 259             ManualControlCommandThrottleGet(&throttleDesired);
 260             collectiveDesired = desired.Thrust;
 261             break;
 262         default:
 263             ManualControlCommandThrottleGet(&throttleDesired);
 264             ManualControlCommandCollectiveGet(&collectiveDesired);
 265         }
 266
 267         bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
 268         bool activeThrottle   = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
 269         bool positiveThrottle = (throttleDesired > 0.00f);
 270         bool multirotor  = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor.
 271         bool fixedwing   = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING); // check if frame is a fixedwing.
 272         bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED;
 273         bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE;
 274
 275         if (alwaysArmed) {
 276             AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous.
 277         }
 278         // safety settings
 279         if (!armed) {
 280             throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check
 281         }
 282
 283         if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
 284             // throttleDesired should never be 0 or go below 0.
 285             // force set all other controls to zero if throttle is cut (previously set in Stabilization)
 286             // todo: can probably remove this
 287             if (!(multirotor && AlwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed
 288                 if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
 289                     desired.Roll = 0.00f;
 290                 }
 291                 if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
 292                     desired.Pitch = 0.00f;
 293                 }
 294                 if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
 295                     desired.Yaw = 0.00f;
 296                 }
 297             }
 298         }
 299
 300 #ifdef DIAG_MIXERSTATUS
 301         MixerStatusGet(&mixerStatus);
 302 #endif
 303
 304         if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
 305             setFailsafe();
 306             continue;
 307         }
 308
 309         AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
 310
 311         float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors.
 312         float curve2 = 0.0f;
 313
 314         // Interpolate curve 1 from throttleDesired as input.
 315         // assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values
 316         // map differently
 317         curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor);
 318
 319         // The source for the secondary curve is selectable
 320         AccessoryDesiredData accessory;
 321         uint8_t curve2Source = mixerSettings.Curve2Source;
 322         switch (curve2Source) {
 323         case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
 324             // assume reversible motor/mixer initially
 325             curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 326             break;
 327         case MIXERSETTINGS_CURVE2SOURCE_ROLL:
 328             // Throttle curve contribution the same for +ve vs -ve roll
 329             if (multirotor) {
 330                 curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 331             } else {
 332                 curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 333             }
 334             break;
 335         case MIXERSETTINGS_CURVE2SOURCE_PITCH:
 336             // Throttle curve contribution the same for +ve vs -ve pitch
 337             if (multirotor) {
 338                 curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2,
 339                                                          MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 340             } else {
 341                 curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
 342                                                      MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 343             }
 344             break;
 345         case MIXERSETTINGS_CURVE2SOURCE_YAW:
 346             // Throttle curve contribution the same for +ve vs -ve yaw
 347             if (multirotor) {
 348                 curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 349             } else {
 350                 curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 351             }
 352             break;
 353         case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
 354             // assume reversible motor/mixer initially
 355             curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
 356                                                      MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 357             break;
 358         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
 359         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
 360         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2:
 361         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3:
 362         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
 363         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
 364             if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
 365                 // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
 366                 curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
 367             } else {
 368                 curve2 = 0.0f;
 369             }
 370             break;
 371         default:
 372             curve2 = 0.0f;
 373             break;
 374         }
 375
 376         float *status   = (float *)&mixerStatus; // access status objects as an array of floats
 377         Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
 378         float maxMotor  = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f
 379         float minMotor  = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f
 380
 381         for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
 382             // During boot all camera actuators should be completely disabled (PWM pulse = 0).
 383             // command.Channel[i] is reused below as a channel PWM activity flag:
 384             // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later.
 385             // Setting it to 1 by default means "Rescale this channel and enable PWM on its output".
 386             command.Channel[ct] = 1;
 387
 388             uint8_t mixer_type = mixers[ct].type;
 389
 390             if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
 391                 // Set to minimum if disabled.  This is not the same as saying PWM pulse = 0 us
 392                 status[ct] = -1;
 393                 continue;
 394             }
 395
 396             if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
 397                 float nonreversible_curve1 = curve1;
 398                 float nonreversible_curve2 = curve2;
 399                 if (nonreversible_curve1 < 0.0f) {
 400                     nonreversible_curve1 = 0.0f;
 401                 }
 402                 if (nonreversible_curve2 < 0.0f) {
 403                     if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks.
 404                         nonreversible_curve2 = 0.0f;
 405                     }
 406                 }
 407                 status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, multirotor, fixedwing);
 408                 // If not armed or motors aren't meant to spin all the time
 409                 if (!armed ||
 410                     (!spinWhileArmed && !positiveThrottle)) {
 411                     status[ct] = -1; // force min throttle
 412                 }
 413                 // If armed meant to keep spinning,
 414                 else if ((spinWhileArmed && !positiveThrottle) ||
 415                          (status[ct] < 0)) {
 416                     if (!multirotor) {
 417                         status[ct] = 0;
 418                         // allow throttle values lower than 0 if multirotor.
 419                         // Values will be scaled to 0 if they need to be in the scaleMotor function
 420                     }
 421                 }
 422             } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
 423                 status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing);
 424                 // Reversable Motors are like Motors but go to neutral instead of minimum
 425                 // If not armed or motor is inactive - no "spinwhilearmed" for this engine type
 426                 if (!armed || !activeThrottle) {
 427                     status[ct] = 0; // force neutral throttle
 428                 }
 429             } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
 430                 status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing);
 431             } else {
 432                 status[ct] = -1;
 433
 434                 // If an accessory channel is selected for direct bypass mode
 435                 // In this configuration the accessory channel is scaled and mapped
 436                 // directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
 437                 // these also will not be updated in failsafe mode.  I'm not sure what
 438                 // the correct behavior is since it seems domain specific.  I don't love
 439                 // this code
 440                 if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
 441                     (mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
 442                     if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
 443                         status[ct] = accessory.AccessoryVal;
 444                     } else {
 445                         status[ct] = -1;
 446                     }
 447                 }
 448
 449                 if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
 450                     (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
 451                     CameraDesiredData cameraDesired;
 452                     if (CameraDesiredGet(&cameraDesired) == 0) {
 453                         switch (mixer_type) {
 454                         case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
 455                             status[ct] = cameraDesired.RollOrServo1;
 456                             break;
 457                         case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
 458                             status[ct] = cameraDesired.PitchOrServo2;
 459                             break;
 460                         case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
 461                             status[ct] = cameraDesired.Yaw;
 462                             break;
 463                         default:
 464                             break;
 465                         }
 466                     } else {
 467                         status[ct] = -1;
 468                     }
 469
 470                     // Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
 471                     if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
 472                         command.Channel[ct] = 0;
 473                     }
 474                 }
 475             }
 476
 477             // If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.
 478             // For use in function scaleMotor
 479             if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
 480                 if (maxMotor < status[ct]) {
 481                     maxMotor = status[ct];
 482                 }
 483                 if (minMotor > status[ct]) {
 484                     minMotor = status[ct];
 485                 }
 486             }
 487         }
 488
 489         // Set real actuator output values scaling them from mixers. All channels
 490         // will be set except explicitly disabled (which will have PWM pulse = 0).
 491         for (int i = 0; i < MAX_MIX_ACTUATORS; i++) {
 492             if (command.Channel[i]) {
 493                 if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors
 494                     command.Channel[i] = scaleMotor(status[i],
 495                                                     actuatorSettings.ChannelMax[i],
 496                                                     actuatorSettings.ChannelMin[i],
 497                                                     actuatorSettings.ChannelNeutral[i],
 498                                                     maxMotor,
 499                                                     minMotor,
 500                                                     armed,
 501                                                     AlwaysStabilizeWhenArmed,
 502                                                     throttleDesired);
 503                 } else { // else we scale the channel
 504                     command.Channel[i] = scaleChannel(status[i],
 505                                                       actuatorSettings.ChannelMax[i],
 506                                                       actuatorSettings.ChannelMin[i],
 507                                                       actuatorSettings.ChannelNeutral[i]);
 508                 }
 509             }
 510         }
 511
 512         // Store update time
 513         command.UpdateTime = dTMilliseconds;
 514         if (command.UpdateTime > command.MaxUpdateTime) {
 515             command.MaxUpdateTime = command.UpdateTime;
 516         }
 517         // Update output object
 518         ActuatorCommandSet(&command);
 519         // Update in case read only (eg. during servo configuration)
 520         ActuatorCommandGet(&command);
 521
 522 #ifdef DIAG_MIXERSTATUS
 523         MixerStatusSet(&mixerStatus);
 524 #endif
 525
 526
 527         // Update servo outputs
 528         bool success = true;
 529
 530         for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
 531             success &= set_channel(n, command.Channel[n]);
 532         }
 533
 534         PIOS_Servo_Update();
 535
 536         if (!success) {
 537             command.NumFailedUpdates++;
 538             ActuatorCommandSet(&command);
 539             AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
 540         }
 541 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 542         PIOS_Instrumentation_TimeEnd(counter);
 543 #endif
 544     }
 545 }
 546
 547
 548 /**
 549  * Process mixing for one actuator
 550  */
 551 float ProcessMixer(const int index, const float curve1, const float curve2,
 552                    ActuatorDesiredData *desired, bool multirotor, bool fixedwing)
 553 {
 554
 555     const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
 556     const Mixer_t *mixer  = &mixers[index];
 557     float differential    = 1.0f;
 558
 559     // Apply differential only for fixedwing and Roll servos
 560     if (fixedwing && (mixerSettings.FirstRollServo > 0) &&
 561         (mixer->type == MIXERSETTINGS_MIXER1TYPE_SERVO) &&
 562         (mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] != 0)) {
 563         // Positive differential
 564         if (mixerSettings.RollDifferential > 0) {
 565             // Check for first Roll servo (should be left aileron or elevon) and Roll desired (positive/negative)
 566             if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f))
 567                 || ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f))) {
 568                 differential -= (mixerSettings.RollDifferential * 0.01f);
 569             }
 570         } else if (mixerSettings.RollDifferential < 0) {
 571             if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f))
 572                 || ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f))) {
 573                 differential -= (-mixerSettings.RollDifferential * 0.01f);
 574             }
 575         }
 576     }
 577
 578     float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
 579                     (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) +
 580                     (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll * differential) +
 581                     (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) +
 582                     (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f;
 583
 584     if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
 585         if (!multirotor) { // we allow negative throttle with a multirotor
 586             if (result < 0.0f) { // zero throttle
 587                 result = 0.0f;
 588             }
 589         }
 590     }
 591
 592     return result;
 593 }
 594
 595
 596 /**
 597  * Interpolate a throttle curve
 598  * Full range input (-1 to 1) for yaw, roll, pitch
 599  * Output range (-1 to 1) reversible motor/throttle curve
 600  *
 601  * Input of -1 -> -lookup(1)
 602  * Input of 0  ->  lookup(0)
 603  * Input of 1  ->  lookup(1)
 604  */
 605 static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor)
 606 {
 607     float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor);
 608
 609     if (input < 0.0f) {
 610         return -unsigned_value;
 611     } else {
 612         return unsigned_value;
 613     }
 614 }
 615
 616 /**
 617  * Interpolate a throttle curve
 618  * Full range input (-1 to 1) for yaw, roll, pitch
 619  * Output range (0 to 1) non-reversible motor/throttle curve
 620  *
 621  * Input of -1 -> lookup(1)
 622  * Input of 0  -> lookup(0)
 623  * Input of 1  -> lookup(1)
 624  */
 625 static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor)
 626 {
 627     float abs_input = fabsf(input);
 628     float scale     = abs_input * (float)(elements - 1);
 629     int idx1 = scale;
 630
 631     scale -= (float)idx1; // remainder
 632     if (curve[0] < -1) {
 633         return abs_input;
 634     }
 635     int idx2 = idx1 + 1;
 636     if (idx2 >= elements) {
 637         idx2 = elements - 1; // clamp to highest entry in table
 638         if (idx1 >= elements) {
 639             if (multirotor) {
 640                 // if multirotor frame we can return throttle values higher than 100%.
 641                 // Since the we don't have elements in the curve higher than 100% we return
 642                 // the last element multiplied by the throttle float
 643                 if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance)
 644                     return curve[idx2] * input;
 645                 } else {
 646                     return curve[idx2] * 2.0f; // return 200% of max value in table
 647                 }
 648             }
 649             idx1 = elements - 1;
 650         }
 651     }
 652
 653     float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
 654     return unsigned_value;
 655 }
 656
 657
 658 /**
 659  * Convert channel from -1/+1 to servo pulse duration in microseconds
 660  */
 661 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral)
 662 {
 663     int16_t valueScaled;
 664
 665     // Scale
 666     if (value >= 0.0f) {
 667         valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral;
 668     } else {
 669         valueScaled = (int16_t)(value * ((float)(neutral - min))) + neutral;
 670     }
 671
 672     if (max > min) {
 673         if (valueScaled > max) {
 674             valueScaled = max;
 675         }
 676         if (valueScaled < min) {
 677             valueScaled = min;
 678         }
 679     } else {
 680         if (valueScaled < max) {
 681             valueScaled = max;
 682         }
 683         if (valueScaled > min) {
 684             valueScaled = min;
 685         }
 686     }
 687
 688     return valueScaled;
 689 }
 690
 691 /**
 692  * Constrain motor values to keep any one motor value from going too far out of range of another motor
 693  */
 694 static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired)
 695 {
 696     int16_t valueScaled;
 697     int16_t maxMotorScaled;
 698     int16_t minMotorScaled;
 699     int16_t diff;
 700
 701     // Scale
 702     valueScaled    = (int16_t)(value * ((float)(max - neutral))) + neutral;
 703     maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral;
 704     minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral;
 705
 706
 707     if (max > min) {
 708         diff = max - maxMotorScaled; // difference between max allowed and actual max motor
 709         if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value
 710             valueScaled += diff;
 711         }
 712         diff = neutral - minMotorScaled; // difference between min allowed and actual min motor
 713         if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value
 714             valueScaled += diff;
 715         }
 716         // todo: make this flow easier to understand
 717         if (valueScaled > max) {
 718             valueScaled = max; // clamp to max value only after scaling is done.
 719         }
 720
 721         if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) {
 722             valueScaled = neutral; // clamp to neutral value only after scaling is done.
 723         } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) {
 724             valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below
 725         } else if (valueScaled < neutral) {
 726             valueScaled = min; // clamp to min value only after scaling is done.
 727         }
 728     } else {
 729         // not sure what to do about reversed polarity right now. Why would anyone do this?
 730         if (valueScaled < max) {
 731             valueScaled = max; // clamp to max value only after scaling is done.
 732         }
 733         if (valueScaled > min) {
 734             valueScaled = min; // clamp to min value only after scaling is done.
 735         }
 736     }
 737
 738     // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max.
 739     // NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option
 740     // This prevents motors startup sync issues causing possible ESC failures.
 741
 742     // safety checks
 743     if (!armed) {
 744         // if not armed return min EVERYTIME!
 745         valueScaled = min;
 746     } else if (!AlwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) {
 747         // all motors idle is AlwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed
 748         // stabilize when armed?
 749         valueScaled = neutral;
 750     } else if (!spinWhileArmed && (throttleDesired <= 0.0f)) {
 751         // soft disarm
 752         valueScaled = min;
 753     }
 754
 755     return valueScaled;
 756 }
 757
 758 /**
 759  * Set actuator output to the neutral values (failsafe)
 760  */
 761 static void setFailsafe()
 762 {
 763     /* grab only the parts that we are going to use */
 764     int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
 765
 766     ActuatorCommandChannelGet(Channel);
 767
 768     const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
 769
 770     // Reset ActuatorCommand to safe values
 771     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
 772         if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
 773             Channel[n] = actuatorSettings.ChannelMin[n];
 774         } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
 775             // reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
 776             Channel[n] = actuatorSettings.ChannelNeutral[n];
 777         } else {
 778             Channel[n] = 0;
 779         }
 780     }
 781
 782     // Set alarm
 783     AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
 784
 785     // Update servo outputs
 786     for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
 787         set_channel(n, Channel[n]);
 788     }
 789     // Send the updated command
 790     PIOS_Servo_Update();
 791
 792     // Update output object's parts that we changed
 793     ActuatorCommandChannelSet(Channel);
 794 }
 795
 796 /**
 797  * determine buzzer or blink sequence
 798  **/
 799
 800 typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype;
 801
 802 static inline bool buzzerState(buzzertype type)
 803 {
 804     // This is for buzzers that take a PWM input
 805
 806     static uint32_t tune[BUZZ_MAX] = { 0 };
 807     static uint32_t tunestate[BUZZ_MAX] = { 0 };
 808
 809
 810     uint32_t newTune = 0;
 811
 812     if (type == BUZZ_BUZZER) {
 813         // Decide what tune to play
 814         if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
 815             newTune = 0b11110110110000; // pause, short, short, short, long
 816         } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
 817             newTune = 0x80000000; // pause, short
 818         } else {
 819             newTune = 0;
 820         }
 821     } else { // BUZZ_ARMING || BUZZ_INFO
 822         uint8_t arming;
 823         FlightStatusArmedGet(&arming);
 824         // base idle tune
 825         newTune = 0x80000000; // 0b1000...
 826
 827         // Merge the error pattern for InfoLed
 828         if (type == BUZZ_INFO) {
 829             if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
 830                 newTune |= 0b00000000001111111011111110000000;
 831             } else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
 832                 newTune |= 0b00000000000000110110110000000000;
 833             }
 834         }
 835         // fast double blink pattern if armed
 836         if (arming == FLIGHTSTATUS_ARMED_ARMED) {
 837             newTune |= 0xA0000000; // 0b101000...
 838         }
 839     }
 840
 841     // Do we need to change tune?
 842     if (newTune != tune[type]) {
 843         tune[type] = newTune;
 844         // resynchronize all tunes on change, so they stay in sync
 845         for (int i = 0; i < BUZZ_MAX; i++) {
 846             tunestate[i] = tune[i];
 847         }
 848     }
 849
 850     // Play tune
 851     bool buzzOn     = false;
 852     static portTickType lastSysTime = 0;
 853     portTickType thisSysTime = xTaskGetTickCount();
 854     portTickType dT = 0;
 855
 856     // For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
 857     if (tune[type]) {
 858         if (thisSysTime > lastSysTime) {
 859             dT = thisSysTime - lastSysTime;
 860         } else {
 861             lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
 862         }
 863
 864         buzzOn = (tunestate[type] & 1);
 865
 866         if (dT > 80) {
 867             // Go to next bit in alarm_seq_state
 868             for (int i = 0; i < BUZZ_MAX; i++) {
 869                 tunestate[i] >>= 1;
 870                 if (tunestate[i] == 0) { // All done, re-start the tune
 871                     tunestate[i] = tune[i];
 872                 }
 873             }
 874             lastSysTime = thisSysTime;
 875         }
 876     }
 877     return buzzOn;
 878 }
 879
 880
 881 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
 882 static bool set_channel(uint8_t mixer_channel, uint16_t value)
 883 {
 884     return true;
 885 }
 886 #else
 887 static bool set_channel(uint8_t mixer_channel, uint16_t value)
 888 {
 889     switch (actuatorSettings.ChannelType[mixer_channel]) {
 890     case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
 891         PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
 892                        buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
 893         return true;
 894
 895     case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
 896         PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
 897                        buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
 898         return true;
 899
 900     case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
 901         PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
 902                        buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
 903         return true;
 904
 905     case ACTUATORSETTINGS_CHANNELTYPE_PWM:
 906     {
 907         uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
 908         switch (mode) {
 909         case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
 910             // Remap 1000-2000 range to 125-250
 911             PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
 912             break;
 913         default:
 914             PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
 915             break;
 916         }
 917         return true;
 918     }
 919
 920 #if defined(PIOS_INCLUDE_I2C_ESC)
 921     case ACTUATORSETTINGS_CHANNELTYPE_MK:
 922         return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value);
 923
 924     case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
 925         return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value);
 926
 927 #endif
 928     default:
 929         return false;
 930     }
 931
 932     return false;
 933 }
 934 #endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */
 935
 936 /**
 937  * @brief Update the servo update rate
 938  */
 939 static void actuator_update_rate_if_changed(bool force_update)
 940 {
 941     static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
 942     static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
 943     bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
 944     bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
 945
 946     // check if any setting is changed
 947     if (updateMode || updateFreq) {
 948         /* Something has changed, apply the settings to HW */
 949
 950         uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
 951         uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
 952         for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
 953             if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
 954                 PIOS_Servo_SetBankMode(i,
 955                                        actuatorSettings.BankMode[i] ==
 956                                        ACTUATORSETTINGS_BANKMODE_PWM ?
 957                                        PIOS_SERVO_BANK_MODE_PWM :
 958                                        PIOS_SERVO_BANK_MODE_SINGLE_PULSE
 959                                        );
 960             }
 961             switch (actuatorSettings.BankMode[i]) {
 962             case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
 963                 freq[i]  = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
 964                 clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
 965                 break;
 966             case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
 967                 freq[i]  = 100;
 968                 clock[i] = ACTUATOR_PWM_CLOCK;
 969                 break;
 970             default: // PWM
 971                 freq[i]  = actuatorSettings.BankUpdateFreq[i];
 972                 clock[i] = ACTUATOR_PWM_CLOCK;
 973                 break;
 974             }
 975         }
 976
 977         memcpy(prevBankMode,
 978                actuatorSettings.BankMode,
 979                sizeof(prevBankMode));
 980
 981         PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
 982
 983         memcpy(prevBankUpdateFreq,
 984                actuatorSettings.BankUpdateFreq,
 985                sizeof(prevBankUpdateFreq));
 986         // retrieve mode from related bank
 987         for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
 988             uint8_t bank = PIOS_Servo_GetPinBank(i);
 989             pinsMode[i] = actuatorSettings.BankMode[bank];
 990         }
 991     }
 992 }
 993
 994 static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 995 {
 996     ActuatorSettingsGet(&actuatorSettings);
 997     spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
 998     if (frameType == FRAME_TYPE_GROUND) {
 999         spinWhileArmed = false;
1000     }
1001     actuator_update_rate_if_changed(false);
1002 }
1003
1004 static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
1005 {
1006     MixerSettingsGet(&mixerSettings);
1007     mixer_settings_count = 0;
1008     Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
1009     for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
1010         if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
1011             mixer_settings_count++;
1012         }
1013     }
1014 }
1015 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
1016 {
1017     frameType = GetCurrentFrameType();
1018 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
1019     uint8_t TreatCustomCraftAs;
1020     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
1021
1022     if (frameType == FRAME_TYPE_CUSTOM) {
1023         switch (TreatCustomCraftAs) {
1024         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
1025             frameType = FRAME_TYPE_FIXED_WING;
1026             break;
1027         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
1028             frameType = FRAME_TYPE_MULTIROTOR;
1029             break;
1030         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
1031             frameType = FRAME_TYPE_GROUND;
1032             break;
1033         }
1034     }
1035 #endif
1036
1037     SystemSettingsThrustControlGet(&thrustType);
1038 }
1039
1040 /**
1041  * @}
1042  * @}
1043  */