Rebased

Makefile

@@ -506,6 +506,7 @@ COMMON_SRC = \
             drivers/serial.c \
             drivers/serial_uart.c \
             drivers/sound_beeper.c \
+            drivers/stack_check.c \
             drivers/system.c \
             drivers/timer.c \
             fc/config.c \
@@ -756,7 +757,11 @@ ifeq ($(DEBUG),GDB)
 OPTIMIZE    = -O0
 LTO_FLAGS   = $(OPTIMIZE)
 else
+ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
 OPTIMIZE    = -Os
+else
+OPTIMIZE    = -Ofast
+endif
 LTO_FLAGS   = -flto -fuse-linker-plugin $(OPTIMIZE)
 endif
 

src/main/blackbox/blackbox.c

@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -349,7 +350,7 @@ bool blackboxMayEditConfig()
 }
 
 static bool blackboxIsOnlyLoggingIntraframes() {
-    return masterConfig.blackboxConfig.rate_num == 1 && masterConfig.blackboxConfig.rate_denom == 32;
+    return blackboxConfig()->rate_num == 1 && blackboxConfig()->rate_denom == 32;
 }
 
 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
@@ -369,7 +370,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
             return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
 
         case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
-            return masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI;
+            return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI;
 
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
@@ -394,7 +395,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
             return feature(FEATURE_VBAT);
 
         case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
-            return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
+            return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC;
 
         case FLIGHT_LOG_FIELD_CONDITION_SONAR:
 #ifdef SONAR
@@ -404,10 +405,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 #endif
 
         case FLIGHT_LOG_FIELD_CONDITION_RSSI:
-            return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
+            return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
 
         case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
-            return masterConfig.blackboxConfig.rate_num < masterConfig.blackboxConfig.rate_denom;
+            return blackboxConfig()->rate_num < blackboxConfig()->rate_denom;
 
         case FLIGHT_LOG_FIELD_CONDITION_NEVER:
             return false;
@@ -495,7 +496,7 @@ static void writeIntraframe(void)
      * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
      * Throttle lies in range [minthrottle..maxthrottle]:
      */
-    blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle);
+    blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
         /*
@@ -539,7 +540,7 @@ static void writeIntraframe(void)
     blackboxWriteSigned16VBArray(blackboxCurrent->debug, 4);
 
     //Motors can be below minthrottle when disarmed, but that doesn't happen much
-    blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle);
+    blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
 
     //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
     for (x = 1; x < motorCount; x++) {
@@ -758,22 +759,22 @@ static void validateBlackboxConfig()
 {
     int div;
 
-    if (masterConfig.blackboxConfig.rate_num == 0 || masterConfig.blackboxConfig.rate_denom == 0
+    if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
-            || masterConfig.blackboxConfig.rate_num >= masterConfig.blackboxConfig.rate_denom) {
+            || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
-        masterConfig.blackboxConfig.rate_num = 1;
+        blackboxConfig()->rate_num = 1;
-        masterConfig.blackboxConfig.rate_denom = 1;
+        blackboxConfig()->rate_denom = 1;
     } else {
         /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
          * itself more frequently)
          */
-        div = gcd(masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
+        div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
 
-        masterConfig.blackboxConfig.rate_num /= div;
+        blackboxConfig()->rate_num /= div;
-        masterConfig.blackboxConfig.rate_denom /= div;
+        blackboxConfig()->rate_denom /= div;
     }
 
     // If we've chosen an unsupported device, change the device to serial
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
 #endif
@@ -785,7 +786,7 @@ static void validateBlackboxConfig()
         break;
 
         default:
-            masterConfig.blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
+            blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
     }
 }
 
@@ -867,7 +868,7 @@ bool startedLoggingInTestMode = false;
 void startInTestMode(void)
 {
     if(!startedLoggingInTestMode) {
-        if (masterConfig.blackboxConfig.device == BLACKBOX_DEVICE_SERIAL) {
+        if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) {
             serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
             if (sharedBlackboxAndMspPort) {
                 return; // When in test mode, we cannot share the MSP and serial logger port!
@@ -898,13 +899,13 @@ bool inMotorTestMode(void) {
     static uint32_t resetTime = 0;
     uint16_t inactiveMotorCommand;
     if (feature(FEATURE_3D)) {
-       inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d;
+       inactiveMotorCommand = flight3DConfig()->neutral3d;
 #ifdef USE_DSHOT
     } else if (isMotorProtocolDshot()) {
        inactiveMotorCommand = DSHOT_DISARM_COMMAND;
 #endif
     } else {
-       inactiveMotorCommand = masterConfig.motorConfig.mincommand;
+       inactiveMotorCommand = motorConfig()->mincommand;
     }
 
     int i;
@@ -937,7 +938,7 @@ static void writeGPSHomeFrame()
     gpsHistory.GPS_home[1] = GPS_home[1];
 }
 
-static void writeGPSFrame(uint32_t currentTime)
+static void writeGPSFrame(timeUs_t currentTimeUs)
 {
     blackboxWrite('G');
 
@@ -949,7 +950,7 @@ static void writeGPSFrame(uint32_t currentTime)
      */
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
         // Predict the time of the last frame in the main log
-        blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time);
+        blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
     }
 
     blackboxWriteUnsignedVB(GPS_numSat);
@@ -968,12 +969,12 @@ static void writeGPSFrame(uint32_t currentTime)
 /**
  * Fill the current state of the blackbox using values read from the flight controller
  */
-static void loadMainState(uint32_t currentTime)
+static void loadMainState(timeUs_t currentTimeUs)
 {
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
     int i;
 
-    blackboxCurrent->time = currentTime;
+    blackboxCurrent->time = currentTimeUs;
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
         blackboxCurrent->axisPID_P[i] = axisPID_P[i];
@@ -990,11 +991,11 @@ static void loadMainState(uint32_t currentTime)
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->gyroADC[i] = gyroADC[i];
+        blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->accSmooth[i] = accSmooth[i];
+        blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
     }
 
     for (i = 0; i < 4; i++) {
@@ -1010,12 +1011,12 @@ static void loadMainState(uint32_t currentTime)
 
 #ifdef MAG
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->magADC[i] = magADC[i];
+        blackboxCurrent->magADC[i] = mag.magADC[i];
     }
 #endif
 
 #ifdef BARO
-    blackboxCurrent->BaroAlt = BaroAlt;
+    blackboxCurrent->BaroAlt = baro.BaroAlt;
 #endif
 
 #ifdef SONAR
@@ -1172,34 +1173,34 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
         BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s",                 buildDate, buildTime);
         BLACKBOX_PRINT_HEADER_LINE("Craft name:%s",                       masterConfig.name);
-        BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d",                    masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
+        BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d",                    blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
-        BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                      masterConfig.motorConfig.minthrottle);
+        BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                      motorConfig()->minthrottle);
-        BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                      masterConfig.motorConfig.maxthrottle);
+        BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                      motorConfig()->maxthrottle);
-        BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x",                     castFloatBytesToInt(gyro.scale));
+        BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x",                     castFloatBytesToInt(gyro.dev.scale));
-        BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u",                           acc.acc_1G);
+        BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u",                           acc.dev.acc_1G);
 
         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
             if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
-                blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale);
+                blackboxPrintfHeaderLine("vbatscale:%u", batteryConfig()->vbatscale);
             } else {
                 xmitState.headerIndex += 2; // Skip the next two vbat fields too
             }
             );
 
-        BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u",            masterConfig.batteryConfig.vbatmincellvoltage,
+        BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u",            batteryConfig()->vbatmincellvoltage,
-                                                                          masterConfig.batteryConfig.vbatwarningcellvoltage,
+                                                                          batteryConfig()->vbatwarningcellvoltage,
-                                                                          masterConfig.batteryConfig.vbatmaxcellvoltage);
+                                                                          batteryConfig()->vbatmaxcellvoltage);
         BLACKBOX_PRINT_HEADER_LINE("vbatref:%u",                          vbatReference);
 
         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
             //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
             if (feature(FEATURE_CURRENT_METER)) {
-                blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale);
+                blackboxPrintfHeaderLine("currentMeter:%d,%d", batteryConfig()->currentMeterOffset, batteryConfig()->currentMeterScale);
             }
             );
 
         BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                         gyro.targetLooptime);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d",                  masterConfig.gyroConfig.gyro_sync_denom);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d",                  gyroConfig()->gyro_sync_denom);
         BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d",                masterConfig.pid_process_denom);
         BLACKBOX_PRINT_HEADER_LINE("rcRate:%d",                           masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
         BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d",                           masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
@@ -1260,27 +1261,27 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d",                   masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit);
         // End of Betaflight controller parameters
 
-        BLACKBOX_PRINT_HEADER_LINE("deadband:%d",                         masterConfig.rcControlsConfig.deadband);
+        BLACKBOX_PRINT_HEADER_LINE("deadband:%d",                         rcControlsConfig()->deadband);
-        BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d",                     masterConfig.rcControlsConfig.yaw_deadband);
+        BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d",                     rcControlsConfig()->yaw_deadband);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d",                         masterConfig.gyroConfig.gyro_lpf);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d",                         gyroConfig()->gyro_lpf);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d",                   masterConfig.gyroConfig.gyro_soft_lpf_type);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d",                   gyroConfig()->gyro_soft_lpf_type);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d",                  masterConfig.gyroConfig.gyro_soft_lpf_hz);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d",                  gyroConfig()->gyro_soft_lpf_hz);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d",                 masterConfig.gyroConfig.gyro_soft_notch_hz_1,
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d",                 gyroConfig()->gyro_soft_notch_hz_1,
-                                                                          masterConfig.gyroConfig.gyro_soft_notch_hz_2);
+                                                                          gyroConfig()->gyro_soft_notch_hz_2);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d",             masterConfig.gyroConfig.gyro_soft_notch_cutoff_1,
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d",             gyroConfig()->gyro_soft_notch_cutoff_1,
-                                                                          masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
+                                                                          gyroConfig()->gyro_soft_notch_cutoff_2);
-        BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d",                 (int)(masterConfig.acc_lpf_hz * 100.0f));
+        BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d",                 (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
-        BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d",                     masterConfig.sensorSelectionConfig.acc_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d",                     sensorSelectionConfig()->acc_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d",                    masterConfig.sensorSelectionConfig.baro_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d",                    sensorSelectionConfig()->baro_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d",                     masterConfig.sensorSelectionConfig.mag_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d",                     sensorSelectionConfig()->mag_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d",            masterConfig.armingConfig.gyro_cal_on_first_arm);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d",            armingConfig()->gyro_cal_on_first_arm);
-        BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d",                 masterConfig.rxConfig.rcInterpolation);
+        BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d",                 rxConfig()->rcInterpolation);
-        BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d",        masterConfig.rxConfig.rcInterpolationInterval);
+        BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d",        rxConfig()->rcInterpolationInterval);
-        BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d",        masterConfig.rxConfig.airModeActivateThreshold);
+        BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d",        rxConfig()->airModeActivateThreshold);
-        BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d",                masterConfig.rxConfig.serialrx_provider);
+        BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d",                rxConfig()->serialrx_provider);
-        BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d",                masterConfig.motorConfig.useUnsyncedPwm);
+        BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d",                motorConfig()->useUnsyncedPwm);
-        BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d",                masterConfig.motorConfig.motorPwmProtocol);
+        BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d",                motorConfig()->motorPwmProtocol);
-        BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d",                   masterConfig.motorConfig.motorPwmRate);
+        BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d",                   motorConfig()->motorPwmRate);
         BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d",                       masterConfig.debug_mode);
         BLACKBOX_PRINT_HEADER_LINE("features:%d",                         masterConfig.enabledFeatures);
 
@@ -1376,10 +1377,10 @@ static void blackboxCheckAndLogFlightMode()
  */
 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
 {
-    /* Adding a magic shift of "masterConfig.blackboxConfig.rate_num - 1" in here creates a better spread of
+    /* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
      * recorded / skipped frames when the I frame's position is considered:
      */
-    return (pFrameIndex + masterConfig.blackboxConfig.rate_num - 1) % masterConfig.blackboxConfig.rate_denom < masterConfig.blackboxConfig.rate_num;
+    return (pFrameIndex + blackboxConfig()->rate_num - 1) % blackboxConfig()->rate_denom < blackboxConfig()->rate_num;
 }
 
 static bool blackboxShouldLogIFrame() {
@@ -1400,7 +1401,7 @@ static void blackboxAdvanceIterationTimers()
 }
 
 // Called once every FC loop in order to log the current state
-static void blackboxLogIteration(uint32_t currentTime)
+static void blackboxLogIteration(timeUs_t currentTimeUs)
 {
     // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
     if (blackboxShouldLogIFrame()) {
@@ -1410,7 +1411,7 @@ static void blackboxLogIteration(uint32_t currentTime)
          */
         writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
 
-        loadMainState(currentTime);
+        loadMainState(currentTimeUs);
         writeIntraframe();
     } else {
         blackboxCheckAndLogArmingBeep();
@@ -1423,7 +1424,7 @@ static void blackboxLogIteration(uint32_t currentTime)
              */
             writeSlowFrameIfNeeded(true);
 
-            loadMainState(currentTime);
+            loadMainState(currentTimeUs);
             writeInterframe();
         }
 #ifdef GPS
@@ -1439,11 +1440,11 @@ static void blackboxLogIteration(uint32_t currentTime)
                 || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
 
                 writeGPSHomeFrame();
-                writeGPSFrame(currentTime);
+                writeGPSFrame(currentTimeUs);
             } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
                     || GPS_coord[1] != gpsHistory.GPS_coord[1]) {
                 //We could check for velocity changes as well but I doubt it changes independent of position
-                writeGPSFrame(currentTime);
+                writeGPSFrame(currentTimeUs);
             }
         }
 #endif
@@ -1456,7 +1457,7 @@ static void blackboxLogIteration(uint32_t currentTime)
 /**
  * Call each flight loop iteration to perform blackbox logging.
  */
-void handleBlackbox(uint32_t currentTime)
+void handleBlackbox(timeUs_t currentTimeUs)
 {
     int i;
 
@@ -1548,12 +1549,12 @@ void handleBlackbox(uint32_t currentTime)
                 flightLogEvent_loggingResume_t resume;
 
                 resume.logIteration = blackboxIteration;
-                resume.currentTime = currentTime;
+                resume.currentTime = currentTimeUs;
 
                 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
                 blackboxSetState(BLACKBOX_STATE_RUNNING);
 
-                blackboxLogIteration(currentTime);
+                blackboxLogIteration(currentTimeUs);
             }
 
             // Keep the logging timers ticking so our log iteration continues to advance
@@ -1565,7 +1566,7 @@ void handleBlackbox(uint32_t currentTime)
             if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) {
                 blackboxSetState(BLACKBOX_STATE_PAUSED);
             } else {
-                blackboxLogIteration(currentTime);
+                blackboxLogIteration(currentTimeUs);
             }
 
             blackboxAdvanceIterationTimers();
