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