flight/modules/Stabilization/innerloop.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup StabilizationModule Stabilization Module
   6  * @brief Stabilization PID loops in an airframe type independent manner
   7  * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
   8  * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
   9  * @{
  10  *
  11  * @file       innerloop.c
  12  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
  13  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  14  * @brief      Attitude stabilization module.
  15  *
  16  * @see        The GNU Public License (GPL) Version 3
  17  *
  18  *****************************************************************************/
  19 /*
  20  * This program is free software; you can redistribute it and/or modify
  21  * it under the terms of the GNU General Public License as published by
  22  * the Free Software Foundation; either version 3 of the License, or
  23  * (at your option) any later version.
  24  *
  25  * This program is distributed in the hope that it will be useful, but
  26  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  27  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  28  * for more details.
  29  *
  30  * You should have received a copy of the GNU General Public License along
  31  * with this program; if not, write to the Free Software Foundation, Inc.,
  32  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  33  */
  34
  35 #include <openpilot.h>
  36 #include <pid.h>
  37 #include <sin_lookup.h>
  38 #include <callbackinfo.h>
  39 #include <ratedesired.h>
  40 #include <actuatordesired.h>
  41 #include <gyrostate.h>
  42 #include <airspeedstate.h>
  43 #include <stabilizationstatus.h>
  44 #include <flightstatus.h>
  45 #include <manualcontrolcommand.h>
  46 #include <stabilizationbank.h>
  47 #include <stabilizationdesired.h>
  48 #include <actuatordesired.h>
  49
  50 #include <stabilization.h>
  51 #include <virtualflybar.h>
  52 #include <cruisecontrol.h>
  53 #include <sanitycheck.h>
  54 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
  55 #include <systemidentstate.h>
  56 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
  57
  58 // Private constants
  59
  60 #define CALLBACK_PRIORITY   CALLBACK_PRIORITY_CRITICAL
  61
  62 #define UPDATE_EXPECTED     (1.0f / PIOS_SENSOR_RATE)
  63 #define UPDATE_MIN          1.0e-6f
  64 #define UPDATE_MAX          1.0f
  65 #define UPDATE_ALPHA        1.0e-2f
  66
  67 #define SYSTEM_IDENT_PERIOD ((uint32_t)75)
  68
  69 #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
  70 #define powapprox           fastpow
  71 #define expapprox           fastexp
  72 #else
  73 #define powapprox           powf
  74 #define expapprox           expf
  75 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
  76
  77 // Private variables
  78 static DelayedCallbackInfo *callbackHandle;
  79 static float gyro_filtered[3] = { 0, 0, 0 };
  80 static float axis_lock_accum[3] = { 0, 0, 0 };
  81 static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
  82 static PiOSDeltatimeConfig timeval;
  83 static float speedScaleFactor = 1.0f;
  84 static bool frame_is_multirotor;
  85 static bool measuredDterm_enabled;
  86 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
  87 static uint32_t systemIdentTimeVal = 0;
  88 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
  89
  90 // Private functions
  91 static void stabilizationInnerloopTask();
  92 static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  93 #ifdef REVOLUTION
  94 static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  95 #endif
  96
  97 void stabilizationInnerloopInit()
  98 {
  99     RateDesiredInitialize();
 100     ActuatorDesiredInitialize();
 101     GyroStateInitialize();
 102     StabilizationStatusInitialize();
 103     FlightStatusInitialize();
 104     ManualControlCommandInitialize();
 105     StabilizationDesiredInitialize();
 106     ActuatorDesiredInitialize();
 107 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 108     SystemIdentStateInitialize();
 109 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 110 #ifdef REVOLUTION
 111     AirspeedStateInitialize();
 112     AirspeedStateConnectCallback(AirSpeedUpdatedCb);
 113 #endif
 114     PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 115
 116     callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
 117     GyroStateConnectCallback(GyroStateUpdatedCb);
 118
 119     // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
 120     PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
 121
 122     frame_is_multirotor   = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
 123     measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
 124 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 125     // Settings for system identification
 126     systemIdentTimeVal    = PIOS_DELAY_GetRaw();
 127 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 128 }
 129
 130 static float get_pid_scale_source_value()
 131 {
 132     float value;
 133
 134     switch (stabSettings.stabBank.ThrustPIDScaleSource) {
 135     case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE:
 136         ManualControlCommandThrottleGet(&value);
 137         break;
 138     case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST:
 139         StabilizationDesiredThrustGet(&value);
 140         break;
 141     case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST:
 142         ActuatorDesiredThrustGet(&value);
 143         break;
 144     default:
 145         ActuatorDesiredThrustGet(&value);
 146         break;
 147     }
 148
 149     if (value < 0) {
 150         value = 0.0f;
 151     }
 152
 153     return value;
 154 }
 155
 156 typedef struct pid_curve_scaler {
 157     float  x;
 158     pointf points[5];
 159 } pid_curve_scaler;
 160
 161 static float pid_curve_value(const pid_curve_scaler *scaler)
 162 {
 163     float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
 164
 165     return 1.0f + (IS_REAL(y) ? y : 0.0f);
 166 }
 167
 168 static pid_scaler create_pid_scaler(int axis)
 169 {
 170     pid_scaler scaler;
 171
 172     // Always scaled with the this.
 173     scaler.p = scaler.i = scaler.d = speedScaleFactor;
 174
 175     if (stabSettings.thrust_pid_scaling_enabled[axis][0]
 176         || stabSettings.thrust_pid_scaling_enabled[axis][1]
 177         || stabSettings.thrust_pid_scaling_enabled[axis][2]) {
 178         const pid_curve_scaler curve_scaler = {
 179             .x      = get_pid_scale_source_value(),
 180             .points = {
 181                 { 0.00f, stabSettings.floatThrustPIDScaleCurve[0] },
 182                 { 0.25f, stabSettings.floatThrustPIDScaleCurve[1] },
 183                 { 0.50f, stabSettings.floatThrustPIDScaleCurve[2] },
 184                 { 0.75f, stabSettings.floatThrustPIDScaleCurve[3] },
 185                 { 1.00f, stabSettings.floatThrustPIDScaleCurve[4] }
 186             }
 187         };
 188
 189         float curve_value = pid_curve_value(&curve_scaler);
 190
 191         if (stabSettings.thrust_pid_scaling_enabled[axis][0]) {
 192             scaler.p *= curve_value;
 193         }
 194         if (stabSettings.thrust_pid_scaling_enabled[axis][1]) {
 195             scaler.i *= curve_value;
 196         }
 197         if (stabSettings.thrust_pid_scaling_enabled[axis][2]) {
 198             scaler.d *= curve_value;
 199         }
 200     }
 201
 202     return scaler;
 203 }
 204
 205 /**
 206  * WARNING! This callback executes with critical flight control priority every
 207  * time a gyroscope update happens do NOT put any time consuming calculations
 208  * in this loop unless they really have to execute with every gyro update
 209  */
 210 static void stabilizationInnerloopTask()
 211 {
 212     // watchdog and error handling
 213     {
 214 #ifdef PIOS_INCLUDE_WDG
 215         PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
 216 #endif
 217         bool warn  = false;
 218         bool error = false;
 219         bool crit  = false;
 220         // check if outer loop keeps executing
 221         if (stabSettings.monitor.rateupdates > -64) {
 222             stabSettings.monitor.rateupdates--;
 223         }
 224         if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
 225             // warning if rate loop skipped more than 2 execution
 226             warn = true;
 227         }
 228         if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
 229             // critical if rate loop skipped more than 4 executions
 230             crit = true;
 231         }
 232         // check if gyro keeps updating
 233         if (stabSettings.monitor.gyroupdates < 1) {
 234             // error if gyro didn't update at all!
 235             error = true;
 236         }
 237         if (stabSettings.monitor.gyroupdates > 1) {
 238             // warning if we missed a gyro update
 239             warn = true;
 240         }
 241         if (stabSettings.monitor.gyroupdates > 3) {
 242             // critical if we missed 3 gyro updates
 243             crit = true;
 244         }
 245         stabSettings.monitor.gyroupdates = 0;
 246
 247         if (crit) {
 248             AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
 249         } else if (error) {
 250             AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
 251         } else if (warn) {
 252             AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
 253         } else {
 254             AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
 255         }
 256     }
 257
 258     RateDesiredData rateDesired;
 259     ActuatorDesiredData actuator;
 260     StabilizationStatusInnerLoopData enabled;
 261     FlightStatusControlChainData cchain;
 262
 263     RateDesiredGet(&rateDesired);
 264     ActuatorDesiredGet(&actuator);
 265     StabilizationStatusInnerLoopGet(&enabled);
 266     FlightStatusControlChainGet(&cchain);
 267     float *rate = &rateDesired.Roll;
 268     float *actuatorDesiredAxis = &actuator.Roll;
 269     int t;
 270     float dT;
 271     bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor
 272     dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 273
 274     StabilizationStatusOuterLoopData outerLoop;
 275     StabilizationStatusOuterLoopGet(&outerLoop);
 276     bool allowPiroComp = true;
 277
 278
 279     for (t = 0; t < AXES; t++) {
 280         bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
 281         previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
 282
 283         if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
 284             if (reinit) {
 285                 stabSettings.innerPids[t].iAccumulator = 0;
 286                 if (frame_is_multirotor) {
 287                     // Multirotors should dump axis lock accumulators when unarmed or throttle is low.
 288                     // Fixed wing or ground vehicles can fly/drive with low throttle.
 289                     axis_lock_accum[t] = 0;
 290                 }
 291             }
 292             // Any self leveling on roll or pitch must prevent pirouette compensation
 293             if (t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] != STABILIZATIONSTATUS_OUTERLOOP_DIRECT) {
 294                 allowPiroComp = false;
 295             }
 296             switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
 297             case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
 298                 stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
 299                 break;
 300             case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
 301                 if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
 302                     // While getting strong commands act like rate mode
 303                     axis_lock_accum[t] = 0;
 304                 } else {
 305                     // For weaker commands or no command simply attitude lock (almost) on no gyro change
 306                     axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
 307                     axis_lock_accum[t]  = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
 308                     rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
 309                 }
 310             // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
 311             // keep order as it is, RATE must follow!
 312             case STABILIZATIONSTATUS_INNERLOOP_RATE:
 313             {
 314                 // limit rate to maximum configured limits (once here instead of 5 times in outer loop)
 315                 rate[t] = boundf(rate[t],
 316                                  -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
 317                                  StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
 318                                  );
 319                 pid_scaler scaler = create_pid_scaler(t);
 320                 actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
 321             }
 322             break;
 323             case STABILIZATIONSTATUS_INNERLOOP_ACRO:
 324             {
 325                 float stickinput[3];
 326                 stickinput[0] = boundf(rate[0] / stabSettings.stabBank.ManualRate.Roll, -1.0f, 1.0f);
 327                 stickinput[1] = boundf(rate[1] / stabSettings.stabBank.ManualRate.Pitch, -1.0f, 1.0f);
 328                 stickinput[2] = boundf(rate[2] / stabSettings.stabBank.ManualRate.Yaw, -1.0f, 1.0f);
 329                 rate[t] = boundf(rate[t],
 330                                  -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
 331                                  StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
 332                                  );
 333
 334                 pid_scaler ascaler = create_pid_scaler(t);
 335                 ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually
 336                 float arate  = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
 337                 float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t];
 338                 actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
 339             }
 340             break;
 341
 342 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 343             case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
 344             {
 345                 static int8_t identIteration = 0;
 346                 static float identOffsets[3] = { 0 };
 347
 348                 if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) {
 349                     const float SCALE_BIAS = 7.1f;
 350                     SystemIdentStateBetaData systemIdentBeta;
 351
 352                     SystemIdentStateBetaGet(&systemIdentBeta);
 353                     systemIdentTimeVal = PIOS_DELAY_GetRaw();
 354                     identOffsets[0]    = 0.0f;
 355                     identOffsets[1]    = 0.0f;
 356                     identOffsets[2]    = 0.0f;
 357                     identIteration     = (identIteration + 1) & 7;
 358                     // why does yaw change twice a cycle and roll/pitch change only once?
 359                     uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' }
 360                                      )[identIteration];
 361                     float scale   = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentBeta)[index]);
 362                     // if roll or pitch limit to 25% of range
 363                     if (identIteration & 1) {
 364                         if (scale > 0.25f) {
 365                             scale = 0.25f;
 366                         }
 367                     }
 368                     // else it is yaw that can be a little more radical
 369                     else {
 370                         if (scale > 0.45f) {
 371                             scale = 0.45f;
 372                         }
 373                     }
 374                     if (identIteration & 2) {
 375                         scale = -scale;
 376                     }
 377                     identOffsets[index] = scale;
 378                     // this results in:
 379                     // when identIteration==0: identOffsets[2] = yaw_scale;
 380                     // when identIteration==1: identOffsets[0] = roll_scale;
 381                     // when identIteration==2: identOffsets[2] = -yaw_scale;
 382                     // when identIteration==3: identOffsets[0] = -roll_scale;
 383                     // when identIteration==4: identOffsets[2] = yaw_scale;
 384                     // when identIteration==5: identOffsets[1] = pitch_scale;
 385                     // when identIteration==6: identOffsets[2] = -yaw_scale;
 386                     // when identIteration==7: identOffsets[1] = -pitch_scale;
 387                     // each change has one axis with an offset
 388                     // and another axis coming back to zero from having an offset
 389                 }
 390
 391                 rate[t] = boundf(rate[t],
 392                                  -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
 393                                  StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
 394                                  );
 395                 pid_scaler scaler = create_pid_scaler(t);
 396                 actuatorDesiredAxis[t]  = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
 397                 actuatorDesiredAxis[t] += identOffsets[t];
 398             }
 399             break;
 400 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 401
 402             case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
 403             default:
 404                 actuatorDesiredAxis[t] = rate[t];
 405                 break;
 406             }
 407         } else {
 408             switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
 409             case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
 410                 actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
 411                 break;
 412             case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
 413             default:
 414                 actuatorDesiredAxis[t] = rate[t];
 415                 break;
 416             }
 417         }
 418
 419         if (!multirotor) {
 420             // we only need to clamp the desired axis to a sane range if the frame is not a multirotor type
 421             // we don't want to do any clamping until after the motors are calculated and scaled.
 422             // need to figure out what to do with a tricopter tail servo.
 423             actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
 424         }
 425     }
 426
 427     actuator.UpdateTime = dT * 1000;
 428
 429     if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 430         ActuatorDesiredSet(&actuator);
 431     } else {
 432         // Force all axes to reinitialize when engaged
 433         for (t = 0; t < AXES; t++) {
 434             previous_mode[t] = 255;
 435         }
 436     }
 437
 438     if (allowPiroComp && stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
 439         // attempted piro compensation - rotate pitch and yaw integrals (experimental)
 440         float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
 441         float sinYaw   = sinf(angleYaw);
 442         float cosYaw   = cosf(angleYaw);
 443         float rollAcc  = stabSettings.innerPids[0].iAccumulator / stabSettings.innerPids[0].iLim;
 444         float pitchAcc = stabSettings.innerPids[1].iAccumulator / stabSettings.innerPids[1].iLim;
 445         stabSettings.innerPids[0].iAccumulator = stabSettings.innerPids[0].iLim * (cosYaw * rollAcc + sinYaw * pitchAcc);
 446         stabSettings.innerPids[1].iAccumulator = stabSettings.innerPids[1].iLim * (cosYaw * pitchAcc - sinYaw * rollAcc);
 447     }
 448
 449     {
 450         FlightStatusArmedOptions armed;
 451         FlightStatusArmedGet(&armed);
 452         float throttleDesired;
 453         ManualControlCommandThrottleGet(&throttleDesired);
 454         if (armed != FLIGHTSTATUS_ARMED_ARMED ||
 455             ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
 456             // Force all axes to reinitialize when engaged
 457             for (t = 0; t < AXES; t++) {
 458                 previous_mode[t] = 255;
 459             }
 460         }
 461     }
 462     PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
 463 }
 464
 465
 466 static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 467 {
 468     GyroStateData gyroState;
 469
 470     GyroStateGet(&gyroState);
 471
 472     gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
 473     gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
 474     gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
 475
 476     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 477     stabSettings.monitor.gyroupdates++;
 478 }
 479
 480 #ifdef REVOLUTION
 481 static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 482 {
 483     // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
 484     AirspeedStateData airspeedState;
 485
 486     AirspeedStateGet(&airspeedState);
 487     if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
 488         // feature has been turned off
 489         speedScaleFactor = 1.0f;
 490     } else {
 491         // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
 492         speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed),
 493                                   stabSettings.settings.ScaleToAirspeedLimits.Min,
 494                                   stabSettings.settings.ScaleToAirspeedLimits.Max);
 495     }
 496 }
 497 #endif
 498
 499 /**
 500  * @}
 501  * @}
 502  */