@@ -1595,7 +1596,7 @@ void handleBlackbox(uint32_t currentTime)
         if (startedLoggingInTestMode) startedLoggingInTestMode = false;
     } else { // Only log in test mode if there is room!
 
-        if(masterConfig.blackboxConfig.on_motor_test) {
+        if(blackboxConfig()->on_motor_test) {
             // Handle Motor Test Mode
             if(inMotorTestMode()) {
                 if(blackboxState==BLACKBOX_STATE_STOPPED)

src/main/blackbox/blackbox.h

@@ -19,6 +19,8 @@
 
 #include "blackbox/blackbox_fielddefs.h"
 
+#include "common/time.h"
+
 typedef struct blackboxConfig_s {
     uint8_t rate_num;
     uint8_t rate_denom;
@@ -29,7 +31,7 @@ typedef struct blackboxConfig_s {
 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
 
 void initBlackbox(void);
-void handleBlackbox(uint32_t currentTime);
+void handleBlackbox(timeUs_t currentTimeUs);
 void startBlackbox(void);
 void finishBlackbox(void);
 

src/main/blackbox/blackbox_io.c

@@ -65,7 +65,7 @@ static struct {
 
 void blackboxWrite(uint8_t value)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
             flashfsWriteByte(value); // Write byte asynchronously
@@ -137,7 +137,7 @@ int blackboxPrint(const char *s)
     int length;
     const uint8_t *pos;
 
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
@@ -479,7 +479,7 @@ void blackboxWriteFloat(float value)
  */
 void blackboxDeviceFlush(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 #ifdef USE_FLASHFS
         /*
          * This is our only output device which requires us to call flush() in order for it to write anything. The other
@@ -502,7 +502,7 @@ void blackboxDeviceFlush(void)
  */
 bool blackboxDeviceFlushForce(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             // Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer
             return isSerialTransmitBufferEmpty(blackboxPort);
@@ -530,7 +530,7 @@ bool blackboxDeviceFlushForce(void)
  */
 bool blackboxDeviceOpen(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             {
                 serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
@@ -604,7 +604,7 @@ bool blackboxDeviceOpen(void)
  */
 void blackboxDeviceClose(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             // Since the serial port could be shared with other processes, we have to give it back here
             closeSerialPort(blackboxPort);
@@ -748,7 +748,7 @@ static bool blackboxSDCardBeginLog()
  */
 bool blackboxDeviceBeginLog(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 #ifdef USE_SDCARD
         case BLACKBOX_DEVICE_SDCARD:
             return blackboxSDCardBeginLog();
@@ -772,7 +772,7 @@ bool blackboxDeviceEndLog(bool retainLog)
     (void) retainLog;
 #endif
 
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
 #ifdef USE_SDCARD
         case BLACKBOX_DEVICE_SDCARD:
             // Keep retrying until the close operation queues
@@ -794,7 +794,7 @@ bool blackboxDeviceEndLog(bool retainLog)
 
 bool isBlackboxDeviceFull(void)
 {
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             return false;
 
@@ -821,7 +821,7 @@ void blackboxReplenishHeaderBudget()
 {
     int32_t freeSpace;
 
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             freeSpace = serialTxBytesFree(blackboxPort);
         break;
@@ -867,7 +867,7 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
     }
 
     // Handle failure:
-    switch (masterConfig.blackboxConfig.device) {
+    switch (blackboxConfig()->device) {
         case BLACKBOX_DEVICE_SERIAL:
             /*
              * One byte of the tx buffer isn't available for user data (due to its circular list implementation),

src/main/build/debug.h

@@ -60,5 +60,7 @@ typedef enum {
     DEBUG_DTERM_FILTER,
     DEBUG_ANGLERATE,
     DEBUG_ESC_TELEMETRY,
+    DEBUG_SCHEDULER,
+    DEBUG_STACK,
     DEBUG_COUNT
 } debugType_e;

src/main/cms/cms.c

@@ -947,16 +947,16 @@ static void cmsUpdate(uint32_t currentTimeUs)
     lastCalledMs = currentTimeMs;
 }
 
-void cmsHandler(uint32_t currentTime)
+void cmsHandler(timeUs_t currentTimeUs)
 {
     if (cmsDeviceCount < 0)
         return;
 
-    static uint32_t lastCalled = 0;
+    static timeUs_t lastCalledUs = 0;
 
-    if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) {
+    if (currentTimeUs >= lastCalledUs + CMS_UPDATE_INTERVAL_US) {
-        lastCalled = currentTime;
+        lastCalledUs = currentTimeUs;
-        cmsUpdate(currentTime);
+        cmsUpdate(currentTimeUs);
     }
 }
 

src/main/cms/cms.h

@@ -2,12 +2,14 @@
 
 #include "drivers/display.h"
 
+#include "common/time.h"
+
 // Device management
 bool cmsDisplayPortRegister(displayPort_t *pDisplay);
 
 // For main.c and scheduler
 void cmsInit(void);
-void cmsHandler(uint32_t currentTime);
+void cmsHandler(timeUs_t currentTimeUs);
 
 long cmsMenuChange(displayPort_t *pPort, const void *ptr);
 long cmsMenuExit(displayPort_t *pPort, const void *ptr);

src/main/cms/cms_menu_blackbox.c

@@ -90,7 +90,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
 {
     { "-- BLACKBOX --", OME_Label, NULL, NULL, 0},
     { "ENABLED",     OME_Bool,    NULL,            &cmsx_FeatureBlackbox,                                      0 },
-    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &masterConfig.blackboxConfig.rate_denom,1,32,1 }, 0 },
+    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &blackboxConfig()->rate_denom,1,32,1 }, 0 },
 
 #ifdef USE_FLASHFS
     { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL,                                                       0 },

src/main/cms/cms_menu_imu.c

@@ -130,6 +130,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
         masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
         masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
     }
+    pidInitConfig(&currentProfile->pidProfile);
 
     return 0;
 }
@@ -248,6 +249,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 
     masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
     masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
+    pidInitConfig(&currentProfile->pidProfile);
 
     masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
     masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
@@ -282,11 +284,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
 {
     { "-- FILTER GLB  --", OME_Label, NULL, NULL, 0 },
 
-    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &masterConfig.gyroConfig.gyro_soft_lpf_hz,         0, 255, 1 }, 0 },
+    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &gyroConfig()->gyro_soft_lpf_hz,         0, 255, 1 }, 0 },
-    { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
+    { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
-    { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
+    { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
-    { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
+    { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
-    { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
+    { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
 
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }

src/main/cms/cms_menu_misc.c

@@ -83,9 +83,9 @@ static OSD_Entry menuMiscEntries[]=
 {
     { "-- MISC --", OME_Label, NULL, NULL, 0 },
 
-    { "MIN THR",    OME_UINT16,  NULL,          &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle,         1000, 2000, 1 }, 0 },
+    { "MIN THR",    OME_UINT16,  NULL,          &(OSD_UINT16_t){ &motorConfig()->minthrottle,         1000, 2000, 1 }, 0 },
-    { "VBAT SCALE", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale,             1, 250, 1 }, 0 },
+    { "VBAT SCALE", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig()->vbatscale,             1, 250, 1 }, 0 },
-    { "VBAT CLMAX", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage,   10,  50, 1 }, 0 },
+    { "VBAT CLMAX", OME_UINT8,   NULL,          &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage,   10,  50, 1 }, 0 },
     { "RC PREV",    OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
 
     { "BACK", OME_Back, NULL, NULL, 0},

src/main/cms/cms_menu_osd.c

@@ -33,10 +33,10 @@
 
 #if defined(OSD) && defined(CMS)
 
-OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5};
+OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
-OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50};
+OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};
-OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1};
+OSD_UINT16_t enryAlarmFlyTime = {&osdProfile()->time_alarm, 1, 200, 1};
-OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1};
+OSD_UINT16_t entryAlarmAltitude = {&osdProfile()->alt_alarm, 1, 200, 1};
 
 OSD_Entry cmsx_menuAlarmsEntries[] =
 {
@@ -61,25 +61,25 @@ CMS_Menu cmsx_menuAlarms = {
 OSD_Entry menuOsdActiveElemsEntries[] =
 {
     {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
-    {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0},
+    {"RSSI", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_RSSI_VALUE], 0},
-    {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
+    {"MAIN BATTERY", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
-    {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0},
+    {"HORIZON", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], 0},
-    {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0},
+    {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_HORIZON_SIDEBARS], 0},
-    {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0},
+    {"UPTIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ONTIME], 0},
-    {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0},
+    {"FLY TIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYTIME], 0},
-    {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0},
+    {"FLY MODE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYMODE], 0},
-    {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0},
+    {"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0},
-    {"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0},
+    {"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0},
 #ifdef VTX
-    {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]},
+    {"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL]},
 #endif // VTX
-    {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0},
+    {"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0},
-    {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0},
+    {"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0},
 #ifdef GPS
-    {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0},
+    {"GPS SPEED", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SPEED], 0},
-    {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0},
+    {"GPS SATS.", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SATS], 0},
 #endif // GPS
-    {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0},
+    {"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };

src/main/common/filter.h

@@ -15,6 +15,8 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma once
+
 #ifdef STM32F10X
 #define MAX_FIR_DENOISE_WINDOW_SIZE 60
 #else

src/main/common/time.h

@@ -0,0 +1,37 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "platform.h"
+
+// time difference, 32 bits always sufficient
+typedef int32_t timeDelta_t;
+// millisecond time
+typedef uint32_t timeMs_t ;
+// microsecond time
+#ifdef USE_64BIT_TIME
+typedef uint64_t timeUs_t;
+#define TIMEUS_MAX UINT64_MAX
+#else
+typedef uint32_t timeUs_t;
+#define TIMEUS_MAX UINT32_MAX
+#endif
+
+static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }

src/main/common/typeconversion.c

@@ -31,9 +31,9 @@ void uli2a(unsigned long int num, unsigned int base, int uc, char *bf)
 
     while (d != 0) {
         int dgt = num / d;
-	*bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+    *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
 
-	// Next digit
+    // Next digit
         num %= d;
         d /= base;
     }
@@ -60,9 +60,9 @@ void ui2a(unsigned int num, unsigned int base, int uc, char *bf)
 
     while (d != 0) {
         int dgt = num / d;
-	*bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
+    *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10);
 
-	// Next digit
+    // Next digit
         num %= d;
         d /= base;
     }

src/main/config/config_eeprom.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#define EEPROM_CONF_VERSION 146
+#define EEPROM_CONF_VERSION 147
 
 void initEEPROM(void);
 void writeEEPROM();

src/main/config/config_master.h

@@ -61,6 +61,44 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 
+#define motorConfig(x) (&masterConfig.motorConfig)
+#define flight3DConfig(x) (&masterConfig.flight3DConfig)
+#define servoConfig(x) (&masterConfig.servoConfig)
+#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
+#define gimbalConfig(x) (&masterConfig.gimbalConfig)
+#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig)
+#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig)
+#define sensorTrims(x) (&masterConfig.sensorTrims)
+#define boardAlignment(x) (&masterConfig.boardAlignment)
+#define imuConfig(x) (&masterConfig.imuConfig)
+#define gyroConfig(x) (&masterConfig.gyroConfig)
+#define compassConfig(x) (&masterConfig.compassConfig)
+#define accelerometerConfig(x) (&masterConfig.accelerometerConfig)
+#define barometerConfig(x) (&masterConfig.barometerConfig)
+#define throttleCorrectionConfig(x) (&masterConfig.throttleCorrectionConfig)
+#define batteryConfig(x) (&masterConfig.batteryConfig)
+#define rcControlsConfig(x) (&masterConfig.rcControlsConfig)
+#define gpsProfile(x) (&masterConfig.gpsProfile)
+#define gpsConfig(x) (&masterConfig.gpsConfig)
+#define rxConfig(x) (&masterConfig.rxConfig)
+#define armingConfig(x) (&masterConfig.armingConfig)
+#define mixerConfig(x) (&masterConfig.mixerConfig)
+#define airplaneConfig(x) (&masterConfig.airplaneConfig)
+#define failsafeConfig(x) (&masterConfig.failsafeConfig)
+#define serialConfig(x) (&masterConfig.serialConfig)
+#define telemetryConfig(x) (&masterConfig.telemetryConfig)
+#define ppmConfig(x) (&masterConfig.ppmConfig)
+#define pwmConfig(x) (&masterConfig.pwmConfig)
+#define adcConfig(x) (&masterConfig.adcConfig)
+#define beeperConfig(x) (&masterConfig.beeperConfig)
+#define sonarConfig(x) (&masterConfig.sonarConfig)
+#define ledStripConfig(x) (&masterConfig.ledStripConfig)
+#define osdProfile(x) (&masterConfig.osdProfile)
+#define vcdProfile(x) (&masterConfig.vcdProfile)
+#define sdcardConfig(x) (&masterConfig.sdcardConfig)
+#define blackboxConfig(x) (&masterConfig.blackboxConfig)
+
+
 // System-wide
 typedef struct master_s {
     uint8_t version;
@@ -87,13 +125,13 @@ typedef struct master_s {
     // global sensor-related stuff
     sensorSelectionConfig_t sensorSelectionConfig;
     sensorAlignmentConfig_t sensorAlignmentConfig;
+    sensorTrims_t sensorTrims;
     boardAlignment_t boardAlignment;
 
-    int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
+    imuConfig_t imuConfig;
-    uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
 
-    uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
+    rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
-    uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
+    uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
 
     uint8_t pid_process_denom;              // Processing denominator for PID controller vs gyro sampling rate
 
@@ -102,15 +140,12 @@ typedef struct master_s {
     gyroConfig_t gyroConfig;
     compassConfig_t compassConfig;
 
-    rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
+    accelerometerConfig_t accelerometerConfig;
 
-    uint16_t acc_lpf_hz;                       // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
-    accDeadband_t accDeadband;
     barometerConfig_t barometerConfig;
-    uint8_t acc_unarmedcal;                 // turn automatic acc compensation on/off
 
-    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
+    throttleCorrectionConfig_t throttleCorrectionConfig;
-    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
+
     batteryConfig_t batteryConfig;
 
     // Radio/ESC-related configuration
@@ -121,9 +156,6 @@ typedef struct master_s {
     gpsConfig_t gpsConfig;
 #endif
 
-    uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
-    sensorTrims_t sensorTrims;
-
     rxConfig_t rxConfig;
     inputFilteringMode_e inputFilteringMode;  // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
 
@@ -140,7 +172,7 @@ typedef struct master_s {
 #ifdef USE_PPM
     ppmConfig_t ppmConfig;
 #endif
-    
+
 #ifdef USE_PWM
     pwmConfig_t pwmConfig;
 #endif
@@ -177,7 +209,7 @@ typedef struct master_s {
 #ifdef USE_MAX7456
     vcdProfile_t vcdProfile;
 #endif
-    
+
 #ifdef USE_SDCARD
     sdcardConfig_t sdcardConfig;
 #endif

src/main/config/parameter_group_ids.h

@@ -30,54 +30,54 @@
 #define PG_CONTROL_RATE_PROFILES 12 // struct OK, needs to be split out of rc_controls.h into rate_profile.h
 #define PG_SERIAL_CONFIG 13 // struct OK
 #define PG_PID_PROFILE 14 // struct OK, CF differences
-#define PG_GTUNE_CONFIG 15
+#define PG_GTUNE_CONFIG 15 // is GTUNE still being used?
-#define PG_ARMING_CONFIG 16
+#define PG_ARMING_CONFIG 16 // structs OK, CF naming differences
-#define PG_TRANSPONDER_CONFIG 17
+#define PG_TRANSPONDER_CONFIG 17 // struct OK
-#define PG_SYSTEM_CONFIG 18
+#define PG_SYSTEM_CONFIG 18 // just has i2c_highspeed
-#define PG_FEATURE_CONFIG 19
+#define PG_FEATURE_CONFIG 19 // just has enabledFeatures
-#define PG_MIXER_CONFIG 20
+#define PG_MIXER_CONFIG 20 // Cleanflight has single struct mixerConfig_t, betaflight has mixerConfig_t and servoMixerConfig_t
-#define PG_SERVO_MIXER 21
+#define PG_SERVO_MIXER 21 // Cleanflight has servoParam_t for each servo, betaflight has single servoParam_t
-#define PG_IMU_CONFIG 22
+#define PG_IMU_CONFIG 22 // Cleanflight has imuConfig_t, betaflight has imuRuntimeConfig_t with additional parameters
-#define PG_PROFILE_SELECTION 23
+#define PG_PROFILE_SELECTION 23 // just contains current_profile_index
-#define PG_RX_CONFIG 24
+#define PG_RX_CONFIG 24 // betaflight rxConfig_t contains different values
-#define PG_RC_CONTROLS_CONFIG 25
+#define PG_RC_CONTROLS_CONFIG 25 // Cleanflight has more parameters in rcControlsConfig_t
-#define PG_MOTOR_3D_CONFIG 26
+#define PG_MOTOR_3D_CONFIG 26 // Cleanflight has motor3DConfig_t, betaflight has flight3DConfig_t with more parameters
-#define PG_LED_STRIP_CONFIG 27
+#define PG_LED_STRIP_CONFIG 27 // structs OK
-#define PG_COLOR_CONFIG 28
+#define PG_COLOR_CONFIG 28 // part of led strip, structs OK
-#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
+#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 // struct OK
-#define PG_GPS_CONFIG 30
+#define PG_GPS_CONFIG 30 // struct OK
-#define PG_TELEMETRY_CONFIG 31
+#define PG_TELEMETRY_CONFIG 31 // betaflight has more and different data in telemetryConfig_t
-#define PG_FRSKY_TELEMETRY_CONFIG 32
+#define PG_FRSKY_TELEMETRY_CONFIG 32 // Cleanflight has split data out of PG_TELEMETRY_CONFIG
-#define PG_HOTT_TELEMETRY_CONFIG 33
+#define PG_HOTT_TELEMETRY_CONFIG 33 // Cleanflight has split data out of PG_TELEMETRY_CONFIG
-#define PG_NAVIGATION_CONFIG 34
+#define PG_NAVIGATION_CONFIG 34 // structs OK
-#define PG_ACCELEROMETER_CONFIG 35
+#define PG_ACCELEROMETER_CONFIG 35 // no accelerometerConfig_t in betaflight
-#define PG_RATE_PROFILE_SELECTION 36
+#define PG_RATE_PROFILE_SELECTION 36 // part of profile in betaflight
-#define PG_ADJUSTMENT_PROFILE 37
+#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
-#define PG_BAROMETER_CONFIG 38
+#define PG_BAROMETER_CONFIG 38 // structs OK
 #define PG_THROTTLE_CORRECTION_CONFIG 39
-#define PG_COMPASS_CONFIGURATION 40
+#define PG_COMPASS_CONFIGURATION 40 // structs OK
-#define PG_MODE_ACTIVATION_PROFILE 41
+#define PG_MODE_ACTIVATION_PROFILE 41 // array needs to be made into struct
 #define PG_SERVO_PROFILE 42
-#define PG_FAILSAFE_CHANNEL_CONFIG 43
+#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
-#define PG_CHANNEL_RANGE_CONFIG 44
+#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
-#define PG_MODE_COLOR_CONFIG 45
+#define PG_MODE_COLOR_CONFIG 45  // part of led strip, structs OK
-#define PG_SPECIAL_COLOR_CONFIG 46
+#define PG_SPECIAL_COLOR_CONFIG 46  // part of led strip, structs OK
-#define PG_PILOT_CONFIG 47
+#define PG_PILOT_CONFIG 47 // does not exist in betaflight
-#define PG_MSP_SERVER_CONFIG 48
+#define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight
-#define PG_VOLTAGE_METER_CONFIG 49
+#define PG_VOLTAGE_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t
-#define PG_AMPERAGE_METER_CONFIG 50
+#define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t
 
 // Driver configuration
-#define PG_DRIVER_PWM_RX_CONFIG 100
+#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
-#define PG_DRIVER_FLASHCHIP_CONFIG 101
+#define PG_DRIVER_FLASHCHIP_CONFIG 101 // does not exist in betaflight
 
 // OSD configuration (subject to change)
-#define PG_OSD_FONT_CONFIG 32768
+#define PG_OSD_FONT_CONFIG 2047
-#define PG_OSD_VIDEO_CONFIG 32769
+#define PG_OSD_VIDEO_CONFIG 2046
-#define PG_OSD_ELEMENT_CONFIG 32770
+#define PG_OSD_ELEMENT_CONFIG 2045
 
-
+// 4095 is currently the highest number that can be used for a PGN due to the top 4 bits of the 16 bit value being reserved for the version when the PG is stored in an EEPROM.
-#define PG_RESERVED_FOR_TESTING_1 65533
+#define PG_RESERVED_FOR_TESTING_1 4095
-#define PG_RESERVED_FOR_TESTING_2 65534
+#define PG_RESERVED_FOR_TESTING_2 4094
-#define PG_RESERVED_FOR_TESTING_3 65535
+#define PG_RESERVED_FOR_TESTING_3 4093
 

src/main/drivers/accgyro.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "common/axis.h"
 #include "drivers/sensor.h"
 
 #ifndef MPU_I2C_INSTANCE
@@ -32,18 +33,20 @@
 #define GYRO_LPF_5HZ        6
 #define GYRO_LPF_NONE       7
 
-typedef struct gyro_s {
+typedef struct gyroDev_s {
     sensorGyroInitFuncPtr init;                             // initialize function
-    sensorReadFuncPtr read;                                 // read 3 axis data function
+    sensorGyroReadFuncPtr read;                                 // read 3 axis data function
     sensorReadFuncPtr temperature;                          // read temperature if available
-    sensorInterruptFuncPtr intStatus;
+    sensorGyroInterruptStatusFuncPtr intStatus;
     float scale;                                            // scalefactor
-    uint32_t targetLooptime;
+    volatile bool dataReady;
-} gyro_t;
+    uint16_t lpf;
+    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+} gyroDev_t;
 
-typedef struct acc_s {
+typedef struct accDev_s {
     sensorAccInitFuncPtr init;                                 // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
     uint16_t acc_1G;
     char revisionCode;                                      // a revision code for the sensor, if known
-} acc_t;
+} accDev_t;

src/main/drivers/accgyro_adxl345.c

@@ -56,16 +56,16 @@
 #define ADXL345_RANGE_16G   0x03
 #define ADXL345_FIFO_STREAM 0x80
 
-static void adxl345Init(acc_t *acc);
+static void adxl345Init(accDev_t *acc);
 static bool adxl345Read(int16_t *accelData);
 
 static bool useFifo = false;
 
-bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
+bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc)
 {
     uint8_t sig = 0;
     bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig);
-    
+
     if (!ack || sig != 0xE5)
         return false;
 
@@ -77,7 +77,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
     return true;
 }
 
-static void adxl345Init(acc_t *acc)
+static void adxl345Init(accDev_t *acc)
 {
     if (useFifo) {
         uint8_t fifoDepth = 16;

src/main/drivers/accgyro_adxl345.h

@@ -22,4 +22,4 @@ typedef struct drv_adxl345_config_s {
     uint16_t dataRate;
 } drv_adxl345_config_t;
 
-bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc);
+bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc);

src/main/drivers/accgyro_bma280.c

@@ -32,10 +32,10 @@
 #define BMA280_PMU_BW      0x10
 #define BMA280_PMU_RANGE   0x0F
 
-static void bma280Init(acc_t *acc);
+static void bma280Init(accDev_t *acc);
 static bool bma280Read(int16_t *accelData);
 
-bool bma280Detect(acc_t *acc)
+bool bma280Detect(accDev_t *acc)
 {
     uint8_t sig = 0;
     bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
@@ -48,7 +48,7 @@ bool bma280Detect(acc_t *acc)
     return true;
 }
 
-static void bma280Init(acc_t *acc)
+static void bma280Init(accDev_t *acc)
 {
     i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
     i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW

src/main/drivers/accgyro_bma280.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool bma280Detect(acc_t *acc);
+bool bma280Detect(accDev_t *acc);

src/main/drivers/accgyro_l3g4200d.c

@@ -54,10 +54,10 @@
 #define L3G4200D_DLPF_78HZ       0x80
 #define L3G4200D_DLPF_93HZ       0xC0
 
-static void l3g4200dInit(uint8_t lpf);
+static void l3g4200dInit(gyroDev_t *gyro);
-static bool l3g4200dRead(int16_t *gyroADC);
+static bool l3g4200dRead(gyroDev_t *gyro);
 
-bool l3g4200dDetect(gyro_t *gyro)
+bool l3g4200dDetect(gyroDev_t *gyro)
 {
     uint8_t deviceid;
 
@@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
     return true;
 }
 
-static void l3g4200dInit(uint8_t lpf)
+static void l3g4200dInit(gyroDev_t *gyro)
 {
     bool ack;
 
     uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
 
-    switch (lpf) {
+    switch (gyro->lpf) {
         default:
             case 32:
             mpuLowPassFilter = L3G4200D_DLPF_32HZ;
@@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf)
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static bool l3g4200dRead(int16_t *gyroADC)
+static bool l3g4200dRead(gyroDev_t *gyro)
 {
     uint8_t buf[6];
 
@@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC)
         return false;
     }
 
-    gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
 
     return true;
 }

src/main/drivers/accgyro_l3g4200d.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3g4200dDetect(gyro_t *gyro);
+bool l3g4200dDetect(gyroDev_t *gyro);

src/main/drivers/accgyro_l3gd20.c

@@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
     spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
 }
 
-void l3gd20GyroInit(uint8_t lpf)
+void l3gd20GyroInit(gyroDev_t *gyro)
 {
-    UNUSED(lpf); // FIXME use it!
+    UNUSED(gyro); // FIXME use it!
 
     l3gd20SpiInit(L3GD20_SPI);
 
@@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf)
     delay(100);
 }
 
-static bool l3gd20GyroRead(int16_t *gyroADC)
+static bool l3gd20GyroRead(gyroDev_t *gyro)
 {
     uint8_t buf[6];
 
@@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
 
     DISABLE_L3GD20;
 
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
 
 #if 0
     debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
@@ -150,7 +150,7 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
 #define L3GD20_GYRO_SCALE_FACTOR  0.07f
 
-bool l3gd20Detect(gyro_t *gyro)
+bool l3gd20Detect(gyroDev_t *gyro)
 {
     gyro->init = l3gd20GyroInit;
     gyro->read = l3gd20GyroRead;

src/main/drivers/accgyro_l3gd20.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3gd20Detect(gyro_t *gyro);
+bool l3gd20Detect(gyroDev_t *gyro);

src/main/drivers/accgyro_lsm303dlhc.c

@@ -115,7 +115,7 @@ int32_t accelSummedSamples100Hz[3];
 
 int32_t accelSummedSamples500Hz[3];
 
-void lsm303dlhcAccInit(acc_t *acc)
+void lsm303dlhcAccInit(accDev_t *acc)
 {
     i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
 
@@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
     return true;
 }
 
-bool lsm303dlhcAccDetect(acc_t *acc)
+bool lsm303dlhcAccDetect(accDev_t *acc)
 {
     bool ack;
     uint8_t status;

src/main/drivers/accgyro_lsm303dlhc.h

@@ -195,10 +195,10 @@ typedef struct {
 /** @defgroup Acc_Full_Scale_Selection
   * @{
   */
-#define LSM303DLHC_FULLSCALE_2G            ((uint8_t)0x00)  /*!< ±2 g */
+#define LSM303DLHC_FULLSCALE_2G            ((uint8_t)0x00)  /*!< +/-2 g */
-#define LSM303DLHC_FULLSCALE_4G            ((uint8_t)0x10)  /*!< ±4 g */
+#define LSM303DLHC_FULLSCALE_4G            ((uint8_t)0x10)  /*!< +/-4 g */
-#define LSM303DLHC_FULLSCALE_8G            ((uint8_t)0x20)  /*!< ±8 g */
+#define LSM303DLHC_FULLSCALE_8G            ((uint8_t)0x20)  /*!< +/-8 g */
-#define LSM303DLHC_FULLSCALE_16G           ((uint8_t)0x30)  /*!< ±16 g */
+#define LSM303DLHC_FULLSCALE_16G           ((uint8_t)0x30)  /*!< +/-16 g */
 /**
   * @}
   */
@@ -388,13 +388,13 @@ typedef struct {
 /** @defgroup Mag_Full_Scale
   * @{
   */
-#define  LSM303DLHC_FS_1_3_GA               ((uint8_t) 0x20)  /*!< Full scale = ±1.3 Gauss */
+#define  LSM303DLHC_FS_1_3_GA               ((uint8_t) 0x20)  /*!< Full scale = +/-1.3 Gauss */
-#define  LSM303DLHC_FS_1_9_GA               ((uint8_t) 0x40)  /*!< Full scale = ±1.9 Gauss */
+#define  LSM303DLHC_FS_1_9_GA               ((uint8_t) 0x40)  /*!< Full scale = +/-1.9 Gauss */
-#define  LSM303DLHC_FS_2_5_GA               ((uint8_t) 0x60)  /*!< Full scale = ±2.5 Gauss */
+#define  LSM303DLHC_FS_2_5_GA               ((uint8_t) 0x60)  /*!< Full scale = +/-2.5 Gauss */
-#define  LSM303DLHC_FS_4_0_GA               ((uint8_t) 0x80)  /*!< Full scale = ±4.0 Gauss */
+#define  LSM303DLHC_FS_4_0_GA               ((uint8_t) 0x80)  /*!< Full scale = +/-4.0 Gauss */
-#define  LSM303DLHC_FS_4_7_GA               ((uint8_t) 0xA0)  /*!< Full scale = ±4.7 Gauss */
+#define  LSM303DLHC_FS_4_7_GA               ((uint8_t) 0xA0)  /*!< Full scale = +/-4.7 Gauss */
-#define  LSM303DLHC_FS_5_6_GA               ((uint8_t) 0xC0)  /*!< Full scale = ±5.6 Gauss */
+#define  LSM303DLHC_FS_5_6_GA               ((uint8_t) 0xC0)  /*!< Full scale = +/-5.6 Gauss */
-#define  LSM303DLHC_FS_8_1_GA               ((uint8_t) 0xE0)  /*!< Full scale = ±8.1 Gauss */
+#define  LSM303DLHC_FS_8_1_GA               ((uint8_t) 0xE0)  /*!< Full scale = +/-8.1 Gauss */
 /**
   * @}
   */
@@ -438,5 +438,5 @@ typedef struct {
 #define LSM303DLHC_TEMPSENSOR_DISABLE        ((uint8_t) 0x00)   /*!< Temp sensor Disable */
 
 
-bool lsm303dlhcAccDetect(acc_t *acc);
+bool lsm303dlhcAccDetect(accDev_t *acc);
 

src/main/drivers/accgyro_mma845x.c

@@ -81,10 +81,10 @@
 
 static uint8_t device_id;
 
-static void mma8452Init(acc_t *acc);
+static void mma8452Init(accDev_t *acc);
 static bool mma8452Read(int16_t *accelData);
 
-bool mma8452Detect(acc_t *acc)
+bool mma8452Detect(accDev_t *acc)
 {
     uint8_t sig = 0;
     bool ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
@@ -113,7 +113,7 @@ static inline void mma8451ConfigureInterrupt(void)
     i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
 }
 
-static void mma8452Init(acc_t *acc)
+static void mma8452Init(accDev_t *acc)
 {
 
     i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff

src/main/drivers/accgyro_mma845x.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool mma8452Detect(acc_t *acc);
+bool mma8452Detect(accDev_t *acc);

src/main/drivers/accgyro_mpu.c

@@ -25,7 +25,9 @@
 #include "build/build_config.h"
 #include "build/debug.h"
 
+#include "common/filter.h"
 #include "common/maths.h"
+#include "common/utils.h"
 
 #include "nvic.h"
 
@@ -52,8 +54,6 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
 
 static void mpu6050FindRevision(void);
 
-static volatile bool mpuDataReady;
-
 #ifdef USE_SPI
 static bool detectSPISensorsAndUpdateDetectionResult(void);
 #endif
@@ -221,12 +221,22 @@ static void mpu6050FindRevision(void)
     }
 }
 
-extiCallbackRec_t mpuIntCallbackRec;
+typedef struct mpuIntRec_s {
+   extiCallbackRec_t exti;
+   gyroDev_t *gyro;
+} mpuIntRec_t;
 
-void mpuIntExtiHandler(extiCallbackRec_t *cb)
+mpuIntRec_t mpuIntRec;
+
+/*
+ * Gyro interrupt service routine
+ */
+#if defined(MPU_INT_EXTI)
+static void mpuIntExtiHandler(extiCallbackRec_t *cb)
 {
-    UNUSED(cb);
+    mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
-    mpuDataReady = true;
+    gyroDev_t *gyro = rec->gyro;
+    gyro->dataReady = true;
 
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
     static uint32_t lastCalledAt = 0;
@@ -236,17 +246,18 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
     lastCalledAt = now;
 #endif
 }
+#endif
 
-void mpuIntExtiInit(void)
+static void mpuIntExtiInit(gyroDev_t *gyro)
 {
+    mpuIntRec.gyro = gyro;
+#if defined(MPU_INT_EXTI)
     static bool mpuExtiInitDone = false;
 
     if (mpuExtiInitDone || !mpuIntExtiConfig) {
         return;
     }
 
-#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
-
     IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
 
 #ifdef ENSURE_MPU_DATA_READY_IS_LOW
@@ -258,20 +269,20 @@ void mpuIntExtiInit(void)
 
 #if defined (STM32F7)
     IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
-    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
+    EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
-    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));   // TODO - maybe pullup / pulldown ?
+    EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));   // TODO - maybe pullup / pulldown ?
 #else
 
     IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
     IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING);   // TODO - maybe pullup / pulldown ?
 
-    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
+    EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
-    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
+    EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
     EXTIEnable(mpuIntIO, true);
-#endif
 #endif
 
     mpuExtiInitDone = true;
+#endif
 }
 
 static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
@@ -302,28 +313,33 @@ bool mpuAccRead(int16_t *accData)
     return true;
 }
 
-bool mpuGyroRead(int16_t *gyroADC)
+bool mpuGyroRead(gyroDev_t *gyro)
 {
     uint8_t data[6];
 
-    bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
+    const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
     if (!ack) {
         return false;
     }
 
-    gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
+    gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
-    gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
+    gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
-    gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
+    gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }
 
-bool checkMPUDataReady(void)
+void mpuGyroInit(gyroDev_t *gyro)
+{
+    mpuIntExtiInit(gyro);
+}
+
+bool checkMPUDataReady(gyroDev_t* gyro)
 {
     bool ret;
-    if (mpuDataReady) {
+    if (gyro->dataReady) {
         ret = true;
-        mpuDataReady= false;
+        gyro->dataReady= false;
     } else {
         ret = false;
     }

src/main/drivers/accgyro_mpu.h

@@ -17,7 +17,6 @@
 
 #pragma once
 
-#include "io_types.h"
 #include "exti.h"
 
 // MPU6050
@@ -185,8 +184,9 @@ typedef struct mpuDetectionResult_s {
 extern mpuDetectionResult_t mpuDetectionResult;
 
 void configureMPUDataReadyInterruptHandling(void);
-void mpuIntExtiInit(void);
+struct gyroDev_s;
+void mpuGyroInit(struct gyroDev_s *gyro);
 bool mpuAccRead(int16_t *accData);
-bool mpuGyroRead(int16_t *gyroADC);
+bool mpuGyroRead(struct gyroDev_s *gyro);
 mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
-bool checkMPUDataReady(void);
+bool checkMPUDataReady(struct gyroDev_s *gyro);

src/main/drivers/accgyro_mpu3050.c

@@ -46,10 +46,10 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-static void mpu3050Init(uint8_t lpf);
+static void mpu3050Init(gyroDev_t *gyro);
 static bool mpu3050ReadTemp(int16_t *tempData);
 
-bool mpu3050Detect(gyro_t *gyro)
+bool mpu3050Detect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_3050) {
         return false;
@@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
     return true;
 }
 
-static void mpu3050Init(uint8_t lpf)
+static void mpu3050Init(gyroDev_t *gyro)
 {
     bool ack;
 
@@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf)
     if (!ack)
         failureMode(FAILURE_ACC_INIT);
 
-    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
+    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
     mpuConfiguration.write(MPU3050_INT_CFG, 0);
     mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
     mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

src/main/drivers/accgyro_mpu3050.h

@@ -26,4 +26,4 @@
 #define MPU3050_USER_CTRL       0x3D
 #define MPU3050_PWR_MGM         0x3E
 
-bool mpu3050Detect(gyro_t *gyro);
+bool mpu3050Detect(gyroDev_t *gyro);

src/main/drivers/accgyro_mpu6050.c

@@ -50,10 +50,10 @@
 
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
-static void mpu6050AccInit(acc_t *acc);
+static void mpu6050AccInit(accDev_t *acc);
-static void mpu6050GyroInit(uint8_t lpf);
+static void mpu6050GyroInit(gyroDev_t *gyro);
 
-bool mpu6050AccDetect(acc_t *acc)
+bool mpu6050AccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
@@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6050GyroDetect(gyro_t *gyro)
+bool mpu6050GyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
@@ -81,10 +81,8 @@ bool mpu6050GyroDetect(gyro_t *gyro)
     return true;
 }
 
-static void mpu6050AccInit(acc_t *acc)
+static void mpu6050AccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     switch (mpuDetectionResult.resolution) {
         case MPU_HALF_RESOLUTION:
             acc->acc_1G = 256 * 4;
@@ -95,16 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
     }
 }
 
-static void mpu6050GyroInit(uint8_t lpf)
+static void mpu6050GyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
     // ACC Init stuff.

src/main/drivers/accgyro_mpu6050.h

@@ -17,5 +17,5 @@
 
 #pragma once
 
-bool mpu6050AccDetect(acc_t *acc);
+bool mpu6050AccDetect(accDev_t *acc);
-bool mpu6050GyroDetect(gyro_t *gyro);
+bool mpu6050GyroDetect(gyroDev_t *gyro);

src/main/drivers/accgyro_mpu6500.c

@@ -34,7 +34,7 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu6500.h"
 
-bool mpu6500AccDetect(acc_t *acc)
+bool mpu6500AccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
         return false;
@@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6500GyroDetect(gyro_t *gyro)
+bool mpu6500GyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
         return false;
@@ -62,16 +62,14 @@ bool mpu6500GyroDetect(gyro_t *gyro)
     return true;
 }
 
-void mpu6500AccInit(acc_t *acc)
+void mpu6500AccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 
-void mpu6500GyroInit(uint8_t lpf)
+void mpu6500GyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
@@ -85,7 +83,7 @@ void mpu6500GyroInit(uint8_t lpf)
     delay(15);
     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf);
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro_mpu6500.h

@@ -28,8 +28,8 @@
 #define MPU6500_BIT_I2C_IF_DIS              (1 << 4)
 #define MPU6500_BIT_RAW_RDY_EN              (0x01)
 
-bool mpu6500AccDetect(acc_t *acc);
+bool mpu6500AccDetect(accDev_t *acc);
-bool mpu6500GyroDetect(gyro_t *gyro);
+bool mpu6500GyroDetect(gyroDev_t *gyro);
 
-void mpu6500AccInit(acc_t *acc);
+void mpu6500AccInit(accDev_t *acc);
-void mpu6500GyroInit(uint8_t lpf);
+void mpu6500GyroInit(gyroDev_t *gyro);

src/main/drivers/accgyro_spi_icm20689.c

@@ -107,7 +107,7 @@ bool icm20689SpiDetect(void)
 
 }
 
-bool icm20689SpiAccDetect(acc_t *acc)
+bool icm20689SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != ICM_20689_SPI) {
         return false;
@@ -119,7 +119,7 @@ bool icm20689SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool icm20689SpiGyroDetect(gyro_t *gyro)
+bool icm20689SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != ICM_20689_SPI) {
         return false;
@@ -135,16 +135,14 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
     return true;
 }
 
-void icm20689AccInit(acc_t *acc)
+void icm20689AccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 
-void icm20689GyroInit(uint8_t lpf)
+void icm20689GyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
@@ -160,7 +158,7 @@ void icm20689GyroInit(uint8_t lpf)
     delay(15);
     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf);
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro_spi_icm20689.h

@@ -19,16 +19,16 @@
 #define ICM20689_WHO_AM_I_CONST             (0x98)
 #define ICM20689_BIT_RESET                  (0x80)
 
-bool icm20689AccDetect(acc_t *acc);
+bool icm20689AccDetect(accDev_t *acc);
-bool icm20689GyroDetect(gyro_t *gyro);
+bool icm20689GyroDetect(gyroDev_t *gyro);
 
-void icm20689AccInit(acc_t *acc);
+void icm20689AccInit(accDev_t *acc);
-void icm20689GyroInit(uint8_t lpf);
+void icm20689GyroInit(gyroDev_t *gyro);
 
 bool icm20689SpiDetect(void);
 
-bool icm20689SpiAccDetect(acc_t *acc);
+bool icm20689SpiAccDetect(accDev_t *acc);
-bool icm20689SpiGyroDetect(gyro_t *gyro);
+bool icm20689SpiGyroDetect(gyroDev_t *gyro);
 
 bool icm20689WriteRegister(uint8_t reg, uint8_t data);
-bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
+bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro_spi_mpu6000.c

@@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu6000SpiGyroInit(uint8_t lpf)
+void mpu6000SpiGyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpu6000AccAndGyroInit();
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     // Accel and Gyro DLPF Setting
-    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
+    mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
     delayMicroseconds(1);
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
 
-    int16_t data[3];
+    mpuGyroRead(gyro);
-    mpuGyroRead(data);
 
-    if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
+    if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
         failureMode(FAILURE_GYRO_INIT_FAILED);
     }
 }
 
-void mpu6000SpiAccInit(acc_t *acc)
+void mpu6000SpiAccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 
@@ -257,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
     mpuSpi6000InitDone = true;
 }
 
-bool mpu6000SpiAccDetect(acc_t *acc)
+bool mpu6000SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;
@@ -269,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6000SpiGyroDetect(gyro_t *gyro)
+bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu6000.h

@@ -17,8 +17,8 @@
 
 bool mpu6000SpiDetect(void);
 
-bool mpu6000SpiAccDetect(acc_t *acc);
+bool mpu6000SpiAccDetect(accDev_t *acc);
-bool mpu6000SpiGyroDetect(gyro_t *gyro);
+bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro_spi_mpu6500.c

@@ -94,17 +94,17 @@ bool mpu6500SpiDetect(void)
     return false;
 }
 
-void mpu6500SpiAccInit(acc_t *acc)
+void mpu6500SpiAccInit(accDev_t *acc)
 {
     mpu6500AccInit(acc);
 }
 
-void mpu6500SpiGyroInit(uint8_t lpf)
+void mpu6500SpiGyroInit(gyroDev_t *gyro)
 {
     spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
     delayMicroseconds(1);
 
-    mpu6500GyroInit(lpf);
+    mpu6500GyroInit(gyro);
 
     // Disable Primary I2C Interface
     mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
@@ -114,7 +114,7 @@ void mpu6500SpiGyroInit(uint8_t lpf)
     delayMicroseconds(1);
 }
 
-bool mpu6500SpiAccDetect(acc_t *acc)
+bool mpu6500SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
@@ -126,7 +126,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6500SpiGyroDetect(gyro_t *gyro)
+bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu6500.h

@@ -19,11 +19,11 @@
 
 bool mpu6500SpiDetect(void);
 
-void mpu6500SpiAccInit(acc_t *acc);
+void mpu6500SpiAccInit(accDev_t *acc);
-void mpu6500SpiGyroInit(uint8_t lpf);
+void mpu6500SpiGyroInit(gyroDev_t *gyro);
 
-bool mpu6500SpiAccDetect(acc_t *acc);
+bool mpu6500SpiAccDetect(accDev_t *acc);
-bool mpu6500SpiGyroDetect(gyro_t *gyro);
+bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro_spi_mpu9250.c

@@ -96,31 +96,26 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu9250SpiGyroInit(uint8_t lpf)
+void mpu9250SpiGyroInit(gyroDev_t *gyro)
 {
-    (void)(lpf);
+    mpuGyroInit(gyro);
 
-    mpuIntExtiInit();
+    mpu9250AccAndGyroInit(gyro->lpf);
-
-    mpu9250AccAndGyroInit(lpf);
 
     spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 
     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
 
-    int16_t data[3];
+    mpuGyroRead(gyro);
-    mpuGyroRead(data);
 
-    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
+    if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
         spiResetErrorCounter(MPU9250_SPI_INSTANCE);
         failureMode(FAILURE_GYRO_INIT_FAILED);
     }
 }
 
-void mpu9250SpiAccInit(acc_t *acc)
+void mpu9250SpiAccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 8;
 }
 
@@ -214,7 +209,7 @@ bool mpu9250SpiDetect(void)
     return true;
 }
 
-bool mpu9250SpiAccDetect(acc_t *acc)
+bool mpu9250SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_9250_SPI) {
         return false;
@@ -226,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu9250SpiGyroDetect(gyro_t *gyro)
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_9250_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu9250.h

@@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
 
 bool mpu9250SpiDetect(void);
 
-bool mpu9250SpiAccDetect(acc_t *acc);
+bool mpu9250SpiAccDetect(accDev_t *acc);
-bool mpu9250SpiGyroDetect(gyro_t *gyro);
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
 bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);

src/main/drivers/adc.h

@@ -21,10 +21,10 @@
 
 typedef enum {
     ADC_BATTERY = 0,
-    ADC_RSSI = 1,
+    ADC_CURRENT = 1,
     ADC_EXTERNAL1 = 2,
-    ADC_CURRENT = 3,
+    ADC_RSSI = 3,
-    ADC_CHANNEL_MAX = ADC_CURRENT
+    ADC_CHANNEL_MAX = ADC_RSSI
 } AdcChannel;
 
 #define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1)

src/main/drivers/adc_stm32f10x.c

@@ -119,7 +119,7 @@ void adcInit(adcConfig_t *config)
         adcOperatingConfig[i].sampleTime = ADC_SampleTime_239Cycles5;
         adcOperatingConfig[i].enabled = true;
     }
-    
+
     if (!adcActive) {
         return;
     }

src/main/drivers/adc_stm32f30x.c

@@ -128,7 +128,7 @@ void adcInit(adcConfig_t *config)
     if (config->currentMeter.enabled) {
         adcOperatingConfig[ADC_CURRENT].tag = config->currentMeter.ioTag;  //CURRENT_METER_ADC_CHANNEL;
     }
-    
+
     ADCDevice device = adcDeviceByInstance(ADC_INSTANCE);
     if (device == ADCINVALID)
         return;
@@ -155,7 +155,7 @@ void adcInit(adcConfig_t *config)
     if (!adcActive) {
         return;
     }
-    
+
     if ((device == ADCDEV_1) || (device == ADCDEV_2)) {
         // enable clock for ADC1+2
         RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256);  // 72 MHz divided by 256 = 281.25 kHz

src/main/drivers/adc_stm32f4xx.c

@@ -136,7 +136,7 @@ void adcInit(adcConfig_t *config)
     if (!adcActive) {
         return;
     }
-    
+
     RCC_ClockCmd(adc.rccADC, ENABLE);
 
     dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0);

src/main/drivers/barometer.h

@@ -20,7 +20,7 @@
 typedef void (*baroOpFuncPtr)(void);                       // baro start operation
 typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
 
-typedef struct baro_s {
+typedef struct baroDev_s {
     uint16_t ut_delay;
     uint16_t up_delay;
     baroOpFuncPtr start_ut;
@@ -28,7 +28,7 @@ typedef struct baro_s {
     baroOpFuncPtr start_up;
     baroOpFuncPtr get_up;
     baroCalculateFuncPtr calculate;
-} baro_t;
+} baroDev_t;
 
 #ifndef BARO_I2C_INSTANCE
 #define BARO_I2C_INSTANCE I2C_DEVICE

src/main/drivers/barometer_bmp085.c

@@ -154,7 +154,7 @@ void bmp085Disable(const bmp085Config_t *config)
     BMP085_OFF;
 }
 
-bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
+bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
 {
     uint8_t data;
     bool ack;

src/main/drivers/barometer_bmp085.h

@@ -22,7 +22,7 @@ typedef struct bmp085Config_s {
     ioTag_t eocIO;
 } bmp085Config_t;
 
-bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
+bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro);
 void bmp085Disable(const bmp085Config_t *config);
 
 #if defined(BARO_EOC_GPIO)

src/main/drivers/barometer_bmp280.c

@@ -65,7 +65,7 @@ static void bmp280_get_up(void);
 #endif
 STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
 
-bool bmp280Detect(baro_t *baro)
+bool bmp280Detect(baroDev_t *baro)
 {
     if (bmp280InitDone)
         return true;

src/main/drivers/barometer_bmp280.h

@@ -56,5 +56,5 @@
 #define T_SETUP_PRESSURE_MAX             (10)
 // 10/16 = 0.625 ms
 
-bool bmp280Detect(baro_t *baro);
+bool bmp280Detect(baroDev_t *baro);
 

src/main/drivers/barometer_ms5611.c

@@ -59,7 +59,7 @@ STATIC_UNIT_TESTED uint32_t ms5611_up;  // static result of pressure measurement
 STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB];  // on-chip ROM
 static uint8_t ms5611_osr = CMD_ADC_4096;
 
-bool ms5611Detect(baro_t *baro)
+bool ms5611Detect(baroDev_t *baro)
 {
     uint8_t sig;
     int i;

src/main/drivers/barometer_ms5611.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool ms5611Detect(baro_t *baro);
+bool ms5611Detect(baroDev_t *baro);

src/main/drivers/bus_spi.c

@@ -126,6 +126,7 @@ void spiInitDevice(SPIDevice device)
     IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
 
     if (spi->nss) {
+        IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
         IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
     }
 #endif
@@ -135,6 +136,7 @@ void spiInitDevice(SPIDevice device)
     IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
 
     if (spi->nss) {
+        IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
         IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
     }
 #endif

src/main/drivers/compass.h

@@ -19,10 +19,10 @@
 
 #include "sensor.h"
 
-typedef struct mag_s {
+typedef struct magDev_s {
     sensorInitFuncPtr init;                                 // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
-} mag_t;
+} magDev_t;
 
 #ifndef MAG_I2C_INSTANCE
 #define MAG_I2C_INSTANCE I2C_DEVICE

src/main/drivers/compass_ak8963.c

@@ -188,7 +188,7 @@ bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
 }
 #endif
 
-bool ak8963Detect(mag_t *mag)
+bool ak8963Detect(magDev_t *mag)
 {
     uint8_t sig = 0;
 

src/main/drivers/compass_ak8963.h

@@ -17,6 +17,6 @@
 
 #pragma once
 
-bool ak8963Detect(mag_t *mag);
+bool ak8963Detect(magDev_t *mag);
 void ak8963Init(void);
 bool ak8963Read(int16_t *magData);

src/main/drivers/compass_ak8975.c

@@ -57,7 +57,7 @@
 #define AK8975_MAG_REG_CNTL         0x0a
 #define AK8975_MAG_REG_ASCT         0x0c // self test
 
-bool ak8975Detect(mag_t *mag)
+bool ak8975Detect(magDev_t *mag)
 {
     uint8_t sig = 0;
     bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);

src/main/drivers/compass_ak8975.h

@@ -17,6 +17,6 @@
 
 #pragma once
 
-bool ak8975Detect(mag_t *mag);
+bool ak8975Detect(magDev_t *mag);
 void ak8975Init(void);
 bool ak8975Read(int16_t *magData);

src/main/drivers/compass_hmc5883l.c

@@ -167,13 +167,13 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
 #endif
 }
 
-bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
+bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
 {
     hmc5883Config = hmc5883ConfigToUse;
 
     uint8_t sig = 0;
     bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
-    
+
     if (!ack || sig != 'H')
         return false;
 

src/main/drivers/compass_hmc5883l.h

@@ -23,6 +23,6 @@ typedef struct hmc5883Config_s {
     ioTag_t intTag;
 } hmc5883Config_t;
 
-bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
+bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
 void hmc5883lInit(void);
 bool hmc5883lRead(int16_t *magData);

src/main/drivers/gyro_sync.c

@@ -16,11 +16,11 @@
 
 static uint8_t mpuDividerDrops;
 
-bool gyroSyncCheckUpdate(const gyro_t *gyro)
+bool gyroSyncCheckUpdate(gyroDev_t *gyro)
 {
     if (!gyro->intStatus)
         return false;
-    return gyro->intStatus();
+    return gyro->intStatus(gyro);
 }
 
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)

src/main/drivers/gyro_sync.h

@@ -5,7 +5,8 @@
  *      Author: borisb
  */
 
-struct gyro_s;
+struct gyroDev_s;
-bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
+
+bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
 uint8_t gyroMPU6xxxGetDividerDrops(void);
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

src/main/drivers/pwm_output.c

@@ -64,9 +64,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
     configTimeBase(timerHardware->tim, period, mhz);
     pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
 
-    if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
+    TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
-        TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
-    }
     TIM_Cmd(timerHardware->tim, ENABLE);
 
     port->ccr = timerChCCR(timerHardware);

src/main/drivers/pwm_output_hal.c

@@ -63,11 +63,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
     configTimeBase(timerHardware->tim, period, mhz);
     pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
 
-    if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
+    HAL_TIM_PWM_Start(Handle, timerHardware->channel);
-        HAL_TIM_PWM_Start(Handle, timerHardware->channel);
-    } else {
-        HAL_TIM_PWM_Stop(Handle, timerHardware->channel);
-    }
     HAL_TIM_Base_Start(Handle);
 
     switch (timerHardware->channel) {
@@ -221,7 +217,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
             break;
         }
 
-        const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
+        const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
 
         if (timerHardware == NULL) {
             /* flag failure and disable ability to arm */
@@ -280,7 +276,7 @@ void servoInit(const servoConfig_t *servoConfig)
         IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
         //IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
 
-        const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
+        const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
         IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
 
         if (timer == NULL) {

src/main/drivers/pwm_output_stm32f3xx.c

@@ -74,7 +74,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
     if (!motor->timerHardware->dmaChannel) {
         return;
     }
-    
+
     uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
     motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 

src/main/drivers/pwm_output_stm32f4xx.c

@@ -72,7 +72,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
     if (!motor->timerHardware->dmaStream) {
         return;
     }
-    
+
     uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
     motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 

src/main/drivers/sensor.h

@@ -17,9 +17,12 @@
 
 #pragma once
 
-struct acc_s;
+struct accDev_s;
 typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
 typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
-typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc);    // sensor init prototype
+typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc);    // sensor init prototype
-typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf);         // gyro sensor init prototype
+struct gyroDev_s;
-typedef bool (*sensorInterruptFuncPtr)(void);               // sensor Interrupt Data Ready
+typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorInterruptFuncPtr)(void);

src/main/drivers/serial_escserial.c

@@ -119,7 +119,7 @@ static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polari
 
 void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
 {
-    if((escSerial->mode == PROTOCOL_KISSALL))
+    if(escSerial->mode == PROTOCOL_KISSALL)
     {
         for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) {
             uint8_t state_temp = state;

src/main/drivers/serial_uart_stm32f7xx.c

@@ -415,7 +415,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
             IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
         }
 
-        if (mode & MODE_RX) { 
+        if (mode & MODE_RX) {
             IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
             IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
         }

src/main/drivers/stack_check.c

@@ -0,0 +1,99 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/utils.h"
+
+#include "drivers/stack_check.h"
+
+#define STACK_FILL_CHAR 0xa5
+
+extern char _estack; // end of stack, declared in .LD file
+extern char _Min_Stack_Size; // declared in .LD file
+
+/*
+ * The ARM processor uses a full descending stack. This means the stack pointer holds the address
+ * of the last stacked item in memory. When the processor pushes a new item onto the stack,
+ * it decrements the stack pointer and then writes the item to the new memory location.
+ *
+ *
+ * RAM layout is generally as below, although some targets vary
+ *
+ * F1 Boards
+ * RAM is origin 0x20000000 length 20K that is:
+ * 0x20000000 to 0x20005000
+ *
+ * F3 Boards
+ * RAM is origin 0x20000000 length 40K that is:
+ * 0x20000000 to 0x2000a000
+ *
+ * F4 Boards
+ * RAM is origin 0x20000000 length 128K that is:
+ * 0x20000000 to 0x20020000
+ *
+ */
+
+#ifdef STACK_CHECK
+
+static uint32_t usedStackSize;
+
+void taskStackCheck(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    char * const stackHighMem = &_estack;
+    const uint32_t stackSize = (uint32_t)&_Min_Stack_Size;
+    char * const stackLowMem = stackHighMem - stackSize;
+    const char * const stackCurrent = (char *)&stackLowMem;
+
+    char *p;
+    for (p = stackLowMem; p < stackCurrent; ++p) {
+        if (*p != STACK_FILL_CHAR) {
+            break;
+        }
+    }
+
+    usedStackSize = (uint32_t)stackHighMem - (uint32_t)p;
+
+    DEBUG_SET(DEBUG_STACK, 0, (uint32_t)stackHighMem & 0xffff);
+    DEBUG_SET(DEBUG_STACK, 1, (uint32_t)stackLowMem & 0xffff);
+    DEBUG_SET(DEBUG_STACK, 2, (uint32_t)stackCurrent & 0xffff);
+    DEBUG_SET(DEBUG_STACK, 3, (uint32_t)p & 0xffff);
+}
+
+uint32_t stackUsedSize(void)
+{
+    return usedStackSize;
+}
+#endif
+
+uint32_t stackTotalSize(void)
+{
+    return (uint32_t)&_Min_Stack_Size;
+}
+
+uint32_t stackHighMem(void)
+{
+    return (uint32_t)&_estack;
+}

src/main/drivers/stack_check.h

@@ -0,0 +1,25 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void taskStackCheck(timeUs_t currentTimeUs);
+uint32_t stackUsedSize(void);
+uint32_t stackTotalSize(void);
+uint32_t stackHighMem(void);

src/main/drivers/timer.h

@@ -108,8 +108,9 @@ typedef struct timerHardware_s {
 
 typedef enum {
     TIMER_OUTPUT_NONE      = 0x00,
-    TIMER_INPUT_ENABLED    = 0x00,
+    TIMER_INPUT_ENABLED    = 0x01, /* TODO: remove this */
-    TIMER_OUTPUT_ENABLED   = 0x01,
+    TIMER_OUTPUT_ENABLED   = 0x01, /* TODO: remove this */
+    TIMER_OUTPUT_STANDARD  = 0x01,
     TIMER_OUTPUT_INVERTED  = 0x02,
     TIMER_OUTPUT_N_CHANNEL = 0x04
 } timerFlag_e;

src/main/drivers/timer_stm32f10x.c

@@ -1,12 +1,19 @@
 /*
-  modified version of StdPeriph function is located here.
+ * This file is part of Cleanflight.
-  TODO - what license does apply here?
+ *
-  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+ * Cleanflight is free software: you can redistribute it and/or modify
-  http://www.st.com/software_license_agreement_liberty_v2
+ * it under the terms of the GNU General Public License as published by
-*/
+ * the Free Software Foundation, either version 3 of the License, or
-
+ * (at your option) any later version.
-#include <stdbool.h>
+ *
-#include <stdint.h>
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #include "platform.h"
 
@@ -42,59 +49,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
     UNUSED(tim);
     return 1;
 }
-
-/**
-  * @brief  Selects the TIM Output Compare Mode.
-  * @note   This function does NOT disable the selected channel before changing the Output
-  *         Compare Mode.
-  * @param  TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
-  * @param  TIM_Channel: specifies the TIM Channel
-  *   This parameter can be one of the following values:
-  *     @arg TIM_Channel_1: TIM Channel 1
-  *     @arg TIM_Channel_2: TIM Channel 2
-  *     @arg TIM_Channel_3: TIM Channel 3
-  *     @arg TIM_Channel_4: TIM Channel 4
-  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
-  *   This parameter can be one of the following values:
-  *     @arg TIM_OCMode_Timing
-  *     @arg TIM_OCMode_Active
-  *     @arg TIM_OCMode_Toggle
-  *     @arg TIM_OCMode_PWM1
-  *     @arg TIM_OCMode_PWM2
-  *     @arg TIM_ForcedAction_Active
-  *     @arg TIM_ForcedAction_InActive
-  * @retval None
-  */
-
-#define CCMR_Offset                 ((uint16_t)0x0018)
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
-{
-    uint32_t tmp = 0;
-
-    /* Check the parameters */
-    assert_param(IS_TIM_LIST8_PERIPH(TIMx));
-    assert_param(IS_TIM_CHANNEL(TIM_Channel));
-    assert_param(IS_TIM_OCM(TIM_OCMode));
-
-    tmp = (uint32_t) TIMx;
-    tmp += CCMR_Offset;
-
-    if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
-        tmp += (TIM_Channel>>1);
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= TIM_OCMode;
-    } else {
-        tmp += (uint16_t)(TIM_Channel - (uint16_t)4) >> (uint16_t)1;
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
-    }
-}

src/main/drivers/timer_stm32f10x.h

@@ -1,6 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #pragma once
 
 #include "stm32f10x.h"
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

src/main/drivers/timer_stm32f30x.c

@@ -1,12 +1,19 @@
 /*
-  modified version of StdPeriph function is located here.
+ * This file is part of Cleanflight.
-  TODO - what license does apply here?
+ *
-  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+ * Cleanflight is free software: you can redistribute it and/or modify
-  http://www.st.com/software_license_agreement_liberty_v2
+ * it under the terms of the GNU General Public License as published by
-*/
+ * the Free Software Foundation, either version 3 of the License, or
-
+ * (at your option) any later version.
-#include <stdbool.h>
+ *
-#include <stdint.h>
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #include "platform.h"
 
@@ -34,67 +41,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
     UNUSED(tim);
     return 1;
 }
-
-/**
-  * @brief  Selects the TIM Output Compare Mode.
-  * @note   This function does NOT disable the selected channel before changing the Output
-  *         Compare Mode. If needed, user has to enable this channel using
-  *         TIM_CCxCmd() and TIM_CCxNCmd() functions.
-  * @param  TIMx: where x can be 1, 2, 3, 4, 8, 15, 16 or 17 to select the TIM peripheral.
-  * @param  TIM_Channel: specifies the TIM Channel
-  *          This parameter can be one of the following values:
-  *            @arg TIM_Channel_1: TIM Channel 1
-  *            @arg TIM_Channel_2: TIM Channel 2
-  *            @arg TIM_Channel_3: TIM Channel 3
-  *            @arg TIM_Channel_4: TIM Channel 4
-  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
-  *           This parameter can be one of the following values:
-  *            @arg TIM_OCMode_Timing
-  *            @arg TIM_OCMode_Active
-  *            @arg TIM_OCMode_Toggle
-  *            @arg TIM_OCMode_PWM1
-  *            @arg TIM_OCMode_PWM2
-  *            @arg TIM_ForcedAction_Active
-  *            @arg TIM_ForcedAction_InActive
-  *            @arg TIM_OCMode_Retrigerrable_OPM1
-  *            @arg TIM_OCMode_Retrigerrable_OPM2
-  *            @arg TIM_OCMode_Combined_PWM1
-  *            @arg TIM_OCMode_Combined_PWM2
-  *            @arg TIM_OCMode_Asymmetric_PWM1
-  *            @arg TIM_OCMode_Asymmetric_PWM2
-  * @retval None
-  */
-#define CCMR_OFFSET        ((uint16_t)0x0018)
-#define CCMR_OC13M_MASK    ((uint32_t)0xFFFEFF8F)
-#define CCMR_OC24M_MASK    ((uint32_t)0xFEFF8FFF)
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode)
-{
-    uint32_t tmp = 0;
-
-    /* Check the parameters */
-    assert_param(IS_TIM_LIST1_PERIPH(TIMx));
-    assert_param(IS_TIM_CHANNEL(TIM_Channel));
-    assert_param(IS_TIM_OCM(TIM_OCMode));
-
-    tmp = (uint32_t) TIMx;
-    tmp += CCMR_OFFSET;
-
-    if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) {
-        tmp += (TIM_Channel>>1);
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK;
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= TIM_OCMode;
-    } else {
-        tmp += (uint32_t)(TIM_Channel - (uint32_t)4) >> (uint32_t)1;
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK;
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8);
-    }
-}

src/main/drivers/timer_stm32f30x.h

@@ -1,6 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #pragma once
 
 #include "stm32f30x.h"
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

src/main/drivers/timer_stm32f4xx.c

@@ -1,12 +1,19 @@
 /*
-  modified version of StdPeriph function is located here.
+ * This file is part of Cleanflight.
-  TODO - what license does apply here?
+ *
-  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+ * Cleanflight is free software: you can redistribute it and/or modify
-  http://www.st.com/software_license_agreement_liberty_v2
+ * it under the terms of the GNU General Public License as published by
-*/
+ * the Free Software Foundation, either version 3 of the License, or
-
+ * (at your option) any later version.
-#include <stdbool.h>
+ *
-#include <stdint.h>
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #include "platform.h"
 
@@ -16,31 +23,6 @@
 #include "rcc.h"
 #include "timer.h"
 
-/**
-  * @brief  Selects the TIM Output Compare Mode.
-  * @note   This function does NOT disable the selected channel before changing the Output
-  *         Compare Mode.
-  * @param  TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
-  * @param  TIM_Channel: specifies the TIM Channel
-  *   This parameter can be one of the following values:
-  *     @arg TIM_Channel_1: TIM Channel 1
-  *     @arg TIM_Channel_2: TIM Channel 2
-  *     @arg TIM_Channel_3: TIM Channel 3
-  *     @arg TIM_Channel_4: TIM Channel 4
-  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
-  *   This parameter can be one of the following values:
-  *     @arg TIM_OCMode_Timing
-  *     @arg TIM_OCMode_Active
-  *     @arg TIM_OCMode_Toggle
-  *     @arg TIM_OCMode_PWM1
-  *     @arg TIM_OCMode_PWM2
-  *     @arg TIM_ForcedAction_Active
-  *     @arg TIM_ForcedAction_InActive
-  * @retval None
-  */
-
-#define CCMR_Offset                 ((uint16_t)0x0018)
-
 const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     { .TIMx = TIM1,  .rcc = RCC_APB2(TIM1),  .inputIrq = TIM1_CC_IRQn},
     { .TIMx = TIM2,  .rcc = RCC_APB1(TIM2),  .inputIrq = TIM2_IRQn},
@@ -109,34 +91,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
         return 2;
     }
 }
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
-{
-    uint32_t tmp = 0;
-
-    /* Check the parameters */
-    assert_param(IS_TIM_LIST8_PERIPH(TIMx));
-    assert_param(IS_TIM_CHANNEL(TIM_Channel));
-    assert_param(IS_TIM_OCM(TIM_OCMode));
-
-    tmp = (uint32_t) TIMx;
-    tmp += CCMR_Offset;
-
-    if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
-        tmp += (TIM_Channel>>1);
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= TIM_OCMode;
-    } else {
-        tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
-    }
-}

src/main/drivers/timer_stm32f4xx.h

@@ -1,6 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #pragma once
 
 #include "stm32f4xx.h"
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

src/main/drivers/timer_stm32f7xx.c

@@ -1,12 +1,19 @@
 /*
-  modified version of StdPeriph function is located here.
+ * This file is part of Cleanflight.
-  TODO - what license does apply here?
+ *
-  original file was lincesed under MCD-ST Liberty SW License Agreement V2
+ * Cleanflight is free software: you can redistribute it and/or modify
-  http://www.st.com/software_license_agreement_liberty_v2
+ * it under the terms of the GNU General Public License as published by
-*/
+ * the Free Software Foundation, either version 3 of the License, or
-
+ * (at your option) any later version.
-#include <stdbool.h>
+ *
-#include <stdint.h>
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #include "platform.h"
 
@@ -16,31 +23,6 @@
 #include "rcc.h"
 #include "timer.h"
 
-/**
-  * @brief  Selects the TIM Output Compare Mode.
-  * @note   This function does NOT disable the selected channel before changing the Output
-  *         Compare Mode.
-  * @param  TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
-  * @param  TIM_Channel: specifies the TIM Channel
-  *   This parameter can be one of the following values:
-  *     @arg TIM_Channel_1: TIM Channel 1
-  *     @arg TIM_Channel_2: TIM Channel 2
-  *     @arg TIM_Channel_3: TIM Channel 3
-  *     @arg TIM_Channel_4: TIM Channel 4
-  * @param  TIM_OCMode: specifies the TIM Output Compare Mode.
-  *   This parameter can be one of the following values:
-  *     @arg TIM_OCMode_Timing
-  *     @arg TIM_OCMode_Active
-  *     @arg TIM_OCMode_Toggle
-  *     @arg TIM_OCMode_PWM1
-  *     @arg TIM_OCMode_PWM2
-  *     @arg TIM_ForcedAction_Active
-  *     @arg TIM_ForcedAction_InActive
-  * @retval None
-  */
-
-#define CCMR_Offset                 ((uint16_t)0x0018)
-
 const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     { .TIMx = TIM1,  .rcc = RCC_APB2(TIM1),  .inputIrq = TIM1_CC_IRQn},
     { .TIMx = TIM2,  .rcc = RCC_APB1(TIM2),  .inputIrq = TIM2_IRQn},
@@ -93,41 +75,8 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     6                   TIM1_CH1    TIM1_CH2    TIM1_CH1    TIM1_CH4                TIM1_CH3
     7                               TIM8_CH1    TIM8_CH2    TIM8_CH3                            TIM8_CH4
 */
-
 uint8_t timerClockDivisor(TIM_TypeDef *tim)
 {
     UNUSED(tim);
     return 1;
 }
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
-{
-    uint32_t tmp = 0;
-
-    /* Check the parameters */
-    assert_param(IS_TIM_LIST8_PERIPH(TIMx));
-    assert_param(IS_TIM_CHANNEL(TIM_Channel));
-    assert_param(IS_TIM_OCM(TIM_OCMode));
-
-    tmp = (uint32_t) TIMx;
-    tmp += CCMR_Offset;
-
-    if((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) {
-        tmp += (TIM_Channel>>1);
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= TIM_OCMode;
-    } else {
-        tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
-
-        /* Reset the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
-
-        /* Configure the OCxM bits in the CCMRx register */
-        *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
-    }
-}
-

src/main/drivers/timer_stm32f7xx.h

@@ -1,6 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
 
 #pragma once
 
 #include "stm32f7xx.h"
-
-void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

src/main/fc/config.c

@@ -287,7 +287,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
 #endif
     motorConfig->maxthrottle = 2000;
     motorConfig->mincommand = 1000;
-    motorConfig->digitalIdleOffset = 40;
+    motorConfig->digitalIdleOffsetPercent = 3.0f;
 
     int motorIndex = 0;
     for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
@@ -528,7 +528,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
 
 uint16_t getCurrentMinthrottle(void)
 {
-    return masterConfig.motorConfig.minthrottle;
+    return motorConfig()->minthrottle;
 }
 
 
@@ -565,8 +565,8 @@ void createDefaultConfig(master_t *config)
 
     // global settings
     config->current_profile_index = 0;    // default profile
-    config->dcm_kp = 2500;                // 1.0 * 10000
+    config->imuConfig.dcm_kp = 2500;                // 1.0 * 10000
-    config->dcm_ki = 0;                   // 0.003 * 10000
+    config->imuConfig.dcm_ki = 0;                   // 0.003 * 10000
     config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ;    // 256HZ default
 #ifdef STM32F10X
     config->gyroConfig.gyro_sync_denom = 8;
@@ -585,7 +585,7 @@ void createDefaultConfig(master_t *config)
     config->gyroConfig.gyro_soft_notch_hz_2 = 200;
     config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
 
-    config->debug_mode = DEBUG_NONE;
+    config->debug_mode = DEBUG_MODE;
 
     resetAccelerometerTrims(&config->sensorTrims.accZero);
 
@@ -596,7 +596,7 @@ void createDefaultConfig(master_t *config)
     config->boardAlignment.yawDegrees = 0;
     config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT;     // default/autodetect
     config->max_angle_inclination = 700;    // 70 degrees
-    config->yaw_control_direction = 1;
+    config->rcControlsConfig.yaw_control_direction = 1;
     config->gyroConfig.gyroMovementCalibrationThreshold = 32;
 
     // xxx_hardware: 0:default/autodetect, 1: disable
@@ -669,7 +669,7 @@ void createDefaultConfig(master_t *config)
     config->armingConfig.gyro_cal_on_first_arm = 0;  // TODO - Cleanup retarded arm support
     config->armingConfig.disarm_kill_switch = 1;
     config->armingConfig.auto_disarm_delay = 5;
-    config->armingConfig.small_angle = 25;
+    config->imuConfig.small_angle = 25;
 
     config->airplaneConfig.fixedwing_althold_dir = 1;
 
@@ -701,10 +701,11 @@ void createDefaultConfig(master_t *config)
     resetRollAndPitchTrims(&config->accelerometerTrims);
 
     config->compassConfig.mag_declination = 0;
-    config->acc_lpf_hz = 10.0f;
+    config->accelerometerConfig.acc_lpf_hz = 10.0f;
-    config->accDeadband.xy = 40;
+
-    config->accDeadband.z = 40;
+    config->imuConfig.accDeadband.xy = 40;
-    config->acc_unarmedcal = 1;
+    config->imuConfig.accDeadband.z = 40;
+    config->imuConfig.acc_unarmedcal = 1;
 
 #ifdef BARO
     resetBarometerConfig(&config->barometerConfig);
@@ -719,8 +720,8 @@ void createDefaultConfig(master_t *config)
 
     resetRcControlsConfig(&config->rcControlsConfig);
 
-    config->throttle_correction_value = 0;      // could 10 with althold or 40 for fpv
+    config->throttleCorrectionConfig.throttle_correction_value = 0;      // could 10 with althold or 40 for fpv
-    config->throttle_correction_angle = 800;    // could be 80.0 deg with atlhold or 45.0 for fpv
+    config->throttleCorrectionConfig.throttle_correction_angle = 800;    // could be 80.0 deg with atlhold or 45.0 for fpv
 
     // Failsafe Variables
     config->failsafeConfig.failsafe_delay = 10;                            // 1sec
@@ -821,8 +822,6 @@ void activateControlRateConfig(void)
 
 void activateConfig(void)
 {
-    static imuRuntimeConfig_t imuRuntimeConfig;
-
     activateControlRateConfig();
 
     resetAdjustmentStates();
@@ -843,8 +842,8 @@ void activateConfig(void)
 #endif
 
     useFailsafeConfig(&masterConfig.failsafeConfig);
-    setAccelerationTrims(&masterConfig.sensorTrims.accZero);
+    setAccelerationTrims(&sensorTrims()->accZero);
-    setAccelerationFilter(masterConfig.acc_lpf_hz);
+    setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
 
     mixerUseConfigs(
         &masterConfig.flight3DConfig,
@@ -858,16 +857,11 @@ void activateConfig(void)
     servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
 #endif
 
-    imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
-    imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
-    imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
-    imuRuntimeConfig.small_angle = masterConfig.armingConfig.small_angle;
 
     imuConfigure(
-        &imuRuntimeConfig,
+        &masterConfig.imuConfig,
         &currentProfile->pidProfile,
-        &masterConfig.accDeadband,
+        throttleCorrectionConfig()->throttle_correction_angle
-        masterConfig.throttle_correction_angle
     );
 
     configureAltitudeHold(
@@ -884,8 +878,8 @@ void activateConfig(void)
 
 void validateAndFixConfig(void)
 {
-    if((masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) && (masterConfig.motorConfig.mincommand < 1000)){
+    if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
-        masterConfig.motorConfig.mincommand = 1000;
+        motorConfig()->mincommand = 1000;
     }
 
     if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
@@ -914,7 +908,7 @@ void validateAndFixConfig(void)
         // rssi adc needs the same ports
         featureClear(FEATURE_RSSI_ADC);
         // current meter needs the same ports
-        if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+        if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
             featureClear(FEATURE_CURRENT_METER);
         }
 #endif
@@ -936,7 +930,7 @@ void validateAndFixConfig(void)
         // rssi adc needs the same ports
         featureClear(FEATURE_RSSI_ADC);
         // current meter needs the same ports
-        if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+        if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
             featureClear(FEATURE_CURRENT_METER);
         }
 #endif
@@ -944,13 +938,13 @@ void validateAndFixConfig(void)
 #endif
 
 #if defined(NAZE) && defined(SONAR)
-    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
 
 #if defined(OLIMEXINO) && defined(SONAR)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
@@ -971,7 +965,7 @@ void validateAndFixConfig(void)
 #endif
 
 #if defined(COLIBRI_RACE)
-    masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
+    serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
     if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
         featureClear(FEATURE_RX_PARALLEL_PWM);
         featureClear(FEATURE_RX_MSP);
@@ -997,16 +991,16 @@ void validateAndFixConfig(void)
 void validateAndFixGyroConfig(void)
 {
     // Prevent invalid notch cutoff
-    if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) {
+    if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
-        masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0;
+        gyroConfig()->gyro_soft_notch_hz_1 = 0;
     }
-    if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
+    if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
-        masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
+        gyroConfig()->gyro_soft_notch_hz_2 = 0;
     }
 
-    if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) {
+    if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
         masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
-        masterConfig.gyroConfig.gyro_sync_denom = 1;
+        gyroConfig()->gyro_sync_denom = 1;
     }
 }
 

src/main/fc/fc_msp.c

@@ -284,17 +284,21 @@ void initActiveBoxIds(void)
     if (sensors(SENSOR_ACC)) {
         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
+        activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
     }
 
+#ifdef BARO
     if (sensors(SENSOR_BARO)) {
         activeBoxIds[activeBoxIdCount++] = BOXBARO;
     }
+#endif
 
-    if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
+#ifdef MAG
+    if (sensors(SENSOR_MAG)) {
         activeBoxIds[activeBoxIdCount++] = BOXMAG;
-        activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
         activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
     }
+#endif
 
 #ifdef GPS
     if (feature(FEATURE_GPS)) {
@@ -313,7 +317,7 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
     }
 
-    if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
+    if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
     }
 
@@ -350,7 +354,7 @@ void initActiveBoxIds(void)
     }
 
 #ifdef TELEMETRY
-    if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) {
+    if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
         activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
     }
 #endif
@@ -360,7 +364,7 @@ void initActiveBoxIds(void)
 #endif
 
 #ifdef USE_SERVOS
-    if (masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
+    if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
         activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
         activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
         activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
@@ -563,7 +567,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
     // DEPRECATED - Use MSP_API_VERSION
     case MSP_IDENT:
         sbufWriteU8(dst, MW_VERSION);
-        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
+        sbufWriteU8(dst, mixerConfig()->mixerMode);
         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
         sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
         break;
@@ -607,15 +611,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
     case MSP_RAW_IMU:
         {
             // Hack scale due to choice of units for sensor data in multiwii
-            const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
+            const uint8_t scale = (acc.dev.acc_1G > 512) ? 4 : 1;
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, accSmooth[i] / scale);
+                sbufWriteU16(dst, acc.accSmooth[i] / scale);
             }
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, gyroADC[i]);
+                sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
             }
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, magADC[i]);
+                sbufWriteU16(dst, mag.magADC[i]);
             }
         }
         break;
@@ -693,15 +697,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
         sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
         sbufWriteU16(dst, rssi);
-        if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
+        if(batteryConfig()->multiwiiCurrentMeterOutput) {
             sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
         } else
             sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
         break;
 
     case MSP_ARMING_CONFIG:
-        sbufWriteU8(dst, masterConfig.armingConfig.auto_disarm_delay);
+        sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
-        sbufWriteU8(dst, masterConfig.armingConfig.disarm_kill_switch);
+        sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
         break;
 
     case MSP_LOOP_TIME:
@@ -776,33 +780,33 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_MISC:
-        sbufWriteU16(dst, masterConfig.rxConfig.midrc);
+        sbufWriteU16(dst, rxConfig()->midrc);
 
-        sbufWriteU16(dst, masterConfig.motorConfig.minthrottle);
+        sbufWriteU16(dst, motorConfig()->minthrottle);
-        sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle);
+        sbufWriteU16(dst, motorConfig()->maxthrottle);
-        sbufWriteU16(dst, masterConfig.motorConfig.mincommand);
+        sbufWriteU16(dst, motorConfig()->mincommand);
 
-        sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
+        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
 
 #ifdef GPS
-        sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type
+        sbufWriteU8(dst, gpsConfig()->provider); // gps_type
         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
-        sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
+        sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
 #else
         sbufWriteU8(dst, 0); // gps_type
         sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
         sbufWriteU8(dst, 0); // gps_ubx_sbas
 #endif
-        sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
+        sbufWriteU8(dst, batteryConfig()->multiwiiCurrentMeterOutput);
-        sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
+        sbufWriteU8(dst, rxConfig()->rssi_channel);
         sbufWriteU8(dst, 0);
 
-        sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10);
+        sbufWriteU16(dst, compassConfig()->mag_declination / 10);
 
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
+        sbufWriteU8(dst, batteryConfig()->vbatscale);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
         break;
 
     case MSP_MOTOR_PINS:
@@ -866,104 +870,104 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_BOARD_ALIGNMENT:
-        sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
+        sbufWriteU16(dst, boardAlignment()->rollDegrees);
-        sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
+        sbufWriteU16(dst, boardAlignment()->pitchDegrees);
-        sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
+        sbufWriteU16(dst, boardAlignment()->yawDegrees);
         break;
 
     case MSP_VOLTAGE_METER_CONFIG:
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
+        sbufWriteU8(dst, batteryConfig()->vbatscale);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
-        sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
+        sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
         break;
 
     case MSP_CURRENT_METER_CONFIG:
-        sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
+        sbufWriteU16(dst, batteryConfig()->currentMeterScale);
-        sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
+        sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
-        sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType);
+        sbufWriteU8(dst, batteryConfig()->currentMeterType);
-        sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity);
+        sbufWriteU16(dst, batteryConfig()->batteryCapacity);
         break;
 
     case MSP_MIXER:
-        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
+        sbufWriteU8(dst, mixerConfig()->mixerMode);
         break;
 
     case MSP_RX_CONFIG:
-        sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
+        sbufWriteU8(dst, rxConfig()->serialrx_provider);
-        sbufWriteU16(dst, masterConfig.rxConfig.maxcheck);
+        sbufWriteU16(dst, rxConfig()->maxcheck);
-        sbufWriteU16(dst, masterConfig.rxConfig.midrc);
+        sbufWriteU16(dst, rxConfig()->midrc);
-        sbufWriteU16(dst, masterConfig.rxConfig.mincheck);
+        sbufWriteU16(dst, rxConfig()->mincheck);
-        sbufWriteU8(dst, masterConfig.rxConfig.spektrum_sat_bind);
+        sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
-        sbufWriteU16(dst, masterConfig.rxConfig.rx_min_usec);
+        sbufWriteU16(dst, rxConfig()->rx_min_usec);
-        sbufWriteU16(dst, masterConfig.rxConfig.rx_max_usec);
+        sbufWriteU16(dst, rxConfig()->rx_max_usec);
-        sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolation);
+        sbufWriteU8(dst, rxConfig()->rcInterpolation);
-        sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolationInterval);
+        sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
-        sbufWriteU16(dst, masterConfig.rxConfig.airModeActivateThreshold);
+        sbufWriteU16(dst, rxConfig()->airModeActivateThreshold);
-        sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_protocol);
+        sbufWriteU8(dst, rxConfig()->rx_spi_protocol);
-        sbufWriteU32(dst, masterConfig.rxConfig.rx_spi_id);
+        sbufWriteU32(dst, rxConfig()->rx_spi_id);
-        sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_rf_channel_count);
+        sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count);
         break;
 
     case MSP_FAILSAFE_CONFIG:
-        sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_delay);
+        sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
-        sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_off_delay);
+        sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
-        sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
+        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
-        sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_kill_switch);
+        sbufWriteU8(dst, failsafeConfig()->failsafe_kill_switch);
-        sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle_low_delay);
+        sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
-        sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_procedure);
+        sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
         break;
 
     case MSP_RXFAIL_CONFIG:
         for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
-            sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
+            sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode);
-            sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
+            sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
         }
         break;
 
     case MSP_RSSI_CONFIG:
-        sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
+        sbufWriteU8(dst, rxConfig()->rssi_channel);
         break;
 
     case MSP_RX_MAP:
         for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
-            sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]);
+            sbufWriteU8(dst, rxConfig()->rcmap[i]);
         }
         break;
 
     case MSP_BF_CONFIG:
-        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
+        sbufWriteU8(dst, mixerConfig()->mixerMode);
 
         sbufWriteU32(dst, featureMask());
 
-        sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
+        sbufWriteU8(dst, rxConfig()->serialrx_provider);
 
-        sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
+        sbufWriteU16(dst, boardAlignment()->rollDegrees);
-        sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
+        sbufWriteU16(dst, boardAlignment()->pitchDegrees);
-        sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
+        sbufWriteU16(dst, boardAlignment()->yawDegrees);
 
-        sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
+        sbufWriteU16(dst, batteryConfig()->currentMeterScale);
-        sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
+        sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
         break;
 
     case MSP_CF_SERIAL_CONFIG:
         for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
-            if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
+            if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
                 continue;
             };
-            sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].identifier);
+            sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
-            sbufWriteU16(dst, masterConfig.serialConfig.portConfigs[i].functionMask);
+            sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
-            sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
+            sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
-            sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
+            sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
-            sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
+            sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
-            sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
+            sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
         }
         break;
 
 #ifdef LED_STRIP
     case MSP_LED_COLORS:
         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
-            hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
+            hsvColor_t *color = &ledStripConfig()->colors[i];
             sbufWriteU16(dst, color->h);
             sbufWriteU8(dst, color->s);
             sbufWriteU8(dst, color->v);
@@ -972,7 +976,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_LED_STRIP_CONFIG:
         for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
-            ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i];
+            ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
             sbufWriteU32(dst, *ledConfig);
         }
         break;
@@ -982,19 +986,19 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
             for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
                 sbufWriteU8(dst, i);
                 sbufWriteU8(dst, j);
-                sbufWriteU8(dst, masterConfig.ledStripConfig.modeColors[i].color[j]);
+                sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
             }
         }
 
         for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
             sbufWriteU8(dst, LED_MODE_COUNT);
             sbufWriteU8(dst, j);
-            sbufWriteU8(dst, masterConfig.ledStripConfig.specialColors.color[j]);
+            sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
         }
 
         sbufWriteU8(dst, LED_AUX_CHANNEL);
         sbufWriteU8(dst, 0);
-        sbufWriteU8(dst, masterConfig.ledStripConfig.ledstrip_aux_channel);
+        sbufWriteU8(dst, ledStripConfig()->ledstrip_aux_channel);
         break;
 #endif
 
@@ -1005,9 +1009,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
     case MSP_BLACKBOX_CONFIG:
 #ifdef BLACKBOX
         sbufWriteU8(dst, 1); //Blackbox supported
-        sbufWriteU8(dst, masterConfig.blackboxConfig.device);
+        sbufWriteU8(dst, blackboxConfig()->device);
-        sbufWriteU8(dst, masterConfig.blackboxConfig.rate_num);
+        sbufWriteU8(dst, blackboxConfig()->rate_num);
-        sbufWriteU8(dst, masterConfig.blackboxConfig.rate_denom);
+        sbufWriteU8(dst, blackboxConfig()->rate_denom);
 #else
         sbufWriteU8(dst, 0); // Blackbox not supported
         sbufWriteU8(dst, 0);
@@ -1036,17 +1040,17 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, 1); // OSD supported
         // send video system (AUTO/PAL/NTSC)
 #ifdef USE_MAX7456
-        sbufWriteU8(dst, masterConfig.vcdProfile.video_system);
+        sbufWriteU8(dst, vcdProfile()->video_system);
 #else
         sbufWriteU8(dst, 0);
 #endif
-        sbufWriteU8(dst, masterConfig.osdProfile.units);
+        sbufWriteU8(dst, osdProfile()->units);
-        sbufWriteU8(dst, masterConfig.osdProfile.rssi_alarm);
+        sbufWriteU8(dst, osdProfile()->rssi_alarm);
-        sbufWriteU16(dst, masterConfig.osdProfile.cap_alarm);
+        sbufWriteU16(dst, osdProfile()->cap_alarm);
-        sbufWriteU16(dst, masterConfig.osdProfile.time_alarm);
+        sbufWriteU16(dst, osdProfile()->time_alarm);
-        sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm);
+        sbufWriteU16(dst, osdProfile()->alt_alarm);
         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
-            sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]);
+            sbufWriteU16(dst, osdProfile()->item_pos[i]);
         }
 #else
         sbufWriteU8(dst, 0); // OSD not supported
@@ -1062,47 +1066,47 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_3D:
-        sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low);
+        sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
-        sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high);
+        sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
-        sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d);
+        sbufWriteU16(dst, flight3DConfig()->neutral3d);
         break;
 
     case MSP_RC_DEADBAND:
-        sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
+        sbufWriteU8(dst, rcControlsConfig()->deadband);
-        sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
+        sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
-        sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
+        sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
-        sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle);
+        sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
         break;
 
     case MSP_SENSOR_ALIGNMENT:
-        sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align);
+        sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align);
-        sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align);
+        sbufWriteU8(dst, sensorAlignmentConfig()->acc_align);
-        sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align);
+        sbufWriteU8(dst, sensorAlignmentConfig()->mag_align);
         break;
 
     case MSP_ADVANCED_CONFIG:
-        if (masterConfig.gyroConfig.gyro_lpf) {
+        if (gyroConfig()->gyro_lpf) {
             sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
             sbufWriteU8(dst, 1);
         } else {
-            sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
+            sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
             sbufWriteU8(dst, masterConfig.pid_process_denom);
         }
-        sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
+        sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
-        sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol);
+        sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
-        sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate);
+        sbufWriteU16(dst, motorConfig()->motorPwmRate);
         break;
 
     case MSP_FILTER_CONFIG :
-        sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
+        sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
-        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
+        sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
-        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
+        sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
-        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
+        sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
-        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
+        sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
         break;
 
     case MSP_PID_ADVANCED:
@@ -1121,9 +1125,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_SENSOR_CONFIG:
-        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware);
+        sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware);
-        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware);
+        sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware);
-        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware);
+        sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware);
         break;
 
     case MSP_REBOOT:
@@ -1247,8 +1251,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         masterConfig.accelerometerTrims.values.roll  = sbufReadU16(src);
         break;
     case MSP_SET_ARMING_CONFIG:
-        masterConfig.armingConfig.auto_disarm_delay = sbufReadU8(src);
+        armingConfig()->auto_disarm_delay = sbufReadU8(src);
-        masterConfig.armingConfig.disarm_kill_switch = sbufReadU8(src);
+        armingConfig()->disarm_kill_switch = sbufReadU8(src);
         break;
 
     case MSP_SET_LOOP_TIME:
@@ -1264,6 +1268,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             currentProfile->pidProfile.I8[i] = sbufReadU8(src);
             currentProfile->pidProfile.D8[i] = sbufReadU8(src);
         }
+        pidInitConfig(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_MODE_RANGE:
@@ -1334,33 +1339,33 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     case MSP_SET_MISC:
         tmp = sbufReadU16(src);
         if (tmp < 1600 && tmp > 1400)
-            masterConfig.rxConfig.midrc = tmp;
+            rxConfig()->midrc = tmp;
 
-        masterConfig.motorConfig.minthrottle = sbufReadU16(src);
+        motorConfig()->minthrottle = sbufReadU16(src);
-        masterConfig.motorConfig.maxthrottle = sbufReadU16(src);
+        motorConfig()->maxthrottle = sbufReadU16(src);
-        masterConfig.motorConfig.mincommand = sbufReadU16(src);
+        motorConfig()->mincommand = sbufReadU16(src);
 
-        masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
+        failsafeConfig()->failsafe_throttle = sbufReadU16(src);
 
 #ifdef GPS
-        masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type
+        gpsConfig()->provider = sbufReadU8(src); // gps_type
         sbufReadU8(src); // gps_baudrate
-        masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas
+        gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
 #else
         sbufReadU8(src); // gps_type
         sbufReadU8(src); // gps_baudrate
         sbufReadU8(src); // gps_ubx_sbas
 #endif
-        masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src);
+        batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
-        masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
+        rxConfig()->rssi_channel = sbufReadU8(src);
         sbufReadU8(src);
 
-        masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10;
+        compassConfig()->mag_declination = sbufReadU16(src) * 10;
 
-        masterConfig.batteryConfig.vbatscale = sbufReadU8(src);           // actual vbatscale as intended
+        batteryConfig()->vbatscale = sbufReadU8(src);           // actual vbatscale as intended
-        masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
+        batteryConfig()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
-        masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
+        batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
-        masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
+        batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
         break;
 
     case MSP_SET_MOTOR:
@@ -1410,16 +1415,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_3D:
-        masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
+        flight3DConfig()->deadband3d_low = sbufReadU16(src);
-        masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
+        flight3DConfig()->deadband3d_high = sbufReadU16(src);
-        masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
+        flight3DConfig()->neutral3d = sbufReadU16(src);
         break;
 
     case MSP_SET_RC_DEADBAND:
-        masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
+        rcControlsConfig()->deadband = sbufReadU8(src);
-        masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
+        rcControlsConfig()->yaw_deadband = sbufReadU8(src);
-        masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
+        rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
-        masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
+        flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
         break;
 
     case MSP_SET_RESET_CURR_PID:
@@ -1427,36 +1432,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_SENSOR_ALIGNMENT:
-        masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src);
+        sensorAlignmentConfig()->gyro_align = sbufReadU8(src);
-        masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src);
+        sensorAlignmentConfig()->acc_align = sbufReadU8(src);
-        masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src);
+        sensorAlignmentConfig()->mag_align = sbufReadU8(src);
         break;
 
     case MSP_SET_ADVANCED_CONFIG:
-        masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
+        gyroConfig()->gyro_sync_denom = sbufReadU8(src);
         masterConfig.pid_process_denom = sbufReadU8(src);
-        masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
+        motorConfig()->useUnsyncedPwm = sbufReadU8(src);
 #ifdef USE_DSHOT
-        masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
+        motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
 #else
-        masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
+        motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
 #endif
-        masterConfig.motorConfig.motorPwmRate = sbufReadU16(src);
+        motorConfig()->motorPwmRate = sbufReadU16(src);
         break;
 
     case MSP_SET_FILTER_CONFIG:
-        masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src);
+        gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
         currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
         currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
         if (dataSize > 5) {
-            masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
+            gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
-            masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
+            gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
             currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
             currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
         }
         if (dataSize > 13) {
-            masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
+            gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
-            masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
+            gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
         }
         // reinitialize the gyro filters with the new values
         validateAndFixGyroConfig();
@@ -1478,12 +1483,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
         currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
         currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
+        pidInitConfig(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_SENSOR_CONFIG:
-        masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src);
+        sensorSelectionConfig()->acc_hardware = sbufReadU8(src);
-        masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src);
+        sensorSelectionConfig()->baro_hardware = sbufReadU8(src);
-        masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src);
+        sensorSelectionConfig()->mag_hardware = sbufReadU8(src);
         break;
 
     case MSP_RESET_CONF:
@@ -1515,9 +1521,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     case MSP_SET_BLACKBOX_CONFIG:
         // Don't allow config to be updated while Blackbox is logging
         if (blackboxMayEditConfig()) {
-            masterConfig.blackboxConfig.device = sbufReadU8(src);
+            blackboxConfig()->device = sbufReadU8(src);
-            masterConfig.blackboxConfig.rate_num = sbufReadU8(src);
+            blackboxConfig()->rate_num = sbufReadU8(src);
-            masterConfig.blackboxConfig.rate_denom = sbufReadU8(src);
+            blackboxConfig()->rate_denom = sbufReadU8(src);
         }
         break;
 #endif
@@ -1542,18 +1548,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             // set all the other settings
             if ((int8_t)addr == -1) {
 #ifdef USE_MAX7456
-                masterConfig.vcdProfile.video_system = sbufReadU8(src);
+                vcdProfile()->video_system = sbufReadU8(src);
 #else
                 sbufReadU8(src); // Skip video system
 #endif
-                masterConfig.osdProfile.units = sbufReadU8(src);
+                osdProfile()->units = sbufReadU8(src);
-                masterConfig.osdProfile.rssi_alarm = sbufReadU8(src);
+                osdProfile()->rssi_alarm = sbufReadU8(src);
-                masterConfig.osdProfile.cap_alarm = sbufReadU16(src);
+                osdProfile()->cap_alarm = sbufReadU16(src);
-                masterConfig.osdProfile.time_alarm = sbufReadU16(src);
+                osdProfile()->time_alarm = sbufReadU16(src);
-                masterConfig.osdProfile.alt_alarm = sbufReadU16(src);
+                osdProfile()->alt_alarm = sbufReadU16(src);
             } else {
                 // set a position setting
-                masterConfig.osdProfile.item_pos[addr] = sbufReadU16(src);
+                osdProfile()->item_pos[addr] = sbufReadU16(src);
             }
         }
         break;
@@ -1641,79 +1647,79 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_BOARD_ALIGNMENT:
-        masterConfig.boardAlignment.rollDegrees = sbufReadU16(src);
+        boardAlignment()->rollDegrees = sbufReadU16(src);
-        masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src);
+        boardAlignment()->pitchDegrees = sbufReadU16(src);
-        masterConfig.boardAlignment.yawDegrees = sbufReadU16(src);
+        boardAlignment()->yawDegrees = sbufReadU16(src);
         break;
 
     case MSP_SET_VOLTAGE_METER_CONFIG:
-        masterConfig.batteryConfig.vbatscale = sbufReadU8(src);           // actual vbatscale as intended
+        batteryConfig()->vbatscale = sbufReadU8(src);           // actual vbatscale as intended
-        masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
+        batteryConfig()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
-        masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
+        batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
-        masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
+        batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
         break;
 
     case MSP_SET_CURRENT_METER_CONFIG:
-        masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
+        batteryConfig()->currentMeterScale = sbufReadU16(src);
-        masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
+        batteryConfig()->currentMeterOffset = sbufReadU16(src);
-        masterConfig.batteryConfig.currentMeterType = sbufReadU8(src);
+        batteryConfig()->currentMeterType = sbufReadU8(src);
-        masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src);
+        batteryConfig()->batteryCapacity = sbufReadU16(src);
         break;
 
 #ifndef USE_QUAD_MIXER_ONLY
     case MSP_SET_MIXER:
-        masterConfig.mixerConfig.mixerMode = sbufReadU8(src);
+        mixerConfig()->mixerMode = sbufReadU8(src);
         break;
 #endif
 
     case MSP_SET_RX_CONFIG:
-        masterConfig.rxConfig.serialrx_provider = sbufReadU8(src);
+        rxConfig()->serialrx_provider = sbufReadU8(src);
-        masterConfig.rxConfig.maxcheck = sbufReadU16(src);
+        rxConfig()->maxcheck = sbufReadU16(src);
-        masterConfig.rxConfig.midrc = sbufReadU16(src);
+        rxConfig()->midrc = sbufReadU16(src);
-        masterConfig.rxConfig.mincheck = sbufReadU16(src);
+        rxConfig()->mincheck = sbufReadU16(src);
-        masterConfig.rxConfig.spektrum_sat_bind = sbufReadU8(src);
+        rxConfig()->spektrum_sat_bind = sbufReadU8(src);
         if (dataSize > 8) {
-            masterConfig.rxConfig.rx_min_usec = sbufReadU16(src);
+            rxConfig()->rx_min_usec = sbufReadU16(src);
-            masterConfig.rxConfig.rx_max_usec = sbufReadU16(src);
+            rxConfig()->rx_max_usec = sbufReadU16(src);
         }
         if (dataSize > 12) {
-            masterConfig.rxConfig.rcInterpolation = sbufReadU8(src);
+            rxConfig()->rcInterpolation = sbufReadU8(src);
-            masterConfig.rxConfig.rcInterpolationInterval = sbufReadU8(src);
+            rxConfig()->rcInterpolationInterval = sbufReadU8(src);
-            masterConfig.rxConfig.airModeActivateThreshold = sbufReadU16(src);
+            rxConfig()->airModeActivateThreshold = sbufReadU16(src);
         }
         if (dataSize > 16) {
-            masterConfig.rxConfig.rx_spi_protocol = sbufReadU8(src);
+            rxConfig()->rx_spi_protocol = sbufReadU8(src);
-            masterConfig.rxConfig.rx_spi_id = sbufReadU32(src);
+            rxConfig()->rx_spi_id = sbufReadU32(src);
-            masterConfig.rxConfig.rx_spi_rf_channel_count = sbufReadU8(src);
+            rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
         }
         break;
 
     case MSP_SET_FAILSAFE_CONFIG:
-        masterConfig.failsafeConfig.failsafe_delay = sbufReadU8(src);
+        failsafeConfig()->failsafe_delay = sbufReadU8(src);
-        masterConfig.failsafeConfig.failsafe_off_delay = sbufReadU8(src);
+        failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
-        masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
+        failsafeConfig()->failsafe_throttle = sbufReadU16(src);
-        masterConfig.failsafeConfig.failsafe_kill_switch = sbufReadU8(src);
+        failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
-        masterConfig.failsafeConfig.failsafe_throttle_low_delay = sbufReadU16(src);
+        failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
-        masterConfig.failsafeConfig.failsafe_procedure = sbufReadU8(src);
+        failsafeConfig()->failsafe_procedure = sbufReadU8(src);
         break;
 
     case MSP_SET_RXFAIL_CONFIG:
         i = sbufReadU8(src);
         if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
-            masterConfig.rxConfig.failsafe_channel_configurations[i].mode = sbufReadU8(src);
+            rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
-            masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
+            rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
         } else {
             return MSP_RESULT_ERROR;
         }
         break;
 
     case MSP_SET_RSSI_CONFIG:
-        masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
+        rxConfig()->rssi_channel = sbufReadU8(src);
         break;
 
     case MSP_SET_RX_MAP:
         for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
-            masterConfig.rxConfig.rcmap[i] = sbufReadU8(src);
+            rxConfig()->rcmap[i] = sbufReadU8(src);
         }
         break;
 
@@ -1721,20 +1727,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #ifdef USE_QUAD_MIXER_ONLY
         sbufReadU8(src); // mixerMode ignored
 #else
-        masterConfig.mixerConfig.mixerMode = sbufReadU8(src); // mixerMode
+        mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
 #endif
 
         featureClearAll();
         featureSet(sbufReadU32(src)); // features bitmap
 
-        masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type
+        rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
 
-        masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); // board_align_roll
+        boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
-        masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); // board_align_pitch
+        boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
-        masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); // board_align_yaw
+        boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
 
-        masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
+        batteryConfig()->currentMeterScale = sbufReadU16(src);
-        masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
+        batteryConfig()->currentMeterOffset = sbufReadU16(src);
         break;
 
     case MSP_SET_CF_SERIAL_CONFIG:
@@ -1768,7 +1774,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #ifdef LED_STRIP
     case MSP_SET_LED_COLORS:
         for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
-            hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
+            hsvColor_t *color = &ledStripConfig()->colors[i];
             color->h = sbufReadU16(src);
             color->s = sbufReadU8(src);
             color->v = sbufReadU8(src);
@@ -1781,7 +1787,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
                 return MSP_RESULT_ERROR;
             }
-            ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i];
+            ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
             *ledConfig = sbufReadU32(src);
             reevaluateLedConfig();
         }

src/main/fc/fc_tasks.c

@@ -32,6 +32,7 @@
 #include "drivers/accgyro.h"
 #include "drivers/compass.h"
 #include "drivers/serial.h"
+#include "drivers/stack_check.h"
 
 #include "fc/config.h"
 #include "fc/fc_msp.h"
@@ -74,6 +75,10 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#ifdef USE_BST
+void taskBstMasterProcess(timeUs_t currentTimeUs);
+#endif
+
 #define TASK_PERIOD_HZ(hz) (1000000 / (hz))
 #define TASK_PERIOD_MS(ms) ((ms) * 1000)
 #define TASK_PERIOD_US(us) (us)
@@ -84,16 +89,16 @@
 #define IBATINTERVAL (6 * 3500)
 
 
-static void taskUpdateAccelerometer(uint32_t currentTime)
+static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTime);
+    UNUSED(currentTimeUs);
 
     imuUpdateAccelerometer(&masterConfig.accelerometerTrims);
 }
 
-static void taskHandleSerial(uint32_t currentTime)
+static void taskHandleSerial(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTime);
+    UNUSED(currentTimeUs);
 #ifdef USE_CLI
     // in cli mode, all serial stuff goes to here. enter cli mode by sending #
     if (cliMode) {
@@ -104,13 +109,13 @@ static void taskHandleSerial(uint32_t currentTime)
     mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
 }
 
-static void taskUpdateBattery(uint32_t currentTime)
+static void taskUpdateBattery(timeUs_t currentTimeUs)
 {
 #ifdef USE_ADC
     static uint32_t vbatLastServiced = 0;
     if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
-        if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
+        if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
-            vbatLastServiced = currentTime;
+            vbatLastServiced = currentTimeUs;
             updateBattery();
         }
     }
@@ -118,18 +123,18 @@ static void taskUpdateBattery(uint32_t currentTime)
 
     static uint32_t ibatLastServiced = 0;
     if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
-        const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
+        const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
 
         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
-            ibatLastServiced = currentTime;
+            ibatLastServiced = currentTimeUs;
-            updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+            updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
         }
     }
 }
 
-static void taskUpdateRxMain(uint32_t currentTime)
+static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    processRx(currentTime);
+    processRx(currentTimeUs);
     isRXDataNew = true;
 
 #if !defined(BARO) && !defined(SONAR)
@@ -152,18 +157,18 @@ static void taskUpdateRxMain(uint32_t currentTime)
 }
 
 #ifdef MAG
-static void taskUpdateCompass(uint32_t currentTime)
+static void taskUpdateCompass(timeUs_t currentTimeUs)
 {
     if (sensors(SENSOR_MAG)) {
-        compassUpdate(currentTime, &masterConfig.sensorTrims.magZero);
+        compassUpdate(currentTimeUs, &sensorTrims()->magZero);
     }
 }
 #endif
 
 #ifdef BARO
-static void taskUpdateBaro(uint32_t currentTime)
+static void taskUpdateBaro(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTime);
+    UNUSED(currentTimeUs);
 
     if (sensors(SENSOR_BARO)) {
         const uint32_t newDeadline = baroUpdate();
@@ -175,7 +180,7 @@ static void taskUpdateBaro(uint32_t currentTime)
 #endif
 
 #if defined(BARO) || defined(SONAR)
-static void taskCalculateAltitude(uint32_t currentTime)
+static void taskCalculateAltitude(timeUs_t currentTimeUs)
 {
     if (false
 #if defined(BARO)
@@ -185,26 +190,26 @@ static void taskCalculateAltitude(uint32_t currentTime)
         || sensors(SENSOR_SONAR)
 #endif
         ) {
-        calculateEstimatedAltitude(currentTime);
+        calculateEstimatedAltitude(currentTimeUs);
     }}
 #endif
 
 #ifdef TELEMETRY
-static void taskTelemetry(uint32_t currentTime)
+static void taskTelemetry(timeUs_t currentTimeUs)
 {
     telemetryCheckState();
 
     if (!cliMode && feature(FEATURE_TELEMETRY)) {
-        telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+        telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
     }
 }
 #endif
 
 #ifdef USE_ESC_TELEMETRY
-static void taskEscTelemetry(uint32_t currentTime)
+static void taskEscTelemetry(timeUs_t currentTimeUs)
 {
     if (feature(FEATURE_ESC_TELEMETRY)) {
-        escTelemetryProcess(currentTime);
+        escTelemetryProcess(currentTimeUs);
      }
  }
 #endif
@@ -230,7 +235,7 @@ void fcTasksInit(void)
 
     if (sensors(SENSOR_ACC)) {
         setTaskEnabled(TASK_ACCEL, true);
-        rescheduleTask(TASK_ACCEL, accSamplingInterval);
+        rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
     }
 
     setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
@@ -266,10 +271,10 @@ void fcTasksInit(void)
 #ifdef TELEMETRY
     setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
     if (feature(FEATURE_TELEMETRY)) {
-        if (masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
+        if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) {
             // Reschedule telemetry to 500hz for Jeti Exbus
             rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
-        } else if (masterConfig.rxConfig.serialrx_provider == SERIALRX_CRSF) {
+        } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
             // Reschedule telemetry to 500hz, 2ms for CRSF
             rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
         }
@@ -297,6 +302,9 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
 #endif
 #endif
+#ifdef STACK_CHECK
+    setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
 #ifdef VTX_CONTROL
 #ifdef VTX_SMARTAUDIO
     setTaskEnabled(TASK_VTXCTRL, true);
@@ -315,7 +323,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_GYROPID] = {
         .taskName = "PID",
         .subTaskName = "GYRO",
-        .taskFunc = taskMainPidLoopCheck,
+        .taskFunc = taskMainPidLoop,
         .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
         .staticPriority = TASK_PRIORITY_REALTIME,
     },
@@ -480,6 +488,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef STACK_CHECK
+    [TASK_STACK_CHECK] = {
+        .taskName = "STACKCHECK",
+        .taskFunc = taskStackCheck,
+        .desiredPeriod = TASK_PERIOD_HZ(10),          // 10 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #ifdef VTX_CONTROL
     [TASK_VTXCTRL] = {
         .taskName = "VTXCTRL",

src/main/fc/fc_tasks.h

@@ -19,11 +19,4 @@
 
 #define LOOPTIME_SUSPEND_TIME 3  // Prevent too long busy wait times
 
-void taskSystem(uint32_t currentTime);
-void taskMainPidLoopCheck(uint32_t currentTime);
-#ifdef USE_BST
-void taskBstReadWrite(uint32_t currentTime);
-void taskBstMasterProcess(uint32_t currentTime);
-#endif
-
 void fcTasksInit(void);

src/main/fc/mw.c

@@ -177,23 +177,21 @@ void calculateSetpointRate(int axis, int16_t rc) {
         angleRate *= rcSuperfactor;
     }
 
-    if (debugMode == DEBUG_ANGLERATE) {
+    DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
-        debug[axis] = angleRate;
-    }
 
     setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
 }
 
 void scaleRcCommandToFpvCamAngle(void) {
-    //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
+    //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
     static uint8_t lastFpvCamAngleDegrees = 0;
     static float cosFactor = 1.0;
     static float sinFactor = 0.0;
 
-    if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
+    if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
-        lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
+        lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
-        cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
+        cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
-        sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
+        sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
     }
 
     int16_t roll = rcCommand[ROLL];
@@ -210,15 +208,15 @@ void processRcCommand(void)
     uint16_t rxRefreshRate;
     bool readyToCalculateRate = false;
 
-    if (masterConfig.rxConfig.rcInterpolation || flightModeFlags) {
+    if (rxConfig()->rcInterpolation || flightModeFlags) {
         if (isRXDataNew) {
             // Set RC refresh rate for sampling and channels to filter
-            switch (masterConfig.rxConfig.rcInterpolation) {
+            switch (rxConfig()->rcInterpolation) {
                 case(RC_SMOOTHING_AUTO):
                     rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
                     break;
                 case(RC_SMOOTHING_MANUAL):
-                    rxRefreshRate = 1000 * masterConfig.rxConfig.rcInterpolationInterval;
+                    rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
                     break;
                 case(RC_SMOOTHING_OFF):
                 case(RC_SMOOTHING_DEFAULT):
@@ -257,7 +255,7 @@ void processRcCommand(void)
 
     if (readyToCalculateRate || isRXDataNew) {
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
-        if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
+        if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
             scaleRcCommandToFpvCamAngle();
 
         for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]);
@@ -284,23 +282,23 @@ void updateRcCommands(void)
         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
         PIDweight[axis] = prop;
 
-        int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
+        int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
         if (axis == ROLL || axis == PITCH) {
-            if (tmp > masterConfig.rcControlsConfig.deadband) {
+            if (tmp > rcControlsConfig()->deadband) {
-                tmp -= masterConfig.rcControlsConfig.deadband;
+                tmp -= rcControlsConfig()->deadband;
             } else {
                 tmp = 0;
             }
             rcCommand[axis] = tmp;
         } else if (axis == YAW) {
-            if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
+            if (tmp > rcControlsConfig()->yaw_deadband) {
-                tmp -= masterConfig.rcControlsConfig.yaw_deadband;
+                tmp -= rcControlsConfig()->yaw_deadband;
             } else {
                 tmp = 0;
             }
-            rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
+            rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
         }
-        if (rcData[axis] < masterConfig.rxConfig.midrc) {
+        if (rcData[axis] < rxConfig()->midrc) {
             rcCommand[axis] = -rcCommand[axis];
         }
     }
@@ -310,14 +308,14 @@ void updateRcCommands(void)
         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
     } else {
-        tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
+        tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
-        tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
+        tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
     }
     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 
     if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
-        rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc);
+        rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
     }
 
     if (FLIGHT_MODE(HEADFREE_MODE)) {
@@ -377,6 +375,7 @@ void mwDisarm(void)
 
 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
 
+#ifdef TELEMETRY
 static void releaseSharedTelemetryPorts(void) {
     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
     while (sharedPort) {
@@ -384,12 +383,13 @@ static void releaseSharedTelemetryPorts(void) {
         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
     }
 }
+#endif
 
 void mwArm(void)
 {
     static bool firstArmingCalibrationWasCompleted;
 
-    if (masterConfig.armingConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
+    if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
         gyroSetCalibrationCycles();
         armingCalibrationWasInitialised = true;
         firstArmingCalibrationWasCompleted = true;
@@ -418,7 +418,7 @@ void mwArm(void)
                 startBlackbox();
             }
 #endif
-            disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
+            disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
             //beep to indicate arming
 #ifdef GPS
@@ -464,7 +464,7 @@ void handleInflightCalibrationStickPosition(void)
 
 static void updateInflightCalibrationState(void)
 {
-    if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) {   // Copter is airborne and you are turning it off via boxarm : start measurement
+    if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) {   // Copter is airborne and you are turning it off via boxarm : start measurement
         InflightcalibratingA = 50;
         AccInflightCalibrationArmed = false;
     }
@@ -486,19 +486,19 @@ void updateMagHold(void)
             dif += 360;
         if (dif >= +180)
             dif -= 360;
-        dif *= -masterConfig.yaw_control_direction;
+        dif *= -rcControlsConfig()->yaw_control_direction;
         if (STATE(SMALL_ANGLE))
             rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg
     } else
         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 }
 
-void processRx(uint32_t currentTime)
+void processRx(timeUs_t currentTimeUs)
 {
     static bool armedBeeperOn = false;
     static bool airmodeIsActivated;
 
-    calculateRxChannelsAndUpdateFailsafe(currentTime);
+    calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 
     // in 3D mode, we need to be able to disarm by switch at any time
     if (feature(FEATURE_3D)) {
@@ -506,21 +506,21 @@ void processRx(uint32_t currentTime)
             mwDisarm();
     }
 
-    updateRSSI(currentTime);
+    updateRSSI(currentTimeUs);
 
     if (feature(FEATURE_FAILSAFE)) {
 
-        if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
+        if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
             failsafeStartMonitoring();
         }
 
         failsafeUpdateState();
     }
 
-    throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+    throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
 
     if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
-        if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
+        if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
     } else {
         airmodeIsActivated = false;
     }
@@ -548,7 +548,7 @@ void processRx(uint32_t currentTime)
     ) {
         if (isUsingSticksForArming()) {
             if (throttleStatus == THROTTLE_LOW) {
-                if (masterConfig.armingConfig.auto_disarm_delay != 0
+                if (armingConfig()->auto_disarm_delay != 0
                     && (int32_t)(disarmAt - millis()) < 0
                 ) {
                     // auto-disarm configured and delay is over
@@ -561,9 +561,9 @@ void processRx(uint32_t currentTime)
                 }
             } else {
                 // throttle is not low
-                if (masterConfig.armingConfig.auto_disarm_delay != 0) {
+                if (armingConfig()->auto_disarm_delay != 0) {
                     // extend disarm time
-                    disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;
+                    disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
                 }
 
                 if (armedBeeperOn) {
@@ -583,7 +583,7 @@ void processRx(uint32_t currentTime)
         }
     }
 
-    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.armingConfig.disarm_kill_switch);
+    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
 
     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
         updateInflightCalibrationState();
@@ -661,14 +661,14 @@ void processRx(uint32_t currentTime)
         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
     }
 
-    if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
+    if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
     }
 
 #ifdef TELEMETRY
     if (feature(FEATURE_TELEMETRY)) {
-        if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
+        if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
-                (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
+                (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 
             releaseSharedTelemetryPorts();
         } else {
@@ -687,15 +687,15 @@ void processRx(uint32_t currentTime)
 void subTaskPidController(void)
 {
     uint32_t startTime;
-    if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
+    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
     // PID - note this is function pointer set by setPIDController()
     pidController(
         &currentProfile->pidProfile,
         masterConfig.max_angle_inclination,
         &masterConfig.accelerometerTrims,
-        masterConfig.rxConfig.midrc
+        rxConfig()->midrc
     );
-    if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
+    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
 }
 
 void subTaskMainSubprocesses(void)
@@ -704,8 +704,8 @@ void subTaskMainSubprocesses(void)
     const uint32_t startTime = micros();
 
     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
-    if (gyro.temperature) {
+    if (gyro.dev.temperature) {
-        gyro.temperature(&telemTemperature1);
+        gyro.dev.temperature(&telemTemperature1);
     }
 
     #ifdef MAG
@@ -732,21 +732,21 @@ void subTaskMainSubprocesses(void)
         // sticks, do not process yaw input from the rx.  We do this so the
         // motors do not spin up while we are trying to arm or disarm.
         // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
-        if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
+        if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
     #ifndef USE_QUAD_MIXER_ONLY
     #ifdef USE_SERVOS
-                    && !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
+                    && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
     #endif
-                    && masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE
+                    && mixerConfig()->mixerMode != MIXER_AIRPLANE
-                    && masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING
+                    && mixerConfig()->mixerMode != MIXER_FLYING_WING
     #endif
         ) {
             rcCommand[YAW] = 0;
             setpointRate[YAW] = 0;
         }
 
-        if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+        if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-            rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
+            rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
         }
 
         processRcCommand();
@@ -772,7 +772,7 @@ void subTaskMainSubprocesses(void)
     #ifdef TRANSPONDER
         transponderUpdate(startTime);
     #endif
-    if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
+        DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 }
 
 void subTaskMotorUpdate(void)
@@ -798,12 +798,12 @@ void subTaskMotorUpdate(void)
     if (motorControlEnable) {
         writeMotors();
     }
-    if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
+    DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 }
 
 uint8_t setPidUpdateCountDown(void)
 {
-    if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
+    if (gyroConfig()->gyro_soft_lpf_hz) {
         return masterConfig.pid_process_denom - 1;
     } else {
         return 1;
@@ -811,9 +811,9 @@ uint8_t setPidUpdateCountDown(void)
 }
 
 // Function for loop trigger
-void taskMainPidLoopCheck(uint32_t currentTime)
+void taskMainPidLoop(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTime);
+    UNUSED(currentTimeUs);
 
     static bool runTaskMainSubprocesses;
     static uint8_t pidUpdateCountdown;
@@ -830,7 +830,15 @@ void taskMainPidLoopCheck(uint32_t currentTime)
         runTaskMainSubprocesses = false;
     }
 
+    // DEBUG_PIDLOOP, timings for:
+    // 0 - gyroUpdate()
+    // 1 - pidController()
+    // 2 - subTaskMainSubprocesses()
+    // 3 - subTaskMotorUpdate()
+    uint32_t startTime;
+    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
     gyroUpdate();
+    if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;}
 
     if (pidUpdateCountdown) {
         pidUpdateCountdown--;

src/main/fc/mw.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "common/time.h"
+
 extern int16_t magHold;
 extern bool isRXDataNew;
 
@@ -27,7 +29,8 @@ void handleInflightCalibrationStickPosition();
 void mwDisarm(void);
 void mwArm(void);
 
-void processRx(uint32_t currentTime);
+void processRx(timeUs_t currentTimeUs);
 void updateLEDs(void);
 void updateRcCommands(void);
 
+void taskMainPidLoop(timeUs_t currentTimeUs);

src/main/fc/rc_controls.c

@@ -471,6 +471,16 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
         .mode = ADJUSTMENT_MODE_STEP,
         .data = { .stepConfig = { .step = 1 }}
+    },
+    {
+        .adjustmentFunction = ADJUSTMENT_D_SETPOINT,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    },
+    {
+        .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
     }
 };
 
@@ -579,7 +589,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
         case ADJUSTMENT_ROLL_D:
             newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
             pidProfile->D8[PIDROLL] = newValue;
-                blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
             break;
         case ADJUSTMENT_YAW_P:
             newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
@@ -601,6 +611,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             controlRateConfig->rcYawRate8 = newValue;
             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
             break;
+        case ADJUSTMENT_D_SETPOINT:
+            newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in serial_cli.c
+            pidProfile->dtermSetpointWeight = newValue;
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
+        case ADJUSTMENT_D_SETPOINT_TRANSITION:
+            newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            pidProfile->setpointRelaxRatio = newValue;
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
         default:
             break;
     };
@@ -676,7 +694,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
                 continue;
             }
 
-            applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
+            applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
+            pidInitConfig(pidProfile);
         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
             uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
             uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;

src/main/fc/rc_controls.h

@@ -162,13 +162,13 @@ typedef struct rcControlsConfig_s {
     uint8_t yaw_deadband;                   // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
     uint8_t alt_hold_deadband;              // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
+    int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
 } rcControlsConfig_t;
 
 typedef struct armingConfig_s {
     uint8_t gyro_cal_on_first_arm;          // allow disarm/arm on throttle down + roll left/right
     uint8_t disarm_kill_switch;             // allow disarm via AUX switch regardless of throttle value
     uint8_t auto_disarm_delay;              // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
-    uint8_t small_angle;
 } armingConfig_t;
 
 bool areUsingSticksToArm(void);
@@ -205,6 +205,8 @@ typedef enum {
     ADJUSTMENT_ROLL_I,
     ADJUSTMENT_ROLL_D,
     ADJUSTMENT_RC_RATE_YAW,
+    ADJUSTMENT_D_SETPOINT,
+    ADJUSTMENT_D_SETPOINT_TRANSITION,
     ADJUSTMENT_FUNCTION_COUNT,
 
 } adjustmentFunction_e;

src/main/flight/altitudehold.c

@@ -204,9 +204,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
     return result;
 }
 
-void calculateEstimatedAltitude(uint32_t currentTime)
+void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 {
-    static uint32_t previousTime;
+    static timeUs_t previousTimeUs;
     uint32_t dTime;
     int32_t baroVel;
     float dt;
@@ -224,11 +224,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float sonarTransition;
 #endif
 
-    dTime = currentTime - previousTime;
+    dTime = currentTimeUs - previousTimeUs;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
 
-    previousTime = currentTime;
+    previousTimeUs = currentTimeUs;
 
 #ifdef BARO
     if (!isBaroCalibrationComplete()) {
@@ -237,9 +237,9 @@ void calculateEstimatedAltitude(uint32_t currentTime)
         accAlt = 0;
     }
 
-    BaroAlt = baroCalculateAltitude();
+    baro.BaroAlt = baroCalculateAltitude();
 #else
-    BaroAlt = 0;
+    baro.BaroAlt = 0;
 #endif
 
 #ifdef SONAR
@@ -248,14 +248,14 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
         // just use the SONAR
-        baroAlt_offset = BaroAlt - sonarAlt;
+        baroAlt_offset = baro.BaroAlt - sonarAlt;
-        BaroAlt = sonarAlt;
+        baro.BaroAlt = sonarAlt;
     } else {
-        BaroAlt -= baroAlt_offset;
+        baro.BaroAlt -= baroAlt_offset;
         if (sonarAlt > 0  && sonarAlt <= sonarMaxAltWithTiltCm) {
             // SONAR in range, so use complementary filter
             sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
-            BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
+            baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition);
         }
     }
 #endif
@@ -272,7 +272,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     // Integrator - Altitude in cm
     accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                                                 // integrate velocity to get distance (x= a/2 * t^2)
-    accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
+    accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
     vel += vel_acc;
 
 #ifdef DEBUG_ALT_HOLD
@@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 #ifdef SONAR
     if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
         // the sonar has the best range
-        EstAlt = BaroAlt;
+        EstAlt = baro.BaroAlt;
     } else {
         EstAlt = accAlt;
     }
@@ -300,8 +300,8 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     EstAlt = accAlt;
 #endif
 
-    baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
+    baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
-    lastBaroAlt = BaroAlt;
+    lastBaroAlt = baro.BaroAlt;
 
     baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
     baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero

src/main/flight/altitudehold.h

@@ -20,7 +20,7 @@
 extern int32_t AltHold;
 extern int32_t vario;
 
-void calculateEstimatedAltitude(uint32_t currentTime);
+void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 
 struct pidProfile_s;
 struct barometerConfig_s;

src/main/flight/imu.c

@@ -67,17 +67,14 @@ float smallAngleCosZ = 0;
 float magneticDeclination = 0.0f;       // calculated at startup from config
 static bool isAccelUpdatedAtLeastOnce = false;
 
-static imuRuntimeConfig_t *imuRuntimeConfig;
+static imuRuntimeConfig_t imuRuntimeConfig;
 static pidProfile_t *pidProfile;
-static accDeadband_t *accDeadband;
 
 STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
 static float rMat[3][3];
 
 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 
-static float gyroScale;
-
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 {
     float q1q1 = sq(q1);
@@ -105,24 +102,25 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 }
 
 void imuConfigure(
-    imuRuntimeConfig_t *initialImuRuntimeConfig,
+    imuConfig_t *imuConfig,
     pidProfile_t *initialPidProfile,
-    accDeadband_t *initialAccDeadband,
     uint16_t throttle_correction_angle
 )
 {
-    imuRuntimeConfig = initialImuRuntimeConfig;
+    imuRuntimeConfig.dcm_kp = imuConfig->dcm_kp / 10000.0f;
+    imuRuntimeConfig.dcm_ki = imuConfig->dcm_ki / 10000.0f;
+    imuRuntimeConfig.acc_unarmedcal = imuConfig->acc_unarmedcal;
+    imuRuntimeConfig.small_angle = imuConfig->small_angle;
+
     pidProfile = initialPidProfile;
-    accDeadband = initialAccDeadband;
     fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 }
 
 void imuInit(void)
 {
-    smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
+    smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
-    gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
+    accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
-    accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
 
     imuComputeRotationMatrix();
 }
@@ -174,27 +172,27 @@ void imuCalculateAcceleration(uint32_t deltaT)
     // deltaT is measured in us ticks
     dT = (float)deltaT * 1e-6f;
 
-    accel_ned.V.X = accSmooth[0];
+    accel_ned.V.X = acc.accSmooth[X];
-    accel_ned.V.Y = accSmooth[1];
+    accel_ned.V.Y = acc.accSmooth[Y];
-    accel_ned.V.Z = accSmooth[2];
+    accel_ned.V.Z = acc.accSmooth[Z];
 
     imuTransformVectorBodyToEarth(&accel_ned);
 
-    if (imuRuntimeConfig->acc_unarmedcal == 1) {
+    if (imuRuntimeConfig.acc_unarmedcal == 1) {
         if (!ARMING_FLAG(ARMED)) {
             accZoffset -= accZoffset / 64;
             accZoffset += accel_ned.V.Z;
         }
         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
     } else
-        accel_ned.V.Z -= acc.acc_1G;
+        accel_ned.V.Z -= acc.dev.acc_1G;
 
     accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 
     // apply Deadband to reduce integration drift and vibration influence
-    accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
+    accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy);
-    accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
+    accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy);
-    accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
+    accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z);
 
     // sum up Values for later integration to get velocity and distance
     accTimeSum += deltaT;
@@ -286,10 +284,10 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Compute and apply integral feedback if enabled
-    if(imuRuntimeConfig->dcm_ki > 0.0f) {
+    if(imuRuntimeConfig.dcm_ki > 0.0f) {
         // Stop integrating if spinning beyond the certain limit
         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
-            float dcmKiGain = imuRuntimeConfig->dcm_ki;
+            float dcmKiGain = imuRuntimeConfig.dcm_ki;
             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
             integralFBy += dcmKiGain * ey * dt;
             integralFBz += dcmKiGain * ez * dt;
@@ -302,7 +300,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
-    float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
+    float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
 
     // Apply proportional and integral feedback
     gx += dcmKpGain * ex + integralFBx;
@@ -357,10 +355,10 @@ static bool imuIsAccelerometerHealthy(void)
     int32_t accMagnitude = 0;
 
     for (axis = 0; axis < 3; axis++) {
-        accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
+        accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
     }
 
-    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.acc_1G));
+    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));
 
     // Accept accel readings only in range 0.90g - 1.10g
     return (81 < accMagnitude) && (accMagnitude < 121);
@@ -368,10 +366,10 @@ static bool imuIsAccelerometerHealthy(void)
 
 static bool isMagnetometerHealthy(void)
 {
-    return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
+    return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
 }
 
-static void imuCalculateEstimatedAttitude(uint32_t currentTime)
+static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 {
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
@@ -379,8 +377,8 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
     bool useMag = false;
     bool useYaw = false;
 
-    uint32_t deltaT = currentTime - previousIMUUpdateTime;
+    uint32_t deltaT = currentTimeUs - previousIMUUpdateTime;
-    previousIMUUpdateTime = currentTime;
+    previousIMUUpdateTime = currentTimeUs;
 
     if (imuIsAccelerometerHealthy()) {
         useAcc = true;
@@ -398,9 +396,9 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
 #endif
 
     imuMahonyAHRSupdate(deltaT * 1e-6f,
-                        gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
+                        DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
-                        useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
+                        useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
-                        useMag, magADC[X], magADC[Y], magADC[Z],
+                        useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
                         useYaw, rawYawError);
 
     imuUpdateEulerAngles();
@@ -416,14 +414,14 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
     }
 }
 
-void imuUpdateAttitude(uint32_t currentTime)
+void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
-        imuCalculateEstimatedAttitude(currentTime);
+        imuCalculateEstimatedAttitude(currentTimeUs);
     } else {
-        accSmooth[X] = 0;
+        acc.accSmooth[X] = 0;
-        accSmooth[Y] = 0;
+        acc.accSmooth[Y] = 0;
-        accSmooth[Z] = 0;
+        acc.accSmooth[Z] = 0;
     }
 }
 

src/main/flight/imu.h

@@ -19,6 +19,7 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/time.h"
 
 #include "sensors/acceleration.h"
 
@@ -51,12 +52,25 @@ typedef struct accDeadband_s {
     uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
 } accDeadband_t;
 
+typedef struct throttleCorrectionConfig_s {
+    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
+    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
+} throttleCorrectionConfig_t;
+
+typedef struct imuConfig_s {
+    uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
+    uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
+    uint8_t small_angle;
+    uint8_t acc_unarmedcal;                 // turn automatic acc compensation on/off
+    accDeadband_t accDeadband;
+} imuConfig_t;
+
 typedef struct imuRuntimeConfig_s {
-    uint8_t acc_cut_hz;
-    uint8_t acc_unarmedcal;
     float dcm_ki;
     float dcm_kp;
+    uint8_t acc_unarmedcal;
     uint8_t small_angle;
+    accDeadband_t accDeadband;
 } imuRuntimeConfig_t;
 
 typedef enum {
@@ -77,17 +91,16 @@ typedef struct accProcessor_s {
 
 struct pidProfile_s;
 void imuConfigure(
-    imuRuntimeConfig_t *initialImuRuntimeConfig,
+    imuConfig_t *imuConfig,
     struct pidProfile_s *initialPidProfile,
-    accDeadband_t *initialAccDeadband,
     uint16_t throttle_correction_angle
 );
 
 float getCosTiltAngle(void);
-void calculateEstimatedAltitude(uint32_t currentTime);
+void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 union rollAndPitchTrims_u;
 void imuUpdateAccelerometer(union rollAndPitchTrims_u *accelerometerTrims);
-void imuUpdateAttitude(uint32_t currentTime);
+void imuUpdateAttitude(timeUs_t currentTimeUs);
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);

src/main/flight/mixer.c

@@ -236,8 +236,8 @@ const mixer_t mixers[] = {
 
 static motorMixer_t *customMixers;
 
-static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
+static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow;
-static float rcCommandThrottleRange;
+static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
 
 uint8_t getMotorCount() {
     return motorCount;
@@ -257,21 +257,26 @@ void initEscEndpoints(void) {
 #ifdef USE_DSHOT
     if (isMotorProtocolDshot()) {
         disarmMotorOutput = DSHOT_DISARM_COMMAND;
-        minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset;
+        if (feature(FEATURE_3D))
-        maxMotorOutputNormal = DSHOT_MAX_THROTTLE;
+            motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
-        deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes
+        else
-        deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE;  // TODO - Not working yet !! Mixer requires some throttle rescaling changes
+            motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
+        motorOutputHigh = DSHOT_MAX_THROTTLE;
+        deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
+        deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
     } else
 #endif
     {
         disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
-        minMotorOutputNormal = motorConfig->minthrottle;
+        motorOutputLow = motorConfig->minthrottle;
-        maxMotorOutputNormal = motorConfig->maxthrottle;
+        motorOutputHigh = motorConfig->maxthrottle;
         deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
         deadbandMotor3dLow = flight3DConfig->deadband3d_low;
     }
 
-    rcCommandThrottleRange = (2000 - rxConfig->mincheck);
+    rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
+    rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
+    rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
 }
 
 void mixerUseConfigs(
@@ -286,7 +291,6 @@ void mixerUseConfigs(
     mixerConfig = mixerConfigToUse;
     airplaneConfig = airplaneConfigToUse;
     rxConfig = rxConfigToUse;
-    initEscEndpoints();
 }
 
 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@@ -294,6 +298,8 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
     currentMixerMode = mixerMode;
 
     customMixers = initialCustomMixers;
+
+    initEscEndpoints();
 }
 
 #ifndef USE_QUAD_MIXER_ONLY
@@ -412,11 +418,11 @@ void mixTable(pidProfile_t *pidProfile)
 
     // Scale roll/pitch/yaw uniformly to fit within throttle range
     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
-    float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0;
+    float motorMix[MAX_SUPPORTED_MOTORS];
-    float throttleRange = 0, throttle = 0;
+    float motorOutputRange = 0, throttle = 0, currentThrottleInputRange = 0, motorMixRange = 0, motorMixMax = 0, motorMixMin = 0;
-    float motorOutputRange, motorMixRange;
     uint16_t motorOutputMin, motorOutputMax;
     static uint16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
+    bool mixerInversion = false;
 
     // Find min and max throttle based on condition.
     if (feature(FEATURE_3D)) {
@@ -424,30 +430,38 @@ void mixTable(pidProfile_t *pidProfile)
 
         if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
             motorOutputMax = deadbandMotor3dLow;
-            motorOutputMin = minMotorOutputNormal;
+            motorOutputMin = motorOutputLow;
             throttlePrevious = rcCommand[THROTTLE];
-            throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle;
+            throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
+            currentThrottleInputRange = rcCommandThrottleRange3dLow;
+            if(isMotorProtocolDshot()) mixerInversion = true;
         } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
-            motorOutputMax = maxMotorOutputNormal;
+            motorOutputMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
             throttlePrevious = rcCommand[THROTTLE];
-            throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle;
+            throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
+            currentThrottleInputRange = rcCommandThrottleRange3dHigh;
         } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)))  { // Deadband handling from negative to positive
-            throttle = deadbandMotor3dLow;
+            motorOutputMax = deadbandMotor3dLow;
-            motorOutputMin = motorOutputMax = minMotorOutputNormal;
+            motorOutputMin = motorOutputLow;
+            throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
+            currentThrottleInputRange = rcCommandThrottleRange3dLow;
+            if(isMotorProtocolDshot()) mixerInversion = true;
         } else {  // Deadband handling from positive to negative
-            motorOutputMax = maxMotorOutputNormal;
+            motorOutputMax = motorOutputHigh;
-            throttle = motorOutputMin = deadbandMotor3dHigh;
+            motorOutputMin = deadbandMotor3dHigh;
+            throttle = 0;
+            currentThrottleInputRange = rcCommandThrottleRange3dHigh;
         }
     } else {
-        throttle = rcCommand[THROTTLE];
+        throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
-        motorOutputMin = minMotorOutputNormal;
+        currentThrottleInputRange = rcCommandThrottleRange;
-        motorOutputMax = maxMotorOutputNormal;
+        motorOutputMin = motorOutputLow;
+        motorOutputMax = motorOutputHigh;
     }
 
+    throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
     motorOutputRange = motorOutputMax - motorOutputMin;
-    throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f);
-    throttleRange = motorOutputMax - motorOutputMin;
 
     float scaledAxisPIDf[3];
     // Limit the PIDsum
@@ -486,7 +500,12 @@ void mixTable(pidProfile_t *pidProfile)
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (i = 0; i < motorCount; i++) {
-        motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) );
+        motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
+
+        // Dshot works exactly opposite in lower 3D section.
+        if (mixerInversion) {
+            motor[i] = motorOutputMin + (motorOutputMax - motor[i]);
+        }
 
         if (failsafeIsActive()) {
             if (isMotorProtocolDshot())
@@ -510,7 +529,7 @@ void mixTable(pidProfile_t *pidProfile)
         const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000);
 
         // Only makes sense when it's within the range
-        if (maxThrottleStep < throttleRange) {
+        if (maxThrottleStep < motorOutputRange) {
             static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
 
             motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep);  // Only limit accelerating situation

src/main/flight/mixer.h

@@ -43,8 +43,8 @@
 #define DSHOT_DISARM_COMMAND      0
 #define DSHOT_MIN_THROTTLE       48
 #define DSHOT_MAX_THROTTLE     2047
-#define DSHOT_3D_MAX_POSITIVE  1047 // TODO - Not working yet!! Mixer requires some throttle rescaling changes
+#define DSHOT_3D_DEADBAND_LOW  1047
-#define DSHOT_3D_MIN_NEGATIVE  1048//  TODO - Not working yet!! Mixer requires some throttle rescaling changes
+#define DSHOT_3D_DEADBAND_HIGH 1048
 
 // Note: this is called MultiType/MULTITYPE_* in baseflight.
 typedef enum mixerMode

src/main/flight/navigation.c

@@ -26,8 +26,9 @@
 
 #include "build/debug.h"
 
-#include "common/maths.h"
 #include "common/axis.h"
+#include "common/maths.h"
+#include "common/time.h"
 
 #include "drivers/system.h"