diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index b0dfce3fd5..e8df1a8ea4 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -349,7 +349,7 @@ bool blackboxMayEditConfig()
 }
 
 static bool blackboxIsOnlyLoggingIntraframes() {
-    return masterConfig.blackbox_rate_num == 1 && masterConfig.blackbox_rate_denom == 32;
+    return masterConfig.blackboxConfig.rate_num == 1 && masterConfig.blackboxConfig.rate_denom == 32;
 }
 
 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
@@ -369,7 +369,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.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
+            return masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI;
 
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
@@ -407,7 +407,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
             return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
 
         case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
-            return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
+            return masterConfig.blackboxConfig.rate_num < masterConfig.blackboxConfig.rate_denom;
 
         case FLIGHT_LOG_FIELD_CONDITION_NEVER:
             return false;
@@ -758,22 +758,22 @@ static void validateBlackboxConfig()
 {
     int div;
 
-    if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0
-            || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) {
-        masterConfig.blackbox_rate_num = 1;
-        masterConfig.blackbox_rate_denom = 1;
+    if (masterConfig.blackboxConfig.rate_num == 0 || masterConfig.blackboxConfig.rate_denom == 0
+            || masterConfig.blackboxConfig.rate_num >= masterConfig.blackboxConfig.rate_denom) {
+        masterConfig.blackboxConfig.rate_num = 1;
+        masterConfig.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.blackbox_rate_num, masterConfig.blackbox_rate_denom);
+        div = gcd(masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
 
-        masterConfig.blackbox_rate_num /= div;
-        masterConfig.blackbox_rate_denom /= div;
+        masterConfig.blackboxConfig.rate_num /= div;
+        masterConfig.blackboxConfig.rate_denom /= div;
     }
 
     // If we've chosen an unsupported device, change the device to serial
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
 #endif
@@ -785,7 +785,7 @@ static void validateBlackboxConfig()
         break;
 
         default:
-            masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL;
+            masterConfig.blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
     }
 }
 
@@ -867,7 +867,7 @@ bool startedLoggingInTestMode = false;
 void startInTestMode(void)
 {
     if(!startedLoggingInTestMode) {
-        if (masterConfig.blackbox_device == BLACKBOX_DEVICE_SERIAL) {
+        if (masterConfig.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!
@@ -1172,7 +1172,7 @@ 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.blackbox_rate_num, masterConfig.blackbox_rate_denom);
+        BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d",                    masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
         BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                      masterConfig.motorConfig.minthrottle);
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                      masterConfig.motorConfig.maxthrottle);
         BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x",                     castFloatBytesToInt(gyro.scale));
@@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo()
             );
 
         BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                         gyro.targetLooptime);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d",                  masterConfig.gyro_sync_denom);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d",                  masterConfig.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);
@@ -1262,18 +1262,18 @@ static bool blackboxWriteSysinfo()
 
         BLACKBOX_PRINT_HEADER_LINE("deadband:%d",                         masterConfig.rcControlsConfig.deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d",                     masterConfig.rcControlsConfig.yaw_deadband);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d",                         masterConfig.gyro_lpf);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d",                   masterConfig.gyro_soft_type);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d",                  masterConfig.gyro_soft_lpf_hz);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d",                 masterConfig.gyro_soft_notch_hz_1,
-                                                                          masterConfig.gyro_soft_notch_hz_2);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d",             masterConfig.gyro_soft_notch_cutoff_1,
-                                                                          masterConfig.gyro_soft_notch_cutoff_2);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d",                         masterConfig.gyroConfig.gyro_lpf);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d",                   masterConfig.gyroConfig.gyro_soft_lpf_type);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d",                  masterConfig.gyroConfig.gyro_soft_lpf_hz);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d",                 masterConfig.gyroConfig.gyro_soft_notch_hz_1,
+                                                                          masterConfig.gyroConfig.gyro_soft_notch_hz_2);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d",             masterConfig.gyroConfig.gyro_soft_notch_cutoff_1,
+                                                                          masterConfig.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_hardware:%d",                     masterConfig.acc_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d",                    masterConfig.baro_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d",                     masterConfig.mag_hardware);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d",            masterConfig.gyro_cal_on_first_arm);
+        BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d",                     masterConfig.sensorSelectionConfig.acc_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d",                    masterConfig.sensorSelectionConfig.baro_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d",                     masterConfig.sensorSelectionConfig.mag_hardware);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d",            masterConfig.armingConfig.gyro_cal_on_first_arm);
         BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d",                 masterConfig.rxConfig.rcInterpolation);
         BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d",        masterConfig.rxConfig.rcInterpolationInterval);
         BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d",        masterConfig.rxConfig.airModeActivateThreshold);
@@ -1376,10 +1376,10 @@ static void blackboxCheckAndLogFlightMode()
  */
 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
 {
-    /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
+    /* Adding a magic shift of "masterConfig.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.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num;
+    return (pFrameIndex + masterConfig.blackboxConfig.rate_num - 1) % masterConfig.blackboxConfig.rate_denom < masterConfig.blackboxConfig.rate_num;
 }
 
 static bool blackboxShouldLogIFrame() {
@@ -1595,7 +1595,7 @@ void handleBlackbox(uint32_t currentTime)
         if (startedLoggingInTestMode) startedLoggingInTestMode = false;
     } else { // Only log in test mode if there is room!
 
-        if(masterConfig.blackbox_on_motor_test) {
+        if(masterConfig.blackboxConfig.on_motor_test) {
             // Handle Motor Test Mode
             if(inMotorTestMode()) {
                 if(blackboxState==BLACKBOX_STATE_STOPPED)
diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h
index 5650f2995a..b7fc3c6aa8 100644
--- a/src/main/blackbox/blackbox.h
+++ b/src/main/blackbox/blackbox.h
@@ -19,6 +19,13 @@
 
 #include "blackbox/blackbox_fielddefs.h"
 
+typedef struct blackboxConfig_s {
+    uint8_t rate_num;
+    uint8_t rate_denom;
+    uint8_t device;
+    uint8_t on_motor_test;
+} blackboxConfig_t;
+
 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
 
 void initBlackbox(void);
@@ -26,4 +33,4 @@ void handleBlackbox(uint32_t currentTime);
 void startBlackbox(void);
 void finishBlackbox(void);
 
-bool blackboxMayEditConfig();
+bool blackboxMayEditConfig(void);
diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c
index a141f016ed..b9a810d14b 100644
--- a/src/main/blackbox/blackbox_io.c
+++ b/src/main/blackbox/blackbox_io.c
@@ -65,7 +65,7 @@ static struct {
 
 void blackboxWrite(uint8_t value)
 {
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.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.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
 
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
@@ -479,7 +479,7 @@ void blackboxWriteFloat(float value)
  */
 void blackboxDeviceFlush(void)
 {
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.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.blackbox_device) {
+    switch (masterConfig.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.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
         case BLACKBOX_DEVICE_SERIAL:
             {
                 serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
@@ -604,7 +604,7 @@ bool blackboxDeviceOpen(void)
  */
 void blackboxDeviceClose(void)
 {
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.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.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
 #ifdef USE_SDCARD
         case BLACKBOX_DEVICE_SDCARD:
             return blackboxSDCardBeginLog();
@@ -772,7 +772,7 @@ bool blackboxDeviceEndLog(bool retainLog)
     (void) retainLog;
 #endif
 
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.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.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
         case BLACKBOX_DEVICE_SERIAL:
             return false;
 
@@ -821,7 +821,7 @@ void blackboxReplenishHeaderBudget()
 {
     int32_t freeSpace;
 
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
         case BLACKBOX_DEVICE_SERIAL:
             freeSpace = serialTxBytesFree(blackboxPort);
         break;
@@ -867,7 +867,7 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
     }
 
     // Handle failure:
-    switch (masterConfig.blackbox_device) {
+    switch (masterConfig.blackboxConfig.device) {
         case BLACKBOX_DEVICE_SERIAL:
             /*
              * One byte of the tx buffer isn't available for user data (due to its circular list implementation),
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 7ad339ccba..18808b5c6d 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/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.blackbox_rate_denom,1,32,1 }, 0 },
+    { "RATE DENOM",  OME_UINT8,   NULL,            &(OSD_UINT8_t){ &masterConfig.blackboxConfig.rate_denom,1,32,1 }, 0 },
 
 #ifdef USE_FLASHFS
     { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL,                                                       0 },
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 647be77336..32241d1265 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
 {
     { "-- FILTER GLB  --", OME_Label, NULL, NULL, 0 },
 
-    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &masterConfig.gyro_soft_lpf_hz,         0, 255, 1 }, 0 },
-    { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
-    { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
-    { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
-    { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
+    { "GYRO LPF",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &masterConfig.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 NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.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 NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
 
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 6fdf6e5c59..5b580a14e1 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -22,6 +22,7 @@
 
 #include "common/filter.h"
 #include "common/maths.h"
+#include "common/utils.h"
 
 #define M_LN2_FLOAT 0.69314718055994530942f
 #define M_PI_FLOAT  3.14159265358979323846f
@@ -29,6 +30,16 @@
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
+
+// NULL filter
+
+float nullFilterApply(void *filter, float input)
+{
+    UNUSED(filter);
+    return input;
+}
+
+
 // PT1 Low Pass filter
 
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
@@ -178,6 +189,12 @@ float firFilterApply(const firFilter_t *filter)
     return ret;
 }
 
+float firFilterUpdateAndApply(firFilter_t *filter, float input)
+{
+    firFilterUpdate(filter, input);
+    return firFilterApply(filter);
+}
+
 /*
  * Returns average of the last <count> items.
  */
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 2b0e75a431..0121c71b1d 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -62,6 +62,9 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef float (*filterApplyFnPtr)(void *filter, float input);
+
+float nullFilterApply(void *filter, float input);
 
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
@@ -77,6 +80,7 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
 void firFilterUpdate(firFilter_t *filter, float input);
 void firFilterUpdateAverage(firFilter_t *filter, float input);
 float firFilterApply(const firFilter_t *filter);
+float firFilterUpdateAndApply(firFilter_t *filter, float input);
 float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
 float firFilterCalcMovingAverage(const firFilter_t *filter);
 float firFilterLastInput(const firFilter_t *filter);
diff --git a/src/main/common/utils.h b/src/main/common/utils.h
index 23088d076d..2e27c930c2 100644
--- a/src/main/common/utils.h
+++ b/src/main/common/utils.h
@@ -32,7 +32,7 @@
 #define EXPAND_I(x) x
 #define EXPAND(x) EXPAND_I(x)
 
-#if !defined(USE_HAL_DRIVER)
+#if !defined(UNUSED)
 #define UNUSED(x) (void)(x)
 #endif
 #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h
index e8d59235dc..f1f9facba6 100644
--- a/src/main/config/config_eeprom.h
+++ b/src/main/config/config_eeprom.h
@@ -17,7 +17,7 @@
 
 #pragma once
 
-#define EEPROM_CONF_VERSION 145
+#define EEPROM_CONF_VERSION 146
 
 void initEEPROM(void);
 void writeEEPROM();
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index d3215cbd1f..390c6865ff 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -21,6 +21,8 @@
 
 #include "config/config_profile.h"
 
+#include "blackbox/blackbox.h"
+
 #include "cms/cms.h"
 
 #include "drivers/adc.h"
@@ -57,7 +59,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
-
+#include "sensors/compass.h"
 
 // System-wide
 typedef struct master_s {
@@ -65,7 +67,6 @@ typedef struct master_s {
     uint16_t size;
     uint8_t magic_be;                       // magic number, should be 0xBE
 
-    uint8_t mixerMode;
     uint32_t enabledFeatures;
 
     // motor/esc/servo related stuff
@@ -84,20 +85,13 @@ typedef struct master_s {
 #endif
 
     // global sensor-related stuff
+    sensorSelectionConfig_t sensorSelectionConfig;
     sensorAlignmentConfig_t sensorAlignmentConfig;
     boardAlignment_t boardAlignment;
 
     int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
-    uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
-    uint16_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
-    uint8_t gyro_sync_denom;                // Gyro sample divider
-    uint8_t gyro_soft_type;                 // Gyro Filter Type
-    uint8_t gyro_soft_lpf_hz;               // Biquad gyro lpf hz
-    uint16_t gyro_soft_notch_hz_1;          // Biquad gyro notch hz
-    uint16_t gyro_soft_notch_cutoff_1;      // Biquad gyro notch low cutoff
-    uint16_t gyro_soft_notch_hz_2;          // Biquad gyro notch hz
-    uint16_t gyro_soft_notch_cutoff_2;      // Biquad gyro notch low cutoff
+
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
 
@@ -106,11 +100,7 @@ typedef struct master_s {
     uint8_t debug_mode;                     // Processing denominator for PID controller vs gyro sampling rate
 
     gyroConfig_t gyroConfig;
-
-    uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
-    uint8_t baro_hardware;                  // Barometer hardware to use
-    int16_t mag_declination;                // Get your magnetic decliniation from here : http://magnetic-declination.com/
-                                            // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
+    compassConfig_t compassConfig;
 
     rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
 
@@ -132,17 +122,12 @@ typedef struct master_s {
 #endif
 
     uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
-    flightDynamicsTrims_t accZero;
-    flightDynamicsTrims_t magZero;
+    sensorTrims_t sensorTrims;
 
     rxConfig_t rxConfig;
     inputFilteringMode_e inputFilteringMode;  // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
 
-
-    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 armingConfig;
 
     // mixer-related configuration
     mixerConfig_t mixerConfig;
@@ -213,10 +198,7 @@ typedef struct master_s {
 #endif
 
 #ifdef BLACKBOX
-    uint8_t blackbox_rate_num;
-    uint8_t blackbox_rate_denom;
-    uint8_t blackbox_device;
-    uint8_t blackbox_on_motor_test;
+    blackboxConfig_t blackboxConfig;
 #endif
 
     uint32_t beeper_off_flags;
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index c2a7e81dd3..dee3b592d1 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -16,20 +16,20 @@
  */
 
 // FC configuration
-#define PG_FAILSAFE_CONFIG 1
-#define PG_BOARD_ALIGNMENT 2
-#define PG_GIMBAL_CONFIG 3
-#define PG_MOTOR_MIXER 4
-#define PG_BLACKBOX_CONFIG 5
-#define PG_MOTOR_AND_SERVO_CONFIG 6
-#define PG_SENSOR_SELECTION_CONFIG 7
-#define PG_SENSOR_ALIGNMENT_CONFIG 8
-#define PG_SENSOR_TRIMS 9
-#define PG_GYRO_CONFIG 10
-#define PG_BATTERY_CONFIG 11
-#define PG_CONTROL_RATE_PROFILES 12
-#define PG_SERIAL_CONFIG 13
-#define PG_PID_PROFILE 14
+#define PG_FAILSAFE_CONFIG 1 // strruct OK
+#define PG_BOARD_ALIGNMENT 2 // struct OK
+#define PG_GIMBAL_CONFIG 3 // struct OK
+#define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t
+#define PG_BLACKBOX_CONFIG 5 // struct OK
+#define PG_MOTOR_CONFIG 6 // struct OK
+#define PG_SENSOR_SELECTION_CONFIG 7 // struct OK
+#define PG_SENSOR_ALIGNMENT_CONFIG 8 // struct OK
+#define PG_SENSOR_TRIMS 9 // struct OK
+#define PG_GYRO_CONFIG 10 // PR outstanding, need to think about pid_process_denom
+#define PG_BATTERY_CONFIG 11 // struct OK
+#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_ARMING_CONFIG 16
 #define PG_TRANSPONDER_CONFIG 17
diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 0020d770fa..7e43b73078 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -23,6 +23,15 @@
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
+#define GYRO_LPF_256HZ      0
+#define GYRO_LPF_188HZ      1
+#define GYRO_LPF_98HZ       2
+#define GYRO_LPF_42HZ       3
+#define GYRO_LPF_20HZ       4
+#define GYRO_LPF_10HZ       5
+#define GYRO_LPF_5HZ        6
+#define GYRO_LPF_NONE       7
+
 typedef struct gyro_s {
     sensorGyroInitFuncPtr init;                             // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
index 36ec35c1f2..3c29f70da5 100644
--- a/src/main/drivers/adc_impl.h
+++ b/src/main/drivers/adc_impl.h
@@ -33,7 +33,8 @@ typedef enum ADCDevice {
     ADCDEV_1   = 0,
 #if defined(STM32F3)
     ADCDEV_2,
-    ADCDEV_MAX = ADCDEV_2,
+    ADCDEV_3,
+    ADCDEV_MAX = ADCDEV_3,
 #elif defined(STM32F4) || defined(STM32F7)
     ADCDEV_2,
     ADCDEV_3,
diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c
index de158a69e2..13623a4e17 100644
--- a/src/main/drivers/adc_stm32f30x.c
+++ b/src/main/drivers/adc_stm32f30x.c
@@ -41,10 +41,11 @@
 const adcDevice_t adcHardware[] = {
     { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 },
 #ifdef ADC24_DMA_REMAP
-    { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 }
+    { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 },
 #else
-    { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 }
+    { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 },
 #endif
+    { .ADCx = ADC3, .rccADC = RCC_AHB(ADC34), .DMAy_Channelx = DMA2_Channel5 }
 };
 
 const adcTagMap_t adcTagMap[] = {
@@ -97,6 +98,9 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
     if (instance == ADC2)
         return ADCDEV_2;
 
+    if (instance == ADC3)
+        return ADCDEV_3;
+
     return ADCINVALID;
 }
 
@@ -152,7 +156,14 @@ void adcInit(adcConfig_t *config)
         return;
     }
     
-    RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256);  // 72 MHz divided by 256 = 281.25 kHz
+    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
+    } else {
+        // enable clock for ADC3+4
+        RCC_ADCCLKConfig(RCC_ADC34PLLCLK_Div256);  // 72 MHz divided by 256 = 281.25 kHz
+    }
+
     RCC_ClockCmd(adc.rccADC, ENABLE);
 
     dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, 0);
diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c
index dbdc2244c9..4e9f02622b 100644
--- a/src/main/drivers/gyro_sync.c
+++ b/src/main/drivers/gyro_sync.c
@@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro)
     return gyro->intStatus();
 }
 
-#define GYRO_LPF_256HZ      0
-#define GYRO_LPF_188HZ      1
-#define GYRO_LPF_98HZ       2
-#define GYRO_LPF_42HZ       3
-#define GYRO_LPF_20HZ       4
-#define GYRO_LPF_10HZ       5
-#define GYRO_LPF_5HZ        6
-#define GYRO_LPF_NONE       7
-
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
 {
     int gyroSamplePeriod;
diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c
index d60e3af649..0390a0add2 100644
--- a/src/main/drivers/io.c
+++ b/src/main/drivers/io.c
@@ -15,12 +15,12 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
- #include "common/utils.h"
-
 #include "io.h"
 #include "io_impl.h"
 #include "rcc.h"
 
+#include "common/utils.h"
+
 #include "target.h"
 
 // io ports defs are stored in array by index now
diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h
index 470cf8163d..5796bbfc1d 100644
--- a/src/main/drivers/io_impl.h
+++ b/src/main/drivers/io_impl.h
@@ -50,6 +50,8 @@ uint16_t IO_Pin(IO_t io);
 
 #define IO_GPIOBYTAG(tag) IO_GPIO(IOGetByTag(tag))
 #define IO_PINBYTAG(tag) IO_Pin(IOGetByTag(tag))
+#define IO_GPIOPortIdxByTag(tag) DEFIO_TAG_GPIOID(tag)
+#define IO_GPIOPinIdxByTag(tag) DEFIO_TAG_PIN(tag)
 
 uint32_t IO_EXTI_Line(IO_t io);
 ioRec_t *IO_Rec(IO_t io);
diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c
index 5adf8f63b0..b0db2103e7 100644
--- a/src/main/drivers/pwm_output_stm32f3xx.c
+++ b/src/main/drivers/pwm_output_stm32f3xx.c
@@ -71,6 +71,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
+    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
 
diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c
index 0773efe08b..da5eb75440 100644
--- a/src/main/drivers/pwm_output_stm32f4xx.c
+++ b/src/main/drivers/pwm_output_stm32f4xx.c
@@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
+    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
 
diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c
index 32ce620547..2a52652d1a 100644
--- a/src/main/drivers/pwm_output_stm32f7xx.c
+++ b/src/main/drivers/pwm_output_stm32f7xx.c
@@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
+    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
 
@@ -88,7 +92,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
         packet <<= 1;
     }
 
-    if(  HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
+    if(HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
     {
       /* Starting PWM generation Error */
         return;
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index cd154bf9eb..9b083349d6 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -69,7 +69,14 @@ static void uartReconfigure(uartPort_t *uartPort)
     USART_Cmd(uartPort->USARTx, DISABLE);
 
     USART_InitStructure.USART_BaudRate = uartPort->port.baudRate;
-    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+
+    // according to the stm32 documentation wordlen has to be 9 for parity bits
+    // this does not seem to matter for rx but will give bad data on tx!
+    if (uartPort->port.options & SERIAL_PARITY_EVEN) {
+        USART_InitStructure.USART_WordLength = USART_WordLength_9b;
+    } else {
+        USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+    }
 
     USART_InitStructure.USART_StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_StopBits_2 : USART_StopBits_1;
     USART_InitStructure.USART_Parity   = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_Parity_Even : USART_Parity_No;
diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c
index 0e83575b02..ccfedb3972 100644
--- a/src/main/drivers/serial_uart_stm32f4xx.c
+++ b/src/main/drivers/serial_uart_stm32f4xx.c
@@ -348,12 +348,12 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
     else {
         if (mode & MODE_TX) {
             IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
+            IOConfigGPIOAF(tx, IOCFG_AF_PP_UP, uart->af);
         }
 
         if (mode & MODE_RX) {
             IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
+            IOConfigGPIOAF(rx, IOCFG_AF_PP_UP, uart->af);
         }
     }
 
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 93c8832107..92cddade9a 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -430,6 +430,9 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
     batteryConfig->batteryCapacity = 0;
     batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
     batteryConfig->batterynotpresentlevel = 55; // VBAT below 5.5 V will be igonored
+    batteryConfig->useVBatAlerts = true;
+    batteryConfig->useConsumptionAlerts = false;
+    batteryConfig->consumptionWarningPercentage = 10;
 }
 
 #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
@@ -558,48 +561,48 @@ void createDefaultConfig(master_t *config)
 #endif
 
     config->version = EEPROM_CONF_VERSION;
-    config->mixerMode = MIXER_QUADX;
+    config->mixerConfig.mixerMode = MIXER_QUADX;
 
     // global settings
     config->current_profile_index = 0;    // default profile
     config->dcm_kp = 2500;                // 1.0 * 10000
     config->dcm_ki = 0;                   // 0.003 * 10000
-    config->gyro_lpf = 0;                 // 256HZ default
+    config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ;    // 256HZ default
 #ifdef STM32F10X
-    config->gyro_sync_denom = 8;
+    config->gyroConfig.gyro_sync_denom = 8;
     config->pid_process_denom = 1;
 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500)  || defined(USE_GYRO_SPI_ICM20689)
-    config->gyro_sync_denom = 1;
+    config->gyroConfig.gyro_sync_denom = 1;
     config->pid_process_denom = 4;
 #else
-    config->gyro_sync_denom = 4;
+    config->gyroConfig.gyro_sync_denom = 4;
     config->pid_process_denom = 2;
 #endif
-    config->gyro_soft_type = FILTER_PT1;
-    config->gyro_soft_lpf_hz = 90;
-    config->gyro_soft_notch_hz_1 = 400;
-    config->gyro_soft_notch_cutoff_1 = 300;
-    config->gyro_soft_notch_hz_2 = 200;
-    config->gyro_soft_notch_cutoff_2 = 100;
+    config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1;
+    config->gyroConfig.gyro_soft_lpf_hz = 90;
+    config->gyroConfig.gyro_soft_notch_hz_1 = 400;
+    config->gyroConfig.gyro_soft_notch_cutoff_1 = 300;
+    config->gyroConfig.gyro_soft_notch_hz_2 = 200;
+    config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
 
     config->debug_mode = DEBUG_NONE;
 
-    resetAccelerometerTrims(&config->accZero);
+    resetAccelerometerTrims(&config->sensorTrims.accZero);
 
     resetSensorAlignment(&config->sensorAlignmentConfig);
 
     config->boardAlignment.rollDegrees = 0;
     config->boardAlignment.pitchDegrees = 0;
     config->boardAlignment.yawDegrees = 0;
-    config->acc_hardware = ACC_DEFAULT;     // default/autodetect
+    config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT;     // default/autodetect
     config->max_angle_inclination = 700;    // 70 degrees
     config->yaw_control_direction = 1;
     config->gyroConfig.gyroMovementCalibrationThreshold = 32;
 
     // xxx_hardware: 0:default/autodetect, 1: disable
-    config->mag_hardware = 1;
+    config->sensorSelectionConfig.mag_hardware = 1;
 
-    config->baro_hardware = 1;
+    config->sensorSelectionConfig.baro_hardware = 1;
 
     resetBatteryConfig(&config->batteryConfig);
 
@@ -663,10 +666,10 @@ void createDefaultConfig(master_t *config)
 
     config->inputFilteringMode = INPUT_FILTERING_DISABLED;
 
-    config->gyro_cal_on_first_arm = 0;  // TODO - Cleanup retarded arm support
-    config->disarm_kill_switch = 1;
-    config->auto_disarm_delay = 5;
-    config->small_angle = 25;
+    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->airplaneConfig.fixedwing_althold_dir = 1;
 
@@ -697,7 +700,7 @@ void createDefaultConfig(master_t *config)
 
     resetRollAndPitchTrims(&config->accelerometerTrims);
 
-    config->mag_declination = 0;
+    config->compassConfig.mag_declination = 0;
     config->acc_lpf_hz = 10.0f;
     config->accDeadband.xy = 40;
     config->accDeadband.z = 40;
@@ -768,17 +771,17 @@ void createDefaultConfig(master_t *config)
 #ifdef BLACKBOX
 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
     intFeatureSet(FEATURE_BLACKBOX, featuresPtr);
-    config->blackbox_device = BLACKBOX_DEVICE_FLASH;
+    config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH;
 #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
     intFeatureSet(FEATURE_BLACKBOX, featuresPtr);
-    config->blackbox_device = BLACKBOX_DEVICE_SDCARD;
+    config->blackboxConfig.device = BLACKBOX_DEVICE_SDCARD;
 #else
-    config->blackbox_device = BLACKBOX_DEVICE_SERIAL;
+    config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
 #endif
 
-    config->blackbox_rate_num = 1;
-    config->blackbox_rate_denom = 1;
-    config->blackbox_on_motor_test = 0; // default off
+    config->blackboxConfig.rate_num = 1;
+    config->blackboxConfig.rate_denom = 1;
+    config->blackboxConfig.on_motor_test = 0; // default off
 #endif // BLACKBOX
 
 #ifdef SERIALRX_UART
@@ -830,21 +833,6 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    // Prevent invalid notch cutoff
-    if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1)
-        masterConfig.gyro_soft_notch_hz_1 = 0;
-
-    if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2)
-        masterConfig.gyro_soft_notch_hz_2 = 0;
-
-    gyroUseConfig(&masterConfig.gyroConfig,
-        masterConfig.gyro_soft_lpf_hz,
-        masterConfig.gyro_soft_notch_hz_1,
-        masterConfig.gyro_soft_notch_cutoff_1,
-        masterConfig.gyro_soft_notch_hz_2,
-        masterConfig.gyro_soft_notch_cutoff_2,
-        masterConfig.gyro_soft_type);
-
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);
 #endif
@@ -855,7 +843,7 @@ void activateConfig(void)
 #endif
 
     useFailsafeConfig(&masterConfig.failsafeConfig);
-    setAccelerationTrims(&masterConfig.accZero);
+    setAccelerationTrims(&masterConfig.sensorTrims.accZero);
     setAccelerationFilter(masterConfig.acc_lpf_hz);
 
     mixerUseConfigs(
@@ -873,7 +861,7 @@ void activateConfig(void)
     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.small_angle;
+    imuRuntimeConfig.small_angle = masterConfig.armingConfig.small_angle;
 
     imuConfigure(
         &imuRuntimeConfig,
@@ -998,12 +986,30 @@ void validateAndFixConfig(void)
     if (!isSerialConfigValid(serialConfig)) {
         resetSerialConfig(serialConfig);
     }
-    
+
+    validateAndFixGyroConfig();
+
 #if defined(TARGET_VALIDATECONFIG)
     targetValidateConfiguration(&masterConfig);
 #endif
 }
 
+void validateAndFixGyroConfig(void)
+{
+    // Prevent invalid notch cutoff
+    if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) {
+        masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0;
+    }
+    if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
+        masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
+    }
+
+    if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.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;
+    }
+}
+
 void readEEPROMAndNotify(void)
 {
     // re-read written data
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 6f903e5d83..3a3c4334a6 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void);
 
 void saveConfigAndNotify(void);
 void validateAndFixConfig(void);
+void validateAndFixGyroConfig(void);
 void activateConfig(void);
 
 uint8_t getCurrentProfile(void);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index fb60f5591b..e6f999722c 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -313,7 +313,7 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
     }
 
-    if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
+    if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
     }
 
@@ -360,7 +360,7 @@ void initActiveBoxIds(void)
 #endif
 
 #ifdef USE_SERVOS
-    if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
+    if (masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
         activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
         activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
         activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
@@ -563,7 +563,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.mixerMode);
+        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
         sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
         break;
@@ -700,8 +700,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_ARMING_CONFIG:
-        sbufWriteU8(dst, masterConfig.auto_disarm_delay);
-        sbufWriteU8(dst, masterConfig.disarm_kill_switch);
+        sbufWriteU8(dst, masterConfig.armingConfig.auto_disarm_delay);
+        sbufWriteU8(dst, masterConfig.armingConfig.disarm_kill_switch);
         break;
 
     case MSP_LOOP_TIME:
@@ -797,7 +797,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
         sbufWriteU8(dst, 0);
 
-        sbufWriteU16(dst, masterConfig.mag_declination / 10);
+        sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10);
 
         sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
         sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
@@ -886,7 +886,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_MIXER:
-        sbufWriteU8(dst, masterConfig.mixerMode);
+        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
         break;
 
     case MSP_RX_CONFIG:
@@ -932,7 +932,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_BF_CONFIG:
-        sbufWriteU8(dst, masterConfig.mixerMode);
+        sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
 
         sbufWriteU32(dst, featureMask());
 
@@ -1005,9 +1005,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.blackbox_device);
-        sbufWriteU8(dst, masterConfig.blackbox_rate_num);
-        sbufWriteU8(dst, masterConfig.blackbox_rate_denom);
+        sbufWriteU8(dst, masterConfig.blackboxConfig.device);
+        sbufWriteU8(dst, masterConfig.blackboxConfig.rate_num);
+        sbufWriteU8(dst, masterConfig.blackboxConfig.rate_denom);
 #else
         sbufWriteU8(dst, 0); // Blackbox not supported
         sbufWriteU8(dst, 0);
@@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_ADVANCED_CONFIG:
-        if (masterConfig.gyro_lpf) {
+        if (masterConfig.gyroConfig.gyro_lpf) {
             sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
             sbufWriteU8(dst, 1);
         } else {
-            sbufWriteU8(dst, masterConfig.gyro_sync_denom);
+            sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
             sbufWriteU8(dst, masterConfig.pid_process_denom);
         }
         sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
@@ -1094,15 +1094,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_FILTER_CONFIG :
-        sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz);
+        sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
-        sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1);
-        sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1);
+        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
+        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
         sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
-        sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2);
-        sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2);
+        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
+        sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
         break;
 
     case MSP_PID_ADVANCED:
@@ -1121,9 +1121,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         break;
 
     case MSP_SENSOR_CONFIG:
-        sbufWriteU8(dst, masterConfig.acc_hardware);
-        sbufWriteU8(dst, masterConfig.baro_hardware);
-        sbufWriteU8(dst, masterConfig.mag_hardware);
+        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware);
+        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware);
+        sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware);
         break;
 
     case MSP_REBOOT:
@@ -1247,8 +1247,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         masterConfig.accelerometerTrims.values.roll  = sbufReadU16(src);
         break;
     case MSP_SET_ARMING_CONFIG:
-        masterConfig.auto_disarm_delay = sbufReadU8(src);
-        masterConfig.disarm_kill_switch = sbufReadU8(src);
+        masterConfig.armingConfig.auto_disarm_delay = sbufReadU8(src);
+        masterConfig.armingConfig.disarm_kill_switch = sbufReadU8(src);
         break;
 
     case MSP_SET_LOOP_TIME:
@@ -1355,7 +1355,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
         sbufReadU8(src);
 
-        masterConfig.mag_declination = sbufReadU16(src) * 10;
+        masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10;
 
         masterConfig.batteryConfig.vbatscale = sbufReadU8(src);           // actual vbatscale as intended
         masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
@@ -1413,13 +1413,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
         masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
         masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
-        masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
         break;
 
     case MSP_SET_RC_DEADBAND:
         masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
         masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
         masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
+        masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
         break;
 
     case MSP_SET_RESET_CURR_PID:
@@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_ADVANCED_CONFIG:
-        masterConfig.gyro_sync_denom = sbufReadU8(src);
+        masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
         masterConfig.pid_process_denom = sbufReadU8(src);
         masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
 #ifdef USE_DSHOT
@@ -1445,19 +1445,24 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_FILTER_CONFIG:
-        masterConfig.gyro_soft_lpf_hz = sbufReadU8(src);
+        masterConfig.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.gyro_soft_notch_hz_1 = sbufReadU16(src);
-            masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
+            masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
+            masterConfig.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.gyro_soft_notch_hz_2 = sbufReadU16(src);
-            masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
+            masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
+            masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
         }
+        // reinitialize the gyro filters with the new values
+        validateAndFixGyroConfig();
+        gyroInit(&masterConfig.gyroConfig);
+        // reinitialize the PID filters with the new values
+        pidInitFilters(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_PID_ADVANCED:
@@ -1476,9 +1481,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_SENSOR_CONFIG:
-        masterConfig.acc_hardware = sbufReadU8(src);
-        masterConfig.baro_hardware = sbufReadU8(src);
-        masterConfig.mag_hardware = sbufReadU8(src);
+        masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src);
+        masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src);
+        masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src);
         break;
 
     case MSP_RESET_CONF:
@@ -1510,9 +1515,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.blackbox_device = sbufReadU8(src);
-            masterConfig.blackbox_rate_num = sbufReadU8(src);
-            masterConfig.blackbox_rate_denom = sbufReadU8(src);
+            masterConfig.blackboxConfig.device = sbufReadU8(src);
+            masterConfig.blackboxConfig.rate_num = sbufReadU8(src);
+            masterConfig.blackboxConfig.rate_denom = sbufReadU8(src);
         }
         break;
 #endif
@@ -1657,7 +1662,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 
 #ifndef USE_QUAD_MIXER_ONLY
     case MSP_SET_MIXER:
-        masterConfig.mixerMode = sbufReadU8(src);
+        masterConfig.mixerConfig.mixerMode = sbufReadU8(src);
         break;
 #endif
 
@@ -1716,7 +1721,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #ifdef USE_QUAD_MIXER_ONLY
         sbufReadU8(src); // mixerMode ignored
 #else
-        masterConfig.mixerMode = sbufReadU8(src); // mixerMode
+        masterConfig.mixerConfig.mixerMode = sbufReadU8(src); // mixerMode
 #endif
 
         featureClearAll();
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 00719cf2b7..f9dcefd301 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -155,7 +155,7 @@ static void taskUpdateRxMain(uint32_t currentTime)
 static void taskUpdateCompass(uint32_t currentTime)
 {
     if (sensors(SENSOR_MAG)) {
-        compassUpdate(currentTime, &masterConfig.magZero);
+        compassUpdate(currentTime, &masterConfig.sensorTrims.magZero);
     }
 }
 #endif
diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c
index 430c217bec..aab4497f79 100644
--- a/src/main/fc/mw.c
+++ b/src/main/fc/mw.c
@@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised;
 float setpointRate[3];
 float rcInput[3];
 
-extern pidControllerFuncPtr pid_controller;
-
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 {
     masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
@@ -391,7 +389,7 @@ void mwArm(void)
 {
     static bool firstArmingCalibrationWasCompleted;
 
-    if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
+    if (masterConfig.armingConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
         gyroSetCalibrationCycles();
         armingCalibrationWasInitialised = true;
         firstArmingCalibrationWasCompleted = true;
@@ -420,7 +418,7 @@ void mwArm(void)
                 startBlackbox();
             }
 #endif
-            disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
+            disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
             //beep to indicate arming
 #ifdef GPS
@@ -550,7 +548,7 @@ void processRx(uint32_t currentTime)
     ) {
         if (isUsingSticksForArming()) {
             if (throttleStatus == THROTTLE_LOW) {
-                if (masterConfig.auto_disarm_delay != 0
+                if (masterConfig.armingConfig.auto_disarm_delay != 0
                     && (int32_t)(disarmAt - millis()) < 0
                 ) {
                     // auto-disarm configured and delay is over
@@ -563,9 +561,9 @@ void processRx(uint32_t currentTime)
                 }
             } else {
                 // throttle is not low
-                if (masterConfig.auto_disarm_delay != 0) {
+                if (masterConfig.armingConfig.auto_disarm_delay != 0) {
                     // extend disarm time
-                    disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
+                    disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;
                 }
 
                 if (armedBeeperOn) {
@@ -585,7 +583,7 @@ void processRx(uint32_t currentTime)
         }
     }
 
-    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch);
+    processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.armingConfig.disarm_kill_switch);
 
     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
         updateInflightCalibrationState();
@@ -663,7 +661,7 @@ void processRx(uint32_t currentTime)
         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
     }
 
-    if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
+    if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
     }
 
@@ -695,7 +693,7 @@ void subTaskPidController(void)
         &currentProfile->pidProfile,
         masterConfig.max_angle_inclination,
         &masterConfig.accelerometerTrims,
-        &masterConfig.rxConfig
+        masterConfig.rxConfig.midrc
     );
     if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
 }
@@ -737,10 +735,10 @@ void subTaskMainSubprocesses(void)
         if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
     #ifndef USE_QUAD_MIXER_ONLY
     #ifdef USE_SERVOS
-                    && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
+                    && !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
     #endif
-                    && masterConfig.mixerMode != MIXER_AIRPLANE
-                    && masterConfig.mixerMode != MIXER_FLYING_WING
+                    && masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE
+                    && masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING
     #endif
         ) {
             rcCommand[YAW] = 0;
@@ -805,7 +803,7 @@ void subTaskMotorUpdate(void)
 
 uint8_t setPidUpdateCountDown(void)
 {
-    if (masterConfig.gyro_soft_lpf_hz) {
+    if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
         return masterConfig.pid_process_denom - 1;
     } else {
         return 1;
diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
index 6c84dee566..6c7981bdfa 100644
--- a/src/main/fc/rc_controls.h
+++ b/src/main/fc/rc_controls.h
@@ -164,6 +164,13 @@ typedef struct rcControlsConfig_s {
     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
 } 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);
 
 bool areSticksInApModePosition(uint16_t ap_mode);
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index ada8f1d734..3908bfd586 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -93,6 +93,7 @@ typedef struct mixer_s {
 } mixer_t;
 
 typedef struct mixerConfig_s {
+    uint8_t mixerMode;
     int8_t yaw_motor_direction;
 } mixerConfig_t;
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index c2a2364934..20839a2775 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -36,10 +36,6 @@
 #include "flight/navigation.h"
 #include "flight/gtune.h"
 
-#include "io/gps.h"
-
-#include "rx/rx.h"
-
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
@@ -47,8 +43,7 @@ extern float rcInput[3];
 extern float setpointRate[3];
 
 uint32_t targetPidLooptime;
-bool pidStabilisationEnabled;
-uint8_t PIDweight[3];
+static bool pidStabilisationEnabled;
 
 float axisPIDf[3];
 
@@ -59,12 +54,9 @@ uint8_t PIDweight[3];
 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 #endif
 
-int32_t errorGyroI[3];
-float errorGyroIf[3];
+static float errorGyroIf[3];
 
-pidControllerFuncPtr pid_controller; // which pid controller are we using
-
-void setTargetPidLooptime(uint32_t pidLooptime)
+void pidSetTargetLooptime(uint32_t pidLooptime)
 {
     targetPidLooptime = pidLooptime;
 }
@@ -72,7 +64,6 @@ void setTargetPidLooptime(uint32_t pidLooptime)
 void pidResetErrorGyroState(void)
 {
     for (int axis = 0; axis < 3; axis++) {
-        errorGyroI[axis] = 0;
         errorGyroIf[axis] = 0.0f;
     }
 }
@@ -82,67 +73,97 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
     pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
 }
 
-float getdT(void)
-{
-    static float dT;
-    if (!dT) dT = (float)targetPidLooptime * 0.000001f;
-
-    return dT;
-}
-
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-pt1Filter_t deltaFilter[3];
-pt1Filter_t yawFilter;
-biquadFilter_t dtermFilterLpf[3];
-biquadFilter_t dtermFilterNotch[3];
-bool dtermNotchInitialised;
-bool dtermBiquadLpfInitialised;
-firFilterDenoise_t dtermDenoisingState[3];
+static filterApplyFnPtr dtermNotchFilterApplyFn;
+static void *dtermFilterNotch[2];
+static filterApplyFnPtr dtermLpfApplyFn;
+static void *dtermFilterLpf[2];
+static filterApplyFnPtr ptermYawFilterApplyFn;
+static void *ptermYawFilter;
 
-static void pidInitFilters(const pidProfile_t *pidProfile) {
-    int axis;
-    static uint8_t lowpassFilterType;
+void pidInitFilters(const pidProfile_t *pidProfile)
+{
+    static biquadFilter_t biquadFilterNotch[2];
+    static pt1Filter_t pt1Filter[2];
+    static biquadFilter_t biquadFilter[2];
+    static firFilterDenoise_t denoisingFilter[2];
+    static pt1Filter_t pt1FilterYaw;
 
-    if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
-        float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
-        for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
-        dtermNotchInitialised = true;
+    BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
+    const float dT = (float)targetPidLooptime * 0.000001f;
+
+    if (pidProfile->dterm_notch_hz == 0) {
+        dtermNotchFilterApplyFn = nullFilterApply;
+    } else {
+        dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
+        for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+            dtermFilterNotch[axis] = &biquadFilterNotch[axis];
+            biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
+        }
     }
 
-    if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
-        if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
-            for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+    if (pidProfile->dterm_lpf_hz == 0) {
+        dtermLpfApplyFn = nullFilterApply;
+    } else {
+        switch (pidProfile->dterm_filter_type) {
+        default:
+            dtermLpfApplyFn = nullFilterApply;
+            break;
+        case FILTER_PT1:
+            dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+                dtermFilterLpf[axis] = &pt1Filter[axis];
+                pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT);
+            }
+            break;
+        case FILTER_BIQUAD:
+            dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+                dtermFilterLpf[axis] = &biquadFilter[axis];
+                biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+            }
+            break;
+        case FILTER_FIR:
+            dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
+            for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+                dtermFilterLpf[axis] = &denoisingFilter[axis];
+                firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+            }
+            break;
         }
+    }
 
-        if (pidProfile->dterm_filter_type == FILTER_FIR) {
-            for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
-        }
-        lowpassFilterType = pidProfile->dterm_filter_type;
+    if (pidProfile->yaw_lpf_hz == 0) {
+        ptermYawFilterApplyFn = nullFilterApply;
+    } else {
+        ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
+        ptermYawFilter = &pt1FilterYaw;
+        pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT);
     }
 }
 
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
 void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
-         const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
+         const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
 {
-    float errorRate = 0, rD = 0, PVRate = 0, dynC;
-    float ITerm,PTerm,DTerm;
     static float lastRateError[2];
-    static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3];
-    float delta;
-    int axis;
+    static float Kp[3], Ki[3], Kd[3], c[3];
+    static float rollPitchMaxVelocity, yawMaxVelocity;
+    static float previousSetpoint[3], relaxFactor[3];
+    static float dT;
+
+    if (!dT) {
+        dT = (float)targetPidLooptime * 0.000001f;
+    }
+
     float horizonLevelStrength = 1;
-
-    float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
-
-    pidInitFilters(pidProfile);
-
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
-        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
-        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
+        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc));
+        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc));
         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
         // Progressively turn off the horizon self level strength as the stick is banged over
         horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection
@@ -172,7 +193,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
     }
 
     // ----------PID controller----------
-    for (axis = 0; axis < 3; axis++) {
+    const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
         static uint8_t configP[3], configI[3], configD[3];
 
@@ -184,8 +206,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
             c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
             relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
-            yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
-            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
+            yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
+            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
 
             configP[axis] = pidProfile->P8[axis];
             configI[axis] = pidProfile->I8[axis];
@@ -193,9 +215,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
         }
 
         // Limit abrupt yaw inputs / stops
-        float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
+        const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
         if (maxVelocity) {
-            float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
+            const float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
             if (ABS(currentVelocity) > maxVelocity) {
                 setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
             }
@@ -222,35 +244,34 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             }
         }
 
-        PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
+        const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
         //  ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error).  ----------
-        // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
-        // ----- calculate error / angle rates  ----------
-        errorRate = setpointRate[axis] - PVRate;       // r - y
+
+        // -----calculate error rate
+        const float errorRate = setpointRate[axis] - PVRate;       // r - y
 
         // -----calculate P component and add Dynamic Part based on stick input
-        PTerm = Kp[axis] * errorRate * tpaFactor;
+        float PTerm = Kp[axis] * errorRate * tpaFactor;
 
-        // -----calculate I component.
+        // -----calculate I component
         // Reduce strong Iterm accumulation during higher stick inputs
-        float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
-        float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
+        const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
+        const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
+        const float itermScaler = setpointRateScaler * kiThrottleGain;
 
-        // Handle All windup Scenarios
+        float ITerm = errorGyroIf[axis];
+        ITerm += Ki[axis] * errorRate * dT * itermScaler;;
         // limit maximum integrator value to prevent WindUp
-        float itermScaler = setpointRateScaler * kiThrottleGain;
+        ITerm = constrainf(ITerm, -250.0f, 250.0f);
+        errorGyroIf[axis] = ITerm;
 
-        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
-
-        // I coefficient (I8) moved before integration to make limiting independent from PID settings
-        ITerm = errorGyroIf[axis];
-
-        //-----calculate D-term (Yaw D not yet supported)
-        if (axis != YAW) {
+        // -----calculate D component (Yaw D not yet supported)
+        float DTerm = 0.0;
+        if (axis != FD_YAW) {
             static float previousSetpoint[3];
-            dynC = c[axis];
+            float dynC = c[axis];
             if (pidProfile->setpointRelaxRatio < 100) {
                 dynC = c[axis];
                 if (setpointRate[axis] > 0) {
@@ -262,39 +283,24 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 }
             }
             previousSetpoint[axis] = setpointRate[axis];
-            rD = dynC * setpointRate[axis] - PVRate;    // cr - y
-            delta = rD - lastRateError[axis];
+            const float rD = dynC * setpointRate[axis] - PVRate;    // cr - y
+            // Divide rate change by dT to get differential (ie dr/dt)
+            const float delta = (rD - lastRateError[axis]) / dT;
             lastRateError[axis] = rD;
 
-            // Divide delta by targetLooptime to get differential (ie dr/dt)
-            delta *= (1.0f / getdT());
-
-            if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
-
-            // Filter delta
-            if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
-
-            if (pidProfile->dterm_lpf_hz) {
-                if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
-                    delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
-                else if (pidProfile->dterm_filter_type == FILTER_PT1)
-                    delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
-                else
-                    delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
-            }
-
             DTerm = Kd[axis] * delta * tpaFactor;
+            DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
+
+            // apply filters
+            DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
+            DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
 
-            // -----calculate total PID output
-            axisPIDf[axis] = PTerm + ITerm + DTerm;
         } else {
-            if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
-
-            axisPIDf[axis] = PTerm + ITerm;
-
-            DTerm = 0.0f; // needed for blackbox
+            PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
         }
 
+        // -----calculate total PID output
+        axisPIDf[axis] = PTerm + ITerm + DTerm;
         // Disable PID control at zero throttle
         if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
 
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 0719f78b30..e059ed84c7 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -20,7 +20,7 @@
 #include <stdbool.h>
 
 #define PID_CONTROLLER_BETAFLIGHT 1
-#define PID_MIXER_SCALING 1000.0f
+#define PID_MIXER_SCALING 900.0f
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
 #define PIDSUM_LIMIT 0.5f
@@ -90,14 +90,9 @@ typedef struct pidProfile_s {
 #endif
 } pidProfile_t;
 
-struct controlRateConfig_s;
 union rollAndPitchTrims_u;
-struct rxConfig_s;
-typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
-        const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);            // pid controller function prototype
-
 void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
-        const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);
+        const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
 
 extern float axisPIDf[3];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
@@ -109,5 +104,6 @@ extern uint8_t PIDweight[3];
 
 void pidResetErrorGyroState(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
-void setTargetPidLooptime(uint32_t pidLooptime);
+void pidSetTargetLooptime(uint32_t pidLooptime);
+void pidInitFilters(const pidProfile_t *pidProfile);
 
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index ebc242cc21..8dcc4b14a2 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -110,8 +110,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate)
 int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier)
 {
     for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
-        const serialPortUsage_t *candidate = &serialPortUsageList[index];
-        if (candidate->identifier == identifier) {
+        if (serialPortIdentifiers[index] == identifier) {
             return index;
         }
     }
@@ -209,7 +208,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
 }
 
 #ifdef TELEMETRY
-#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_MAVLINK)
+#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT)
 #else
 #define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
 #endif
@@ -222,7 +221,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
      * rules:
      * - 1 MSP port minimum, max MSP ports is defined and must be adhered to.
      * - MSP is allowed to be shared with EITHER any telemetry OR blackbox.
-     * - serial RX and FrSky / LTM telemetry can be shared
+     * - serial RX and FrSky / LTM / MAVLink telemetry can be shared
      * - No other sharing combinations are valid.
      */
     uint8_t mspPortCount = 0;
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 22150ed86e..b255eb7708 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -127,7 +127,9 @@ static void cliExit(char *cmdline);
 static void cliFeature(char *cmdline);
 static void cliMotor(char *cmdline);
 static void cliName(char *cmdline);
+#if (FLASH_SIZE > 128)
 static void cliPlaySound(char *cmdline);
+#endif
 static void cliProfile(char *cmdline);
 static void cliRateProfile(char *cmdline);
 static void cliReboot(void);
@@ -331,8 +333,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
     CLI_COMMAND_DEF("motor",  "get/set motor",
        "<index> [<value>]", cliMotor),
+#if (FLASH_SIZE > 128)
     CLI_COMMAND_DEF("play_sound", NULL,
         "[<index>]\r\n", cliPlaySound),
+#endif
     CLI_COMMAND_DEF("profile", "change profile",
         "[<index>]", cliProfile),
     CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
@@ -711,10 +715,10 @@ const clivalue_t valueTable[] = {
     { "motor_pwm_protocol",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
     { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } },
 
-    { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
-    { "gyro_cal_on_first_arm",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
-    { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.auto_disarm_delay, .config.minmax = { 0,  60 } },
-    { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.small_angle, .config.minmax = { 0,  180 } },
+    { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.armingConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
+    { "gyro_cal_on_first_arm",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.armingConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
+    { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.armingConfig.auto_disarm_delay, .config.minmax = { 0,  60 } },
+    { "small_angle",                VAR_UINT8  | MASTER_VALUE,  &masterConfig.armingConfig.small_angle, .config.minmax = { 0,  180 } },
 
     { "fixedwing_althold_dir",      VAR_INT8   | MASTER_VALUE,  &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1,  1 } },
 
@@ -789,11 +793,14 @@ const clivalue_t valueTable[] = {
     { "vbat_min_cell_voltage",      VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10,  50 } },
     { "vbat_warning_cell_voltage",  VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10,  50 } },
     { "vbat_hysteresis",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0,  250 } },
-    { "current_meter_scale",        VAR_INT16  | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000,  10000 } },
-    { "current_meter_offset",       VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0,  3300 } },
+    { "current_meter_scale",        VAR_INT16  | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -16000,  16000 } },
+    { "current_meter_offset",       VAR_INT16 | MASTER_VALUE,  &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { -16000, 16000 } },
     { "multiwii_current_meter_output", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
     { "current_meter_type",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
     { "battery_notpresent_level",   VAR_UINT8  | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } },
+    { "use_vbat_alerts",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
+    { "use_consumption_alerts",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },
+    { "consumption_warning_percentage", VAR_UINT8  | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } },
     { "align_gyro",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
     { "align_mag",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
@@ -803,14 +810,14 @@ const clivalue_t valueTable[] = {
     { "align_board_yaw",            VAR_INT16  | MASTER_VALUE,  &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180,  360 } },
 
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.max_angle_inclination, .config.minmax = { 100,  900 } },
-    { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
-    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_sync_denom, .config.minmax = { 1,  8 } },
-    { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
-    { "gyro_lowpass",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  255 } },
-    { "gyro_notch1_hz",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0,  1000 } },
-    { "gyro_notch1_cutoff",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1,  1000 } },
-    { "gyro_notch2_hz",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0,  1000 } },
-    { "gyro_notch2_cutoff",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
+    { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
+    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1,  8 } },
+    { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
+    { "gyro_lowpass",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  255 } },
+    { "gyro_notch1_hz",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0,  1000 } },
+    { "gyro_notch1_cutoff",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1,  1000 } },
+    { "gyro_notch2_hz",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0,  1000 } },
+    { "gyro_notch2_cutoff",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0,  128 } },
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, .config.minmax = { 0,  50000 } },
     { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_ki, .config.minmax = { 0,  50000 } },
@@ -860,7 +867,7 @@ const clivalue_t valueTable[] = {
     { "rx_min_usec",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },
     { "rx_max_usec",                VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },
 
-    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
+    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorSelectionConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
     { "acc_lpf_hz",                 VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0,  400 } },
     { "accxy_deadband",             VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0,  100 } },
     { "accz_deadband",              VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0,  100 } },
@@ -873,12 +880,12 @@ const clivalue_t valueTable[] = {
     { "baro_noise_lpf",             VAR_FLOAT  | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } },
     { "baro_cf_vel",                VAR_FLOAT  | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } },
     { "baro_cf_alt",                VAR_FLOAT  | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } },
-    { "baro_hardware",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
+    { "baro_hardware",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorSelectionConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
 #endif
 
 #ifdef MAG
-    { "mag_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
-    { "mag_declination",            VAR_INT16  | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000,  18000 } },
+    { "mag_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.sensorSelectionConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
+    { "mag_declination",            VAR_INT16  | MASTER_VALUE, &masterConfig.compassConfig.mag_declination, .config.minmax = { -18000,  18000 } },
 #endif
     { "dterm_lowpass_type",         VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
     { "dterm_lowpass",              VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
@@ -922,10 +929,10 @@ const clivalue_t valueTable[] = {
     { "level_sensitivity",          VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1,  3.0 } },
 
 #ifdef BLACKBOX
-    { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, .config.minmax = { 1,  32 } },
-    { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, .config.minmax = { 1,  32 } },
-    { "blackbox_device",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
-    { "blackbox_on_motor_test",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } },
+    { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackboxConfig.rate_num, .config.minmax = { 1,  32 } },
+    { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackboxConfig.rate_denom, .config.minmax = { 1,  32 } },
+    { "blackbox_device",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.blackboxConfig.device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
+    { "blackbox_on_motor_test",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.blackboxConfig.on_motor_test, .config.lookup = { TABLE_OFF_ON } },
 #endif
 
 #ifdef VTX
@@ -936,9 +943,9 @@ const clivalue_t valueTable[] = {
 #endif
 
 #ifdef MAG
-    { "magzero_x",                  VAR_INT16  | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768,  32767 } },
-    { "magzero_y",                  VAR_INT16  | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768,  32767 } },
-    { "magzero_z",                  VAR_INT16  | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768,  32767 } },
+    { "magzero_x",                  VAR_INT16  | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[X], .config.minmax = { -32768,  32767 } },
+    { "magzero_y",                  VAR_INT16  | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Y], .config.minmax = { -32768,  32767 } },
+    { "magzero_z",                  VAR_INT16  | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Z], .config.minmax = { -32768,  32767 } },
 #endif
 #ifdef LED_STRIP
     { "ledstrip_visual_beeper",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
@@ -1441,10 +1448,16 @@ static void cliSerialPassthrough(char *cmdline)
                    passThroughPort->mode, mode);
             serialSetMode(passThroughPort, mode);
         }
+        // If this port has a rx callback associated we need to remove it now.
+        // Otherwise no data will be pushed in the serial port buffer!
+        if (passThroughPort->rxCallback) {
+            printf("Removing rxCallback from port\r\n");
+            passThroughPort->rxCallback = 0;
+        }
     }
 
     printf("Relaying data to device on port %d, Reset your board to exit "
-           "serial passthrough mode.\r\n");
+           "serial passthrough mode.\r\n", id);
 
     serialPassthrough(cliPort, passThroughPort, NULL, NULL);
 }
@@ -2774,10 +2787,10 @@ static void printConfig(char *cmdline, bool doDiff)
 #ifndef CLI_MINIMAL_VERBOSITY
         cliPrint("\r\n# mixer\r\n");
 #endif
-        const bool equalsDefault = masterConfig.mixerMode == defaultConfig.mixerMode;
+        const bool equalsDefault = masterConfig.mixerConfig.mixerMode == defaultConfig.mixerConfig.mixerMode;
         const char *formatMixer = "mixer %s\r\n";
-        cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerMode - 1]);
-        cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerMode - 1]);
+        cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerConfig.mixerMode - 1]);
+        cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerConfig.mixerMode - 1]);
 
         cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "\r\nmmix reset\r\n\r\n");
 
@@ -3072,7 +3085,7 @@ static void cliMixer(char *cmdline)
     len = strlen(cmdline);
 
     if (len == 0) {
-        cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
+        cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerConfig.mixerMode - 1]);
         return;
     } else if (strncasecmp(cmdline, "list", len) == 0) {
         cliPrint("Available mixers: ");
@@ -3091,7 +3104,7 @@ static void cliMixer(char *cmdline)
             return;
         }
         if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
-            masterConfig.mixerMode = i + 1;
+            masterConfig.mixerConfig.mixerMode = i + 1;
             break;
         }
     }
@@ -3144,11 +3157,9 @@ static void cliMotor(char *cmdline)
     cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
 }
 
+#if (FLASH_SIZE > 128)
 static void cliPlaySound(char *cmdline)
 {
-#if (FLASH_SIZE <= 64) && !defined(CLI_MINIMAL_VERBOSITY)
-    UNUSED(cmdline);
-#else
     int i;
     const char *name;
     static int lastSoundIdx = -1;
@@ -3178,8 +3189,8 @@ static void cliPlaySound(char *cmdline)
     beeperSilence();
     cliPrintf("Playing sound %d: %s\r\n", i, name);
     beeper(beeperModeForTableIndex(i));
-#endif
 }
+#endif
 
 static void cliProfile(char *cmdline)
 {
@@ -3800,47 +3811,43 @@ static void printResource(uint8_t dumpMask, master_t *defaultConfig)
 
         if (resourceTable[i].maxIndex > 0) {
             for (int index = 0; index < resourceTable[i].maxIndex; index++) {
-                ioTag_t ioPtr = *(resourceTable[i].ptr + index);
-                ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
+                ioTag_t ioTag = *(resourceTable[i].ptr + index);
+                ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
 
-                IO_t io = IOGetByTag(ioPtr);
-                IO_t ioDefault = IOGetByTag(ioPtrDefault);
-                bool equalsDefault = io == ioDefault;
+                bool equalsDefault = ioTag == ioTagDefault;
                 const char *format = "resource %s %d %c%02d\r\n";
                 const char *formatUnassigned = "resource %s %d NONE\r\n";
-                if (DEFIO_TAG_ISEMPTY(ioDefault)) {
+                if (!ioTagDefault) {
                     cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index));
                 } else {
-                    cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault));
+                    cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault));
                 }
-                if (DEFIO_TAG_ISEMPTY(io)) {
+                if (!ioTag) {
                     if (!(dumpMask & HIDE_UNUSED)) {
                         cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index));
                     }
                 } else {
-                    cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io));
+                    cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
                 }
             }
         } else {
-            ioTag_t ioPtr = *resourceTable[i].ptr;
-            ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
+            ioTag_t ioTag = *resourceTable[i].ptr;
+            ioTag_t ioTagDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
 
-            IO_t io = IOGetByTag(ioPtr);
-            IO_t ioDefault = IOGetByTag(ioPtrDefault);
-            bool equalsDefault = io == ioDefault;
+            bool equalsDefault = ioTag == ioTagDefault;
             const char *format = "resource %s %c%02d\r\n";
             const char *formatUnassigned = "resource %s NONE\r\n";
-            if (DEFIO_TAG_ISEMPTY(ioDefault)) {
+            if (!ioTagDefault) {
                 cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
             } else {
-                cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault));
+                cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault));
             }
-            if (DEFIO_TAG_ISEMPTY(io)) {
+            if (!ioTag) {
                 if (!(dumpMask & HIDE_UNUSED)) {
                     cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
                 }
             } else {
-                cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io));
+                cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
             }
         }
     }
diff --git a/src/main/main.c b/src/main/main.c
index dfbf4f4785..2d00900098 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -251,7 +251,7 @@ void init(void)
     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 #endif
 
-    mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
+    mixerInit(masterConfig.mixerConfig.mixerMode, masterConfig.customMotorMixer);
 #ifdef USE_SERVOS
     servoMixerInit(masterConfig.customServoMixer);
 #endif
@@ -419,13 +419,16 @@ void init(void)
     }
 #endif
 
+#ifdef SONAR
+    const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig;
+#else
+    const void *sonarConfig = NULL;
+#endif
     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
-            masterConfig.acc_hardware,
-            masterConfig.mag_hardware,
-            masterConfig.baro_hardware,
-            masterConfig.mag_declination,
-            masterConfig.gyro_lpf,
-            masterConfig.gyro_sync_denom)) {
+            &masterConfig.sensorSelectionConfig,
+            masterConfig.compassConfig.mag_declination,
+            &masterConfig.gyroConfig,
+            sonarConfig)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }
@@ -447,10 +450,9 @@ void init(void)
     LED0_OFF;
     LED1_OFF;
 
-#ifdef MAG
-    if (sensors(SENSOR_MAG))
-        compassInit();
-#endif
+    // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
+    pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
+    pidInitFilters(&currentProfile->pidProfile);
 
     imuInit();
 
@@ -482,12 +484,6 @@ void init(void)
     }
 #endif
 
-#ifdef SONAR
-    if (feature(FEATURE_SONAR)) {
-        sonarInit(&masterConfig.sonarConfig);
-    }
-#endif
-
 #ifdef LED_STRIP
     ledStripInit(&masterConfig.ledStripConfig);
 
@@ -541,18 +537,11 @@ void init(void)
     }
 #endif
 
-    if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
-        masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
-        masterConfig.gyro_sync_denom = 1;
-    }
-
-    setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
-
 #ifdef BLACKBOX
     initBlackbox();
 #endif
 
-    if (masterConfig.mixerMode == MIXER_GIMBAL) {
+    if (masterConfig.mixerConfig.mixerMode == MIXER_GIMBAL) {
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
     }
     gyroSetCalibrationCycles();
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 9b38932bfd..74f34fd95e 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -125,20 +125,21 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
     serialBeginWrite(msp->port);
     const int len = sbufBytesRemaining(&packet->buf);
     const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
-    const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
-    serialWriteBuf(msp->port, hdr, sizeof(hdr));
-    uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field
+    uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+    int hdrLen = 5;
+#define CHECKSUM_STARTPOS 3  // checksum starts from mspLen field
     if (len >= JUMBO_FRAME_SIZE_LIMIT) {
-        serialWrite(msp->port, len & 0xff);
-        checksum ^= len & 0xff;
-        serialWrite(msp->port, (len >> 8) & 0xff);
-        checksum ^= (len >> 8) & 0xff;
+        hdrLen += 2;
+        hdr[5] = len & 0xff;
+        hdr[6] = (len >> 8) & 0xff;
     }
+    serialWriteBuf(msp->port, hdr, hdrLen);
+    uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS);
     if (len > 0) {
         serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
         checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
     }
-    serialWrite(msp->port, checksum);
+    serialWriteBuf(msp->port, &checksum, 1);
     serialEndWrite(msp->port);
     return sizeof(hdr) + len + 1; // header, data, and checksum
 }
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index dee83f43ff..f1330bf415 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -44,7 +44,12 @@
 
 #include "common/utils.h"
 
-#define VBATT_LPF_FREQ  0.4f
+#define VBAT_LPF_FREQ  0.4f
+#define IBAT_LPF_FREQ  0.4f
+
+#define VBAT_STABLE_MAX_DELTA 2
+
+#define ADCVREF 3300   // in mV
 
 // Battery monitoring stuff
 uint8_t batteryCellCount;
@@ -53,12 +58,14 @@ uint16_t batteryCriticalVoltage;
 
 uint16_t vbat = 0;                  // battery voltage in 0.1V steps (filtered)
 uint16_t vbatLatest = 0;            // most recent unsmoothed value
-uint16_t amperageLatest = 0;        // most recent value
 
 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
+uint16_t amperageLatest = 0;        // most recent value
+
 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
 
-static batteryState_e batteryState;
+static batteryState_e vBatState;
+static batteryState_e consumptionState;
 
 static uint16_t batteryAdcToVoltage(uint16_t src)
 {
@@ -69,47 +76,67 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
 
 static void updateBatteryVoltage(void)
 {
-    static biquadFilter_t vbatFilter;
-    static bool vbatFilterIsInitialised;
+    static biquadFilter_t vBatFilter;
+    static bool vBatFilterIsInitialised;
 
-    // store the battery voltage with some other recent battery voltage readings
-    uint16_t vbatSample;
+    if (!vBatFilterIsInitialised) {
+        biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
+        vBatFilterIsInitialised = true;
+    }
 
     #ifdef USE_ESC_TELEMETRY
     if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
-        vbatSample = vbatLatest = getEscTelemetryVbat();
+        vbatLatest = getEscTelemetryVbat();
+        if (debugMode == DEBUG_BATTERY) {
+            debug[0] = -1;
+        }
+        vbat = biquadFilterApply(&vBatFilter, vbatLatest);
     }
     else
     #endif
     {
-        vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY));
+        uint16_t vBatSample = adcGetChannel(ADC_BATTERY);
+        if (debugMode == DEBUG_BATTERY) {
+            debug[0] = vBatSample;
+        }
+        vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample));
+        vbatLatest = batteryAdcToVoltage(vBatSample);
     }
 
-    if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
-
-    if (!vbatFilterIsInitialised) {
-        biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
-        vbatFilterIsInitialised = true;
+    if (debugMode == DEBUG_BATTERY) {
+        debug[1] = vbat;
     }
-    vbat = biquadFilterApply(&vbatFilter, vbatSample);
-
-    if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
 }
 
-#define VBAT_STABLE_MAX_DELTA 2
+static void updateBatteryAlert(void)
+{
+    switch(getBatteryState()) {
+        case BATTERY_WARNING:
+            beeper(BEEPER_BAT_LOW);
+
+            break;
+        case BATTERY_CRITICAL:
+            beeper(BEEPER_BAT_CRIT_LOW);
+
+            break;
+        case BATTERY_OK:
+        case BATTERY_NOT_PRESENT:
+            break;
+    }
+}
 
 void updateBattery(void)
 {
-    uint16_t vbatPrevious = vbatLatest;
+    uint16_t vBatPrevious = vbatLatest;
     updateBatteryVoltage();
-    uint16_t vbatMeasured = vbatLatest;
+    uint16_t vBatMeasured = vbatLatest;
 
     /* battery has just been connected*/
-    if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
+    if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
         /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
-        batteryState = BATTERY_OK;
+        vBatState = BATTERY_OK;
 
-        unsigned cells = (vbatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
+        unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
         if (cells > 8) {
             // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
             cells = 8;
@@ -118,49 +145,55 @@ void updateBattery(void)
         batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
         batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
     /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
-    } else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) {
-        batteryState = BATTERY_NOT_PRESENT;
+    } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
+        vBatState = BATTERY_NOT_PRESENT;
         batteryCellCount = 0;
         batteryWarningVoltage = 0;
         batteryCriticalVoltage = 0;
     }
 
-    if (debugMode == DEBUG_BATTERY) debug[2] = batteryState;
-    if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount;
+    if (debugMode == DEBUG_BATTERY) {
+        debug[2] = vBatState;
+        debug[3] = batteryCellCount;
+    }
 
-    switch(batteryState)
-    {
-        case BATTERY_OK:
-            if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
-                batteryState = BATTERY_WARNING;
-                beeper(BEEPER_BAT_LOW);
-            }
-            break;
-        case BATTERY_WARNING:
-            if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
-                batteryState = BATTERY_CRITICAL;
-                beeper(BEEPER_BAT_CRIT_LOW);
-            } else if (vbat > batteryWarningVoltage) {
-                batteryState = BATTERY_OK;
-            } else {
-                beeper(BEEPER_BAT_LOW);
-            }
-            break;
-        case BATTERY_CRITICAL:
-            if (vbat > batteryCriticalVoltage) {
-                batteryState = BATTERY_WARNING;
-                beeper(BEEPER_BAT_LOW);
-            } else {
-                beeper(BEEPER_BAT_CRIT_LOW);
-            }
-            break;
-        case BATTERY_NOT_PRESENT:
-            break;
+    if (batteryConfig->useVBatAlerts) {
+        switch(vBatState) {
+            case BATTERY_OK:
+                if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
+                    vBatState = BATTERY_WARNING;
+                }
+
+                break;
+            case BATTERY_WARNING:
+                if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
+                    vBatState = BATTERY_CRITICAL;
+                } else if (vbat > batteryWarningVoltage) {
+                    vBatState = BATTERY_OK;
+                }
+
+                break;
+            case BATTERY_CRITICAL:
+                if (vbat > batteryCriticalVoltage) {
+                    vBatState = BATTERY_WARNING;
+                }
+
+                break;
+            case BATTERY_NOT_PRESENT:
+                break;
+        }
+
+        updateBatteryAlert();
     }
 }
 
 batteryState_e getBatteryState(void)
 {
+    batteryState_e batteryState = BATTERY_NOT_PRESENT;
+    if (vBatState != BATTERY_NOT_PRESENT) {
+        batteryState = MAX(vBatState, consumptionState);
+    }
+
     return batteryState;
 }
 
@@ -168,19 +201,19 @@ const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PR
 
 const char * getBatteryStateString(void)
 {
-    return batteryStateStrings[batteryState];
+    return batteryStateStrings[getBatteryState()];
 }
 
 void batteryInit(batteryConfig_t *initialBatteryConfig)
 {
     batteryConfig = initialBatteryConfig;
-    batteryState = BATTERY_NOT_PRESENT;
+    vBatState = BATTERY_NOT_PRESENT;
+    consumptionState = BATTERY_OK;
     batteryCellCount = 0;
     batteryWarningVoltage = 0;
     batteryCriticalVoltage = 0;
 }
 
-#define ADCVREF 3300   // in mV
 static int32_t currentSensorToCentiamps(uint16_t src)
 {
     int32_t millivolts;
@@ -191,50 +224,88 @@ static int32_t currentSensorToCentiamps(uint16_t src)
     return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
 }
 
+static void updateBatteryCurrent(void)
+{
+    static biquadFilter_t iBatFilter;
+    static bool iBatFilterIsInitialised;
+
+    if (!iBatFilterIsInitialised) {
+        biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
+        iBatFilterIsInitialised = true;
+    }
+
+    uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
+    amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample));
+    amperageLatest = currentSensorToCentiamps(iBatSample);
+}
+
+static void updateCurrentDrawn(int32_t lastUpdateAt)
+{
+    static float mAhDrawnF = 0.0f; // used to get good enough resolution
+
+    mAhDrawnF = mAhDrawnF + (amperage * lastUpdateAt / (100.0f * 1000 * 3600));
+    mAhDrawn = mAhDrawnF;
+}
+
 void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 {
-    #ifdef USE_ESC_TELEMETRY
-    UNUSED(lastUpdateAt);
-    #endif
-
-    static int64_t mAhdrawnRaw = 0;
-    static int32_t amperageRaw = 0;
-
-    int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
-    int32_t throttleFactor = 0;
-
     switch(batteryConfig->currentMeterType) {
         case CURRENT_SENSOR_ADC:
-            amperageRaw -= amperageRaw / 8;
-            amperageRaw += adcGetChannel(ADC_CURRENT);
-            amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8);
+            updateBatteryCurrent();
+
+            updateCurrentDrawn(lastUpdateAt);
+
             break;
         case CURRENT_SENSOR_VIRTUAL:
             amperage = (int32_t)batteryConfig->currentMeterOffset;
             if (ARMING_FLAG(ARMED)) {
                 throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
-                if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
+                int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
+                if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
                     throttleOffset = 0;
-                throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
+                }
+                int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
                 amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale  / 1000;
             }
-            break;
-        case CURRENT_SENSOR_NONE:
-            amperage = 0;
+
+            updateCurrentDrawn(lastUpdateAt);
+
             break;
         case CURRENT_SENSOR_ESC:
             #ifdef USE_ESC_TELEMETRY
-            if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) {
+            if (isEscTelemetryActive()) {
                 amperage = getEscTelemetryCurrent();
                 mAhDrawn = getEscTelemetryConsumption();
             }
+
+            break;
             #endif
+        case CURRENT_SENSOR_NONE:
+            amperage = 0;
+
             break;
     }
 
-    if (!feature(FEATURE_ESC_TELEMETRY)) {
-        mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
-        mAhDrawn = mAhdrawnRaw / (3600 * 100);
+    if (batteryConfig->useConsumptionAlerts) {
+        switch(consumptionState) {
+            case BATTERY_OK:
+                if (calculateBatteryCapacityRemainingPercentage() <= batteryConfig->consumptionWarningPercentage) {
+                    consumptionState = BATTERY_WARNING;
+                }
+
+                break;
+            case BATTERY_WARNING:
+                if (calculateBatteryCapacityRemainingPercentage() == 0) {
+                    vBatState = BATTERY_CRITICAL;
+                }
+
+                break;
+            case BATTERY_CRITICAL:
+            case BATTERY_NOT_PRESENT:
+                break;
+        }
+
+        updateBatteryAlert();
     }
 }
 
diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h
index cac334f850..872e017000 100644
--- a/src/main/sensors/battery.h
+++ b/src/main/sensors/battery.h
@@ -53,13 +53,16 @@ typedef struct batteryConfig_s {
     batterySensor_e batteryMeterType;       // type of battery meter uses, either ADC or ESC
 
     int16_t currentMeterScale;             // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
-    uint16_t currentMeterOffset;            // offset of the current sensor in millivolt steps
+    int16_t currentMeterOffset;            // offset of the current sensor in millivolt steps
     currentSensor_e  currentMeterType;      // type of current meter used, either ADC, Virtual or ESC
 
     // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
     uint8_t multiwiiCurrentMeterOutput;     // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
     uint16_t batteryCapacity;               // mAh
     uint8_t batterynotpresentlevel;         // Below this level battery is considered as not present
+    bool useVBatAlerts;                     // Issue alerts based on VBat readings
+    bool useConsumptionAlerts;              // Issue alerts based on total power consumption
+    uint8_t consumptionWarningPercentage;   // Percentage of remaining capacity that should trigger a battery warning
 } batteryConfig_t;
 
 typedef enum {
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 6e8e23779a..13abb7509c 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -30,6 +30,11 @@ typedef enum {
     MAG_MAX = MAG_AK8963
 } magSensor_e;
 
+typedef struct compassConfig_s {
+    int16_t mag_declination;                // Get your magnetic decliniation from here : http://magnetic-declination.com/
+                                            // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
+} compassConfig_t;
+
 void compassInit(void);
 union flightDynamicsTrims_u;
 void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0af158fe99..56c29bd83d 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -44,51 +44,66 @@ float gyroADCf[XYZ_AXIS_COUNT];
 
 static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 static const gyroConfig_t *gyroConfig;
-static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
-static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
-static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
-static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
-static uint8_t gyroSoftLpfType;
-static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
-static float gyroSoftNotchQ1, gyroSoftNotchQ2;
-static uint8_t gyroSoftLpfHz;
 static uint16_t calibratingG = 0;
-static float gyroDt;
 
-void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
-                   uint8_t gyro_soft_lpf_hz,
-                   uint16_t gyro_soft_notch_hz_1,
-                   uint16_t gyro_soft_notch_cutoff_1,
-                   uint16_t gyro_soft_notch_hz_2,
-                   uint16_t gyro_soft_notch_cutoff_2,
-                   uint8_t gyro_soft_lpf_type)
+static filterApplyFnPtr softLpfFilterApplyFn;
+static void *softLpfFilter[3];
+static filterApplyFnPtr notchFilter1ApplyFn;
+static void *notchFilter1[3];
+static filterApplyFnPtr notchFilter2ApplyFn;
+static void *notchFilter2[3];
+
+void gyroInit(const gyroConfig_t *gyroConfigToUse)
 {
+    static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
+    static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
+    static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
+    static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
+    static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
+
     gyroConfig = gyroConfigToUse;
-    gyroSoftLpfHz = gyro_soft_lpf_hz;
-    gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
-    gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
-    gyroSoftLpfType = gyro_soft_lpf_type;
-    gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
-    gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
-}
 
-void gyroInit(void)
-{
-    if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
-        for (int axis = 0; axis < 3; axis++) {
-            if (gyroSoftLpfType == FILTER_BIQUAD)
-                biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
-            else if (gyroSoftLpfType == FILTER_PT1)
-                gyroDt = (float) gyro.targetLooptime * 0.000001f;
-            else
-                firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
+    softLpfFilterApplyFn = nullFilterApply;
+    notchFilter1ApplyFn = nullFilterApply;
+    notchFilter2ApplyFn = nullFilterApply;
+
+    if (gyroConfig->gyro_soft_lpf_hz) {  // Initialisation needs to happen once samplingrate is known
+        if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
+            softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            for (int axis = 0; axis < 3; axis++) {
+                softLpfFilter[axis] = &gyroFilterLPF[axis];
+                biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
+            }
+        } else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
+            softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
+            for (int axis = 0; axis < 3; axis++) {
+                softLpfFilter[axis] = &gyroFilterPt1[axis];
+                pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
+            }
+        } else {
+            softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
+            for (int axis = 0; axis < 3; axis++) {
+                softLpfFilter[axis] = &gyroDenoiseState[axis];
+                firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
+            }
         }
     }
 
-    if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
+    if (gyroConfig->gyro_soft_notch_hz_1) {
+        notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
         for (int axis = 0; axis < 3; axis++) {
-            biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
-            biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
+            notchFilter1[axis] = &gyroFilterNotch_1[axis];
+            biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
+        }
+    }
+    if (gyroConfig->gyro_soft_notch_hz_2) {
+        notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
+        for (int axis = 0; axis < 3; axis++) {
+            notchFilter2[axis] = &gyroFilterNotch_2[axis];
+            biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
         }
     }
 }
@@ -180,32 +195,20 @@ void gyroUpdate(void)
     gyroADC[Y] -= gyroZero[Y];
     gyroADC[Z] -= gyroZero[Z];
 
-    if (gyroSoftLpfHz) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 
-            if (debugMode == DEBUG_GYRO)
-                debug[axis] = gyroADC[axis];
+        if (debugMode == DEBUG_GYRO)
+            debug[axis] = gyroADC[axis];
 
-            if (gyroSoftLpfType == FILTER_BIQUAD)
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
-            else if (gyroSoftLpfType == FILTER_PT1)
-                gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
-            else
-                gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
+        gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
 
-            if (debugMode == DEBUG_NOTCH)
-                debug[axis] = lrintf(gyroADCf[axis]);
+        if (debugMode == DEBUG_NOTCH)
+            debug[axis] = lrintf(gyroADCf[axis]);
 
-            if (gyroSoftNotchHz1)
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
+        gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
 
-            if (gyroSoftNotchHz2)
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
+        gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
 
-            gyroADC[axis] = lrintf(gyroADCf[axis]);
-        }
-    } else {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
-            gyroADCf[axis] = gyroADC[axis];
+        gyroADC[axis] = lrintf(gyroADCf[axis]);
     }
 }
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 2946a4602f..bea1268e57 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -41,18 +41,19 @@ extern int32_t gyroADC[XYZ_AXIS_COUNT];
 extern float gyroADCf[XYZ_AXIS_COUNT];
 
 typedef struct gyroConfig_s {
-    uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
+    uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
+    uint8_t  gyro_sync_denom;                  // Gyro sample divider
+    uint8_t  gyro_soft_lpf_type;
+    uint8_t  gyro_soft_lpf_hz;
+    uint16_t gyro_lpf;                         // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint16_t gyro_soft_notch_hz_1;
+    uint16_t gyro_soft_notch_cutoff_1;
+    uint16_t gyro_soft_notch_hz_2;
+    uint16_t gyro_soft_notch_cutoff_2;
 } gyroConfig_t;
 
-void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
-                   uint8_t gyro_soft_lpf_hz,
-                   uint16_t gyro_soft_notch_hz_1,
-                   uint16_t gyro_soft_notch_cutoff_1,
-                   uint16_t gyro_soft_notch_hz_2,
-                   uint16_t gyro_soft_notch_cutoff_2,
-                   uint8_t gyro_soft_lpf_type);
 void gyroSetCalibrationCycles(void);
-void gyroInit(void);
+void gyroInit(const gyroConfig_t *gyroConfigToUse);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);
 
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index fe2645b8df..d1812a9b15 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -24,6 +24,8 @@
 
 #include "common/axis.h"
 
+#include "config/feature.h"
+
 #include "drivers/io.h"
 #include "drivers/system.h"
 #include "drivers/exti.h"
@@ -61,6 +63,7 @@
 
 #include "drivers/sonar_hcsr04.h"
 
+#include "fc/config.h"
 #include "fc/runtime_config.h"
 
 #include "sensors/sensors.h"
@@ -609,7 +612,20 @@ retry:
 }
 #endif
 
-void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
+#ifdef SONAR
+static bool detectSonar(void)
+{
+    if (feature(FEATURE_SONAR)) {
+        // the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
+        // since there is no way to detect it
+        sensorsSet(SENSOR_SONAR);
+        return true;
+    }
+    return false;
+}
+#endif
+
+static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
 {
     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
         gyroAlign = sensorAlignmentConfig->gyro_align;
@@ -622,13 +638,11 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
     }
 }
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
-        uint8_t accHardwareToUse,
-        uint8_t magHardwareToUse,
-        uint8_t baroHardwareToUse,
+bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
+        const sensorSelectionConfig_t *sensorSelectionConfig,
         int16_t magDeclinationFromConfig,
-        uint8_t gyroLpf,
-        uint8_t gyroSyncDenominator)
+        const gyroConfig_t *gyroConfig,
+        const sonarConfig_t *sonarConfig)
 {
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
@@ -647,11 +661,11 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
 
     // Now time to init things
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator);    // Set gyro sample rate before initialisation
-    gyro.init(gyroLpf); // driver initialisation
-    gyroInit(); // sensor initialisation
+    gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom);    // Set gyro sample rate before initialisation
+    gyro.init(gyroConfig->gyro_lpf); // driver initialisation
+    gyroInit(gyroConfig); // sensor initialisation
 
-    if (detectAcc(accHardwareToUse)) {
+    if (detectAcc(sensorSelectionConfig->acc_hardware)) {
         acc.acc_1G = 256; // set default
         acc.init(&acc); // driver initialisation
         accInit(gyro.targetLooptime); // sensor initialisation
@@ -661,21 +675,27 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
 #ifdef MAG
     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
-    if (detectMag(magHardwareToUse)) {
+    if (detectMag(sensorSelectionConfig->mag_hardware)) {
         // calculate magnetic declination
         const int16_t deg = magDeclinationFromConfig / 100;
         const int16_t min = magDeclinationFromConfig % 100;
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
+        compassInit();
     }
 #else
-    UNUSED(magHardwareToUse);
     UNUSED(magDeclinationFromConfig);
 #endif
 
 #ifdef BARO
-    detectBaro(baroHardwareToUse);
+    detectBaro(sensorSelectionConfig->baro_hardware);
+#endif
+
+#ifdef SONAR
+    if (detectSonar()) {
+        sonarInit(sonarConfig);
+    }
 #else
-    UNUSED(baroHardwareToUse);
+    UNUSED(sonarConfig);
 #endif
 
     reconfigureAlignment(sensorAlignmentConfig);
diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h
index 325979d548..c1b3204f91 100644
--- a/src/main/sensors/initialisation.h
+++ b/src/main/sensors/initialisation.h
@@ -17,4 +17,12 @@
 
 #pragma once
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);
+struct sensorAlignmentConfig_s;
+struct sensorSelectionConfig_s;
+struct gyroConfig_s;
+struct sonarConfig_s;
+bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig,
+        const struct sensorSelectionConfig_s *sensorSelectionConfig,
+        int16_t magDeclinationFromConfig,
+        const struct gyroConfig_s *gyroConfig,
+        const struct sonarConfig_s *sonarConfig);
diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h
index 9e6fdda92f..3507a2965b 100644
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@@ -69,3 +69,14 @@ typedef struct sensorAlignmentConfig_s {
     sensor_align_e acc_align;               // acc alignment
     sensor_align_e mag_align;               // mag alignment
 } sensorAlignmentConfig_t;
+
+typedef struct sensorSelectionConfig_s {
+    uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
+    uint8_t baro_hardware;                  // Barometer hardware to use
+    uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
+} sensorSelectionConfig_t;
+
+typedef struct sensorTrims_s {
+    flightDynamicsTrims_t accZero;
+    flightDynamicsTrims_t magZero;
+} sensorTrims_t;
diff --git a/src/main/target/ALIENFLIGHTF1/AlienFlight.md b/src/main/target/ALIENFLIGHTF1/AlienFlight.md
index 560536128a..3510b52c1c 100644
--- a/src/main/target/ALIENFLIGHTF1/AlienFlight.md
+++ b/src/main/target/ALIENFLIGHTF1/AlienFlight.md
@@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
 - STM32F103CBT6 MCU (ALIENFLIGHTF1)
 - STM32F303CCT6 MCU (ALIENFLIGHTF3)
 - STM32F405RGT6 MCU (ALIENFLIGHTF4)
-- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
+- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
 - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
 - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
 - extra-wide traces on the PCB, for maximum power throughput (brushed variants)
diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c
index 3f827c995b..049a06e882 100644
--- a/src/main/target/ALIENFLIGHTF1/config.c
+++ b/src/main/target/ALIENFLIGHTF1/config.c
@@ -20,6 +20,8 @@
 
 #include <platform.h>
 
+#include "drivers/pwm_output.h"
+
 #include "fc/rc_controls.h"
 
 #include "flight/failsafe.h"
@@ -31,13 +33,21 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#include "hardware_revision.h"
 
 // alternative defaults settings for AlienFlight targets
 void targetConfiguration(master_t *config)
 {
     config->rxConfig.spektrum_sat_bind = 5;
     config->rxConfig.spektrum_sat_bind_autoreset = 1;
-    config->motorConfig.motorPwmRate = 32000;
+
+    if (hardwareMotorType == MOTOR_BRUSHED) {
+        config->motorConfig.minthrottle = 1000;
+        config->motorConfig.motorPwmRate = 32000;
+        config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
+        config->motorConfig.useUnsyncedPwm = true;
+    }
+
     config->profile[0].pidProfile.P8[ROLL] = 90;
     config->profile[0].pidProfile.I8[ROLL] = 44;
     config->profile[0].pidProfile.D8[ROLL] = 60;
@@ -52,5 +62,5 @@ void targetConfiguration(master_t *config)
     config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f };    // MIDFRONT_R
     config->customMotorMixer[5] = (motorMixer_t){ 1.0f,  1.0f, -0.414178f,  1.0f };    // MIDFRONT_L
     config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f,  0.414178f,  1.0f };    // MIDREAR_R
-    config->customMotorMixer[7] = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L#endif
+    config->customMotorMixer[7] = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L
 }
diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.c b/src/main/target/ALIENFLIGHTF1/hardware_revision.c
new file mode 100644
index 0000000000..a492c075f6
--- /dev/null
+++ b/src/main/target/ALIENFLIGHTF1/hardware_revision.c
@@ -0,0 +1,64 @@
+/*
+ * 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 <stdlib.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+
+#include "drivers/system.h"
+#include "drivers/io.h"
+#include "drivers/exti.h"
+#include "hardware_revision.h"
+
+static const char * const hardwareRevisionNames[] = {
+        "Unknown",
+        "AlienFlight F1 V1",
+};
+
+uint8_t hardwareRevision = AFF1_REV_1;
+uint8_t hardwareMotorType = MOTOR_UNKNOWN;
+
+static IO_t MotorDetectPin = IO_NONE;
+
+void detectHardwareRevision(void)
+{
+    MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
+    IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
+    IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
+
+    delayMicroseconds(10);  // allow configuration to settle
+
+    // Check presence of brushed ESC's
+    if (IORead(MotorDetectPin)) {
+        hardwareMotorType = MOTOR_BRUSHLESS;
+    } else {
+        hardwareMotorType = MOTOR_BRUSHED;
+    }
+}
+
+void updateHardwareRevision(void)
+{
+}
+
+const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
+{
+    return NULL;
+}
diff --git a/src/main/target/ALIENFLIGHTF1/hardware_revision.h b/src/main/target/ALIENFLIGHTF1/hardware_revision.h
new file mode 100644
index 0000000000..8fcb761fcc
--- /dev/null
+++ b/src/main/target/ALIENFLIGHTF1/hardware_revision.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
+
+typedef enum awf1HardwareRevision_t {
+    AFF1_UNKNOWN = 0,
+    AFF1_REV_1, // MPU6050 (I2C)
+} awf1HardwareRevision_e;
+
+typedef enum awf4HardwareMotorType_t {
+    MOTOR_UNKNOWN = 0,
+    MOTOR_BRUSHED,
+    MOTOR_BRUSHLESS
+} awf4HardwareMotorType_e;
+
+extern uint8_t hardwareRevision;
+extern uint8_t hardwareMotorType;
+
+void updateHardwareRevision(void);
+void detectHardwareRevision(void);
+
+struct extiConfig_s;
+const struct extiConfig_s *selectMPUIntExtiConfigByHardwareRevision(void);
diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c
index b7a0fa3288..02f8432156 100644
--- a/src/main/target/ALIENFLIGHTF1/target.c
+++ b/src/main/target/ALIENFLIGHTF1/target.c
@@ -20,22 +20,23 @@
 #include <platform.h>
 #include "drivers/io.h"
 
-#include "drivers/timer.h"
 #include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
-    { TIM2, IO_TAG(PA0),  TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1
-    { TIM2, IO_TAG(PA1),  TIM_Channel_2, TIM_USE_PWM,    0, NULL, 0 }, // PWM2 - RC2
-    { TIM2, IO_TAG(PA2),  TIM_Channel_3, TIM_USE_PWM,    0, NULL, 0 }, // PWM3 - RC3
-    { TIM2, IO_TAG(PA3),  TIM_Channel_4, TIM_USE_PWM,    0, NULL, 0 }, // PWM4 - RC4
-    { TIM3, IO_TAG(PA6),  TIM_Channel_1, TIM_USE_PWM,    0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5
-    { TIM3, IO_TAG(PA7),  TIM_Channel_2, TIM_USE_PWM,    0, NULL, 0 }, // PWM6 - RC6
-    { TIM3, IO_TAG(PB0),  TIM_Channel_3, TIM_USE_PWM,    0, NULL, 0 }, // PWM7 - RC7
-    { TIM3, IO_TAG(PB1),  TIM_Channel_4, TIM_USE_PWM,    0, NULL, 0 }, // PWM8 - RC8
-    { TIM1, IO_TAG(PA8),  TIM_Channel_1, TIM_USE_MOTOR,  1, NULL, 0 }, // PWM9 - OUT1
-    { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR,  1, NULL, 0 }, // PWM10 - OUT2
-    { TIM4, IO_TAG(PB6),  TIM_Channel_1, TIM_USE_MOTOR,  1, NULL, 0 }, // PWM11 - OUT3
-    { TIM4, IO_TAG(PB7),  TIM_Channel_2, TIM_USE_MOTOR,  1, NULL, 0 }, // PWM12 - OUT4
-    { TIM4, IO_TAG(PB8),  TIM_Channel_3, TIM_USE_MOTOR,  1, NULL, 0 }, // PWM13 - OUT5
-    { TIM4, IO_TAG(PB9),  TIM_Channel_4, TIM_USE_MOTOR,  1, NULL, 0 }  // PWM14 - OUT6
+    DEF_TIM(TIM2, CH1, PA0,  TIM_USE_PPM | TIM_USE_PWM, TIMER_INPUT_ENABLED  ), // PWM1 - RC1
+    DEF_TIM(TIM2, CH2, PA1,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM2 - RC2
+    DEF_TIM(TIM2, CH3, PA2,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM3 - RC3
+    DEF_TIM(TIM2, CH4, PA3,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM4 - RC4
+    DEF_TIM(TIM3, CH1, PA6,  TIM_USE_PWM | TIM_USE_LED, TIMER_INPUT_ENABLED  ), // PWM5 - RC5
+    DEF_TIM(TIM3, CH2, PA7,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM6 - RC6
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM7 - RC7
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_PWM,               TIMER_INPUT_ENABLED  ), // PWM8 - RC8
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED ), // PWM9 - OUT1
+    DEF_TIM(TIM1, CH4, PA11, TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED ), // PWM10 - OUT2
+    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED ), // PWM11 - OUT3
+    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED ), // PWM12 - OUT4
+    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED ), // PWM13 - OUT5
+    DEF_TIM(TIM4, CH4, PB9,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED )  // PWM14 - OUT6
 };
diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h
index 875d5e5122..5bafc911c8 100644
--- a/src/main/target/ALIENFLIGHTF1/target.h
+++ b/src/main/target/ALIENFLIGHTF1/target.h
@@ -20,6 +20,9 @@
 #define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1.
 #define TARGET_CONFIG
 
+#define USE_HARDWARE_REVISION_DETECTION
+#define MOTOR_PIN               PA8
+
 #define LED0                    PB3
 #define LED1                    PB4
 
@@ -70,7 +73,6 @@
 // Hardware bind plug at PB5 (Pin 41)
 #define BINDPLUG_PIN            PB5
 
-#define BRUSHED_MOTORS
 #define DEFAULT_FEATURES        FEATURE_MOTOR_STOP
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SPEKTRUM2048
diff --git a/src/main/target/ALIENFLIGHTF3/AlienFlight.md b/src/main/target/ALIENFLIGHTF3/AlienFlight.md
index 560536128a..3510b52c1c 100644
--- a/src/main/target/ALIENFLIGHTF3/AlienFlight.md
+++ b/src/main/target/ALIENFLIGHTF3/AlienFlight.md
@@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
 - STM32F103CBT6 MCU (ALIENFLIGHTF1)
 - STM32F303CCT6 MCU (ALIENFLIGHTF3)
 - STM32F405RGT6 MCU (ALIENFLIGHTF4)
-- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
+- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
 - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
 - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
 - extra-wide traces on the PCB, for maximum power throughput (brushed variants)
diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c
index 0df64d8533..a7cfc3a91e 100644
--- a/src/main/target/ALIENFLIGHTF3/config.c
+++ b/src/main/target/ALIENFLIGHTF3/config.c
@@ -24,6 +24,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/compass.h"
+#include "drivers/pwm_output.h"
 
 #include "fc/rc_controls.h"
 
@@ -39,16 +40,23 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#include "hardware_revision.h"
 
 // alternative defaults settings for AlienFlight targets
 void targetConfiguration(master_t *config)
 {
-    config->mag_hardware = MAG_NONE;            // disabled by default
     config->rxConfig.spektrum_sat_bind = 5;
     config->rxConfig.spektrum_sat_bind_autoreset = 1;
-    config->motorConfig.motorPwmRate = 32000;
-    config->gyro_sync_denom = 2;
-    config->pid_process_denom = 1;
+    config->sensorSelectionConfig.mag_hardware = MAG_NONE;            // disabled by default
+
+    if (hardwareMotorType == MOTOR_BRUSHED) {
+        config->motorConfig.minthrottle = 1000;
+        config->motorConfig.motorPwmRate = 32000;
+        config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
+        config->pid_process_denom = 2;
+        config->motorConfig.useUnsyncedPwm = true;
+    }
+
     config->profile[0].pidProfile.P8[ROLL] = 90;
     config->profile[0].pidProfile.I8[ROLL] = 44;
     config->profile[0].pidProfile.D8[ROLL] = 60;
@@ -63,5 +71,5 @@ void targetConfiguration(master_t *config)
     config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f };    // MIDFRONT_R
     config->customMotorMixer[5] = (motorMixer_t){ 1.0f,  1.0f, -0.414178f,  1.0f };    // MIDFRONT_L
     config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f,  0.414178f,  1.0f };    // MIDREAR_R
-    config->customMotorMixer[7] = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L#endif
+    config->customMotorMixer[7] = (motorMixer_t){ 1.0f,  1.0f,  0.414178f, -1.0f };    // MIDREAR_L
 }
diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c
index f8cabdc76e..f50d744fdc 100644
--- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c
+++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c
@@ -30,13 +30,15 @@
 
 static const char * const hardwareRevisionNames[] = {
         "Unknown",
-        "AlienFlight V1",
-        "AlienFlight V2"
+        "AlienFlight F3 V1",
+        "AlienFlight F3 V2"
 };
 
-uint8_t hardwareRevision = UNKNOWN;
+uint8_t hardwareRevision = AFF3_UNKNOWN;
+uint8_t hardwareMotorType = MOTOR_UNKNOWN;
 
 static IO_t HWDetectPin = IO_NONE;
+static IO_t MotorDetectPin = IO_NONE;
 
 void detectHardwareRevision(void)
 {
@@ -44,14 +46,25 @@ void detectHardwareRevision(void)
     IOInit(HWDetectPin, OWNER_SYSTEM, 0);
     IOConfigGPIO(HWDetectPin, IOCFG_IPU);
 
-    // Check hardware revision
+    MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
+    IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
+    IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
+
     delayMicroseconds(10);  // allow configuration to settle
 
+    // Check hardware revision
     if (IORead(HWDetectPin)) {
         hardwareRevision = AFF3_REV_1;
     } else {
         hardwareRevision = AFF3_REV_2;
     }
+
+    // Check presence of brushed ESC's
+    if (IORead(MotorDetectPin)) {
+        hardwareMotorType = MOTOR_BRUSHLESS;
+    } else {
+        hardwareMotorType = MOTOR_BRUSHED;
+    }
 }
 
 void updateHardwareRevision(void)
diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h
index 745a9f9f52..9b0618561f 100644
--- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h
+++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h
@@ -17,12 +17,19 @@
 #pragma once
 
 typedef enum awf3HardwareRevision_t {
-    UNKNOWN = 0,
-    AFF3_REV_1, // MPU6050 / MPU9150 (I2C)
+    AFF3_UNKNOWN = 0,
+    AFF3_REV_1, // MPU6050 (I2C)
     AFF3_REV_2  // MPU6500 / MPU9250 (SPI)
 } awf3HardwareRevision_e;
 
+typedef enum awf4HardwareMotorType_t {
+    MOTOR_UNKNOWN = 0,
+    MOTOR_BRUSHED,
+    MOTOR_BRUSHLESS
+} awf4HardwareMotorType_e;
+
 extern uint8_t hardwareRevision;
+extern uint8_t hardwareMotorType;
 
 void updateHardwareRevision(void);
 void detectHardwareRevision(void);
diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c
index 65a745dd16..8f1138043d 100644
--- a/src/main/target/ALIENFLIGHTF3/target.c
+++ b/src/main/target/ALIENFLIGHTF3/target.c
@@ -26,16 +26,16 @@
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     // up to 10 Motor Outputs            
-    DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED),  // PWM1  - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2
-    DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED),  // PWM2  - PB14 - TIM1_CH2N, *TIM15_CH1
-    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED),  // PWM3  - PA8  - *TIM1_CH1, TIM4_ETR
-    DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM4  - PB0  - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
-    DEF_TIM(TIM3,  CH1, PA6,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM5  - PA6  - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1
-    DEF_TIM(TIM2,  CH3, PA2,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM6  - PA2  - *TIM2_CH3, !TIM15_CH1
-    DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM7  - PB1  - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
-    DEF_TIM(TIM17, CH1, PA7,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED),  // PWM8  - PA7  - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
-    DEF_TIM(TIM3,  CH2, PA4,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM9  - PA4  - *TIM3_CH2
-    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_PWM,               TIMER_INPUT_ENABLED ),  // PWM10 - PA1  - *TIM2_CH2, TIM15_CH1N
-    DEF_TIM(TIM2,  CH4, PA3,  TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM   - PA3  - TIM2_CH4, TIM15_CH2 - PWM13
+    DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM1  - PB15 - DMA_NONE - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2
+    DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM2  - PB14 - DMA1_CH5 - TIM1_CH2N, *TIM15_CH1
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM3  - PA8  - DMA1_CH2 - *TIM1_CH1, TIM4_ETR
+    DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM4  - PB0  - DMA1_CH2 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
+    DEF_TIM(TIM3,  CH1, PA6,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM5  - PA6  - DMA1_CH6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1
+    DEF_TIM(TIM2,  CH3, PA2,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM6  - PA2  - DMA1_CH1 - *TIM2_CH3, !TIM15_CH1
+    DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM7  - PB1  - DMA1_CH3 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
+    DEF_TIM(TIM17, CH1, PA7,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM8  - PA7  - DMA1_CH7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
+    DEF_TIM(TIM3,  CH2, PA4,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM9  - PA4  - DMA_NONE - *TIM3_CH2
+    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM10 - PA1  - DMA1_CH7 - *TIM2_CH2, TIM15_CH1N
+    DEF_TIM(TIM2,  CH4, PA3,  TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM   - PA3  - TIM2_CH4, TIM15_CH2 - PWM13
 };
 
diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h
index f834b5d619..63a3a9bb08 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/src/main/target/ALIENFLIGHTF3/target.h
@@ -24,6 +24,7 @@
 
 #define USE_HARDWARE_REVISION_DETECTION
 #define HW_PIN                  PB2
+#define MOTOR_PIN               PB15
 
 // LED's V1
 #define LED0                    PB4
@@ -114,7 +115,6 @@
 // Hardware bind plug at PB12 (Pin 25)
 #define BINDPLUG_PIN            PB12
 
-#define BRUSHED_MOTORS
 #define DEFAULT_FEATURES        FEATURE_MOTOR_STOP
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SPEKTRUM2048
diff --git a/src/main/target/ALIENFLIGHTF4/AlienFlight.md b/src/main/target/ALIENFLIGHTF4/AlienFlight.md
index 560536128a..3510b52c1c 100644
--- a/src/main/target/ALIENFLIGHTF4/AlienFlight.md
+++ b/src/main/target/ALIENFLIGHTF4/AlienFlight.md
@@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
 - STM32F103CBT6 MCU (ALIENFLIGHTF1)
 - STM32F303CCT6 MCU (ALIENFLIGHTF3)
 - STM32F405RGT6 MCU (ALIENFLIGHTF4)
-- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
+- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
 - The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
 - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
 - extra-wide traces on the PCB, for maximum power throughput (brushed variants)
diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c
index b5f3188055..216d9e486f 100644
--- a/src/main/target/ALIENFLIGHTF4/config.c
+++ b/src/main/target/ALIENFLIGHTF4/config.c
@@ -24,6 +24,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/compass.h"
+#include "drivers/pwm_output.h"
 #include "drivers/serial.h"
 
 #include "fc/rc_controls.h"
@@ -47,10 +48,26 @@
 
 #include "hardware_revision.h"
 
+#define CURRENTOFFSET 2500                      // ACS712/714-30A - 0A = 2.5V
+#define CURRENTSCALE -667                       // ACS712/714-30A - 66.666 mV/A inverted mode
+
+#define BRUSHED_MOTORS_PWM_RATE 32000           // 32kHz
+
 // alternative defaults settings for AlienFlight targets
 void targetConfiguration(master_t *config)
 {
-    config->mag_hardware = MAG_NONE;            // disabled by default
+    config->batteryConfig.currentMeterOffset = CURRENTOFFSET;
+    config->batteryConfig.currentMeterScale = CURRENTSCALE;
+    config->sensorSelectionConfig.mag_hardware = MAG_NONE;            // disabled by default
+
+    if (hardwareMotorType == MOTOR_BRUSHED) {
+        config->motorConfig.minthrottle = 1000;
+        config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
+        config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
+        config->pid_process_denom = 1;
+        config->motorConfig.useUnsyncedPwm = true;
+    }
+
     if (hardwareRevision == AFF4_REV_1) {
         config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048;
         config->rxConfig.spektrum_sat_bind = 5;
@@ -58,15 +75,11 @@ void targetConfiguration(master_t *config)
     } else {
         config->rxConfig.serialrx_provider = SERIALRX_SBUS;
         config->rxConfig.sbus_inversion = 0;
-        config->serialConfig.portConfigs[SERIAL_PORT_USART2].functionMask = FUNCTION_TELEMETRY_FRSKY;
+        config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY;
         config->telemetryConfig.telemetry_inversion = 0;
         intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures);
-        config->batteryConfig.currentMeterOffset = 2500;
-        config->batteryConfig.currentMeterScale = -667;
     }
-    config->motorConfig.motorPwmRate = 32000;
-    config->gyro_sync_denom = 1;
-    config->pid_process_denom = 1;
+
     config->profile[0].pidProfile.P8[ROLL] = 53;
     config->profile[0].pidProfile.I8[ROLL] = 45;
     config->profile[0].pidProfile.D8[ROLL] = 52;
diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.c b/src/main/target/ALIENFLIGHTF4/hardware_revision.c
index bb58b1bcf2..4e55f9c7fb 100644
--- a/src/main/target/ALIENFLIGHTF4/hardware_revision.c
+++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.c
@@ -25,18 +25,19 @@
 
 #include "drivers/system.h"
 #include "drivers/io.h"
-#include "drivers/exti.h"
 #include "hardware_revision.h"
 
 static const char * const hardwareRevisionNames[] = {
         "Unknown",
-        "AlienFlight V1",
-        "AlienFlight V2"
+        "AlienFlight F4 V1",
+        "AlienFlight F4 V2"
 };
 
-uint8_t hardwareRevision = UNKNOWN;
+uint8_t hardwareRevision = AFF4_UNKNOWN;
+uint8_t hardwareMotorType = MOTOR_UNKNOWN;
 
 static IO_t HWDetectPin = IO_NONE;
+static IO_t MotorDetectPin = IO_NONE;
 
 void detectHardwareRevision(void)
 {
@@ -44,14 +45,25 @@ void detectHardwareRevision(void)
     IOInit(HWDetectPin, OWNER_SYSTEM, 0);
     IOConfigGPIO(HWDetectPin, IOCFG_IPU);
 
-    // Check hardware revision
+    MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
+    IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
+    IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
+
     delayMicroseconds(10);  // allow configuration to settle
 
+    // Check hardware revision
     if (IORead(HWDetectPin)) {
         hardwareRevision = AFF4_REV_1;
     } else {
         hardwareRevision = AFF4_REV_2;
     }
+
+    // Check presence of brushed ESC's
+    if (IORead(MotorDetectPin)) {
+        hardwareMotorType = MOTOR_BRUSHLESS;
+    } else {
+        hardwareMotorType = MOTOR_BRUSHED;
+    }
 }
 
 void updateHardwareRevision(void)
diff --git a/src/main/target/ALIENFLIGHTF4/hardware_revision.h b/src/main/target/ALIENFLIGHTF4/hardware_revision.h
index dc9c8325c3..e421b88960 100644
--- a/src/main/target/ALIENFLIGHTF4/hardware_revision.h
+++ b/src/main/target/ALIENFLIGHTF4/hardware_revision.h
@@ -17,14 +17,19 @@
 #pragma once
 
 typedef enum awf4HardwareRevision_t {
-    UNKNOWN = 0,
-    AFF4_REV_1,
-    AFF4_REV_2
+    AFF4_UNKNOWN = 0,
+    AFF4_REV_1, // MPU6500 / MPU9250 (SPI), (V1.1 Current Sensor (ACS712), SDCard Reader)
+    AFF4_REV_2  // ICM-20602 (SPI), OpenSky RX (CC2510), Current Sensor (ACS712), SDCard Reader
 } awf4HardwareRevision_e;
 
+typedef enum awf4HardwareMotorType_t {
+    MOTOR_UNKNOWN = 0,
+    MOTOR_BRUSHED,
+    MOTOR_BRUSHLESS
+} awf4HardwareMotorType_e;
+
 extern uint8_t hardwareRevision;
+extern uint8_t hardwareMotorType;
 
 void updateHardwareRevision(void);
 void detectHardwareRevision(void);
-
-struct extiConfig_s;
diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c
index a2d744fc68..10a23f8986 100644
--- a/src/main/target/ALIENFLIGHTF4/target.c
+++ b/src/main/target/ALIENFLIGHTF4/target.c
@@ -25,17 +25,17 @@
 #include "drivers/timer_def.h"
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
-    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PWM1  - PA8  RC1
-    DEF_TIM(TIM1, CH2, PB0,  TIM_USE_PWM,               0, 0), // PWM2  - PB0  RC2
-    DEF_TIM(TIM1, CH3, PB1,  TIM_USE_PWM,               0, 0), // PWM3  - PB1  RC3
-    DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM,               0, 0), // PWM4  - PA14 RC4
-    DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM,               0, 0), // PWM5  - PA15 RC5
-    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MOTOR,             1, 0), // PWM6  - PB8  OUT1
-    DEF_TIM(TIM4, CH4, PB9,  TIM_USE_MOTOR,             1, 0), // PWM7  - PB9  OUT2
-    DEF_TIM(TIM5, CH1, PA0,  TIM_USE_MOTOR,             1, 0), // PWM8  - PA0  OUT3
-    DEF_TIM(TIM5, CH2, PA1,  TIM_USE_MOTOR,             1, 0), // PWM9  - PA1  OUT4
-    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MOTOR,             1, 0), // PWM10 - PC6  OUT5
-    DEF_TIM(TIM3, CH2, PC7,  TIM_USE_MOTOR,             1, 0), // PWM11 - PC7  OUT6
-    DEF_TIM(TIM3, CH3, PC8,  TIM_USE_MOTOR,             1, 0), // PWM13 - PC8  OUT7
-    DEF_TIM(TIM3, CH4, PC9,  TIM_USE_MOTOR,             1, 0), // PWM13 - PC9  OUT8
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED,  0), // PWM1  - PA8  RC1
+    DEF_TIM(TIM1, CH2, PB0,  TIM_USE_PWM,               TIMER_INPUT_ENABLED,  0), // PWM2  - PB0  RC2
+    DEF_TIM(TIM1, CH3, PB1,  TIM_USE_PWM,               TIMER_INPUT_ENABLED,  0), // PWM3  - PB1  RC3
+    DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM,               TIMER_INPUT_ENABLED,  0), // PWM4  - PA14 RC4
+    DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM,               TIMER_INPUT_ENABLED,  0), // PWM5  - PA15 RC5
+    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM6  - PB8  OUT1 - DMA1_ST7
+    DEF_TIM(TIM4, CH4, PB9,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM7  - PB9  OUT2 - DMA1_ST3
+    DEF_TIM(TIM5, CH1, PA0,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM8  - PA0  OUT3 - DMA1_ST2
+    DEF_TIM(TIM5, CH2, PA1,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM9  - PA1  OUT4 - DMA1_ST4
+    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM10 - PC6  OUT5 - (DMA1_ST4)
+    DEF_TIM(TIM3, CH2, PC7,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM11 - PC7  OUT6 - DMA1_ST5
+    DEF_TIM(TIM3, CH3, PC8,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC8  OUT7 - (DMA1_ST7)
+    DEF_TIM(TIM3, CH4, PC9,  TIM_USE_MOTOR,             TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC9  OUT8 - (DMA1_ST2)
 };
diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h
index 22380a7048..058759327e 100644
--- a/src/main/target/ALIENFLIGHTF4/target.h
+++ b/src/main/target/ALIENFLIGHTF4/target.h
@@ -21,6 +21,7 @@
 
 #define USE_HARDWARE_REVISION_DETECTION
 #define HW_PIN                  PC13
+#define MOTOR_PIN               PB8
 
 #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
 
@@ -82,10 +83,12 @@
 // Divide to under 25MHz for normal operation:
 #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
 
+//#ifndef USE_DSHOT
 #define SDCARD_DMA_CHANNEL_TX               DMA1_Stream4
 #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
 #define SDCARD_DMA_CLK                      RCC_AHB1Periph_DMA1
 #define SDCARD_DMA_CHANNEL                  DMA_Channel_0
+//#endif
 
 // Performance logging for SD card operations:
 // #define AFATFS_USE_INTROSPECTIVE_LOGGING
@@ -160,12 +163,13 @@
 
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
-#define BRUSHED_MOTORS
 #define DEFAULT_FEATURES        (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX)
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
 #define SERIALRX_UART           SERIAL_PORT_USART2
 #define RX_CHANNELS_TAER
 
+#define TELEMETRY_UART          SERIAL_PORT_USART1
+
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define TARGET_IO_PORTA         0xffff
diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c
index 1eaa69d6d5..ffcb7a3b76 100644
--- a/src/main/target/BLUEJAYF4/config.c
+++ b/src/main/target/BLUEJAYF4/config.c
@@ -51,8 +51,8 @@ void targetValidateConfiguration(master_t *config)
     if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) {
         intFeatureClear(FEATURE_SDCARD, &config->enabledFeatures);
         
-        if (config->blackbox_device == BLACKBOX_DEVICE_SDCARD) {
-            config->blackbox_device = BLACKBOX_DEVICE_FLASH;
+        if (config->blackboxConfig.device == BLACKBOX_DEVICE_SDCARD) {
+            config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH;
         }
     }
 }
diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c
index 83f103e6ec..e01ae9d851 100644
--- a/src/main/target/COLIBRI/config.c
+++ b/src/main/target/COLIBRI/config.c
@@ -39,7 +39,7 @@
 // alternative defaults settings for Colibri/Gemini targets
 void targetConfiguration(master_t *config)
 {
-    config->mixerMode = MIXER_HEX6X;
+    config->mixerConfig.mixerMode = MIXER_HEX6X;
     config->rxConfig.serialrx_provider = 2;
 
     config->motorConfig.minthrottle = 1070;
@@ -48,7 +48,7 @@ void targetConfiguration(master_t *config)
     config->boardAlignment.pitchDegrees = 10;
     //config->rcControlsConfig.deadband = 10;
     //config->rcControlsConfig.yaw_deadband = 10;
-    config->mag_hardware = 1;
+    config->sensorSelectionConfig.mag_hardware = 1;
 
     config->profile[0].controlRateProfile[0].dynThrPID = 45;
     config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700;
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index 1981cfd144..5774973293 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -556,7 +556,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             // DEPRECATED - Use MSP_API_VERSION
         case BST_IDENT:
             bstWrite8(MW_VERSION);
-            bstWrite8(masterConfig.mixerMode);
+            bstWrite8(masterConfig.mixerConfig.mixerMode);
             bstWrite8(BST_PROTOCOL_VERSION);
             bstWrite32(CAP_DYNBALANCE); // "capability"
             break;
@@ -683,8 +683,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
                 bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
             break;
         case BST_ARMING_CONFIG:
-            bstWrite8(masterConfig.auto_disarm_delay);
-            bstWrite8(masterConfig.disarm_kill_switch);
+            bstWrite8(masterConfig.armingConfig.auto_disarm_delay);
+            bstWrite8(masterConfig.armingConfig.disarm_kill_switch);
             break;
         case BST_LOOP_TIME:
             //bstWrite16(masterConfig.looptime);
@@ -769,7 +769,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             bstWrite8(masterConfig.rxConfig.rssi_channel);
             bstWrite8(0);
 
-            bstWrite16(masterConfig.mag_declination / 10);
+            bstWrite16(masterConfig.compassConfig.mag_declination / 10);
 
             bstWrite8(masterConfig.batteryConfig.vbatscale);
             bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage);
@@ -868,7 +868,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             break;
 
         case BST_MIXER:
-            bstWrite8(masterConfig.mixerMode);
+            bstWrite8(masterConfig.mixerConfig.mixerMode);
             break;
 
         case BST_RX_CONFIG:
@@ -904,7 +904,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             break;
 
         case BST_BF_CONFIG:
-            bstWrite8(masterConfig.mixerMode);
+            bstWrite8(masterConfig.mixerConfig.mixerMode);
 
             bstWrite32(featureMask());
 
@@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             bstWrite8(masterConfig.rcControlsConfig.yaw_deadband);
             break;
         case BST_FC_FILTERS:
-            bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
+            bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
             break;
         default:
             // we do not know how to handle the (valid) message, indicate error BST
@@ -1033,8 +1033,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             masterConfig.accelerometerTrims.values.roll  = bstRead16();
             break;
         case BST_SET_ARMING_CONFIG:
-            masterConfig.auto_disarm_delay = bstRead8();
-            masterConfig.disarm_kill_switch = bstRead8();
+            masterConfig.armingConfig.auto_disarm_delay = bstRead8();
+            masterConfig.armingConfig.disarm_kill_switch = bstRead8();
             break;
         case BST_SET_LOOP_TIME:
             //masterConfig.looptime = bstRead16();
@@ -1132,7 +1132,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             masterConfig.rxConfig.rssi_channel = bstRead8();
             bstRead8();
 
-            masterConfig.mag_declination = bstRead16() * 10;
+            masterConfig.compassConfig.mag_declination = bstRead16() * 10;
 
             masterConfig.batteryConfig.vbatscale = bstRead8();           // actual vbatscale as intended
             masterConfig.batteryConfig.vbatmincellvoltage = bstRead8();  // vbatlevel_warn1 in MWC2.3 GUI
@@ -1273,7 +1273,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
 
 #ifndef USE_QUAD_MIXER_ONLY
         case BST_SET_MIXER:
-            masterConfig.mixerMode = bstRead8();
+            masterConfig.mixerConfig.mixerMode = bstRead8();
             break;
 #endif
 
@@ -1319,7 +1319,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
 #ifdef USE_QUAD_MIXER_ONLY
            bstRead8(); // mixerMode ignored
 #else
-           masterConfig.mixerMode = bstRead8(); // mixerMode
+           masterConfig.mixerConfig.mixerMode = bstRead8(); // mixerMode
 #endif
 
            featureClearAll();
@@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             masterConfig.rcControlsConfig.yaw_deadband = bstRead8();
             break;
         case BST_SET_FC_FILTERS:
-            masterConfig.gyro_lpf = bstRead16();
+            masterConfig.gyroConfig.gyro_lpf = bstRead16();
             break;
 
         default:
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index a23d58a37b..0a78914618 100644
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -59,6 +59,15 @@
 #define USE_BARO_MS5611
 //#define USE_BARO_BMP280
 
+#define OSD
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE    SPI3
+#define MAX7456_SPI_CS_PIN      PB14
+
+#define MAX7456_DMA_CHANNEL_TX              DMA1_Stream5
+#define MAX7456_DMA_CHANNEL_RX              DMA1_Stream0
+#define MAX7456_DMA_IRQ_HANDLER_ID          DMA1_ST0_HANDLER
+
 #define M25P16_CS_PIN           PB3
 #define M25P16_SPI_INSTANCE     SPI3
 
diff --git a/src/main/target/KAKUTEF4/target.mk b/src/main/target/KAKUTEF4/target.mk
index b334aa90ff..10665d1634 100644
--- a/src/main/target/KAKUTEF4/target.mk
+++ b/src/main/target/KAKUTEF4/target.mk
@@ -6,4 +6,5 @@ TARGET_SRC = \
             drivers/barometer_bmp280.c \
             drivers/barometer_ms5611.c \
             drivers/compass_ak8963.c \
-            drivers/compass_hmc5883l.c 
\ No newline at end of file
+            drivers/compass_hmc5883l.c \
+            drivers/max7456.c
diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c
index 4cd38f98f2..94a217a364 100755
--- a/src/main/target/MOTOLAB/config.c
+++ b/src/main/target/MOTOLAB/config.c
@@ -26,6 +26,6 @@
 // Motolab target supports 2 different type of boards Tornado / Cyclone.
 void targetConfiguration(master_t *config)
 {
-    config->gyro_sync_denom = 4;
+    config->gyroConfig.gyro_sync_denom = 4;
     config->pid_process_denom = 1;
 }
diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c
index 543db24752..ddc186ccc7 100755
--- a/src/main/target/MULTIFLITEPICO/config.c
+++ b/src/main/target/MULTIFLITEPICO/config.c
@@ -41,7 +41,7 @@
 
 // alternative defaults settings for MULTIFLITEPICO targets
 void targetConfiguration(master_t *config) {
-    config->mag_hardware = MAG_NONE;            // disabled by default
+    config->sensorSelectionConfig.mag_hardware = MAG_NONE;            // disabled by default
 
     config->batteryConfig.vbatscale = 100;
     config->batteryConfig.vbatresdivval = 15;
@@ -70,7 +70,7 @@ void targetConfiguration(master_t *config) {
 
     config->motorConfig.motorPwmRate = 17000;
 
-    config->gyro_sync_denom = 4;
+    config->gyroConfig.gyro_sync_denom = 4;
     config->pid_process_denom = 1;
 
     config->profile[0].pidProfile.P8[ROLL] = 70;
diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c
index 9fdbca437c..b4142093eb 100755
--- a/src/main/target/NAZE/config.c
+++ b/src/main/target/NAZE/config.c
@@ -43,10 +43,10 @@ void targetConfiguration(master_t *config)
 
     config->motorConfig.minthrottle = 1049;
 
-    config->gyro_lpf = 1;
-    config->gyro_soft_lpf_hz = 100;
-    config->gyro_soft_notch_hz_1 = 0;
-    config->gyro_soft_notch_hz_2 = 0;
+    config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ;
+    config->gyroConfig.gyro_soft_lpf_hz = 100;
+    config->gyroConfig.gyro_soft_notch_hz_1 = 0;
+    config->gyroConfig.gyro_soft_notch_hz_2 = 0;
 
     /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
         config->rxConfig.channelRanges[channel].min = 1180;
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 82bc69ba70..184ea76ee5 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -20,6 +20,8 @@
 #define TARGET_CONFIG
 #define USE_HARDWARE_REVISION_DETECTION
 
+#define CLI_MINIMAL_VERBOSITY
+
 #define BOARD_HAS_VOLTAGE_DIVIDER
 
 #define LED0                    PB3
diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c
index 74a21fdb61..16f17c726d 100644
--- a/src/main/target/YUPIF4/target.c
+++ b/src/main/target/YUPIF4/target.c
@@ -20,15 +20,17 @@
 #include <platform.h>
 #include "drivers/io.h"
 #include "drivers/timer.h"
+#include "drivers/timer_def.h"
 #include "drivers/dma.h"
 
+
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
-    { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PPM,   0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN
-    { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1
-    { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2
-    { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3
-    { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4
-    { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5
-    { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6
+    DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM,                 TIMER_INPUT_ENABLED,  0 ), // PPM IN
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR,               TIMER_OUTPUT_ENABLED, 0 ), // S1_OUT - DMA1_ST2
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR,               TIMER_OUTPUT_ENABLED, 0 ), // S2_OUT - DMA1_ST4
+    DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR,               TIMER_OUTPUT_ENABLED, 0 ), // S3_OUT - DMA1_ST1
+    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR,               TIMER_OUTPUT_ENABLED, 1 ), // S4_OUT - DMA1_ST6
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0 ), // S5_OUT - DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR,               TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2
 };
 
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index b25ea293c7..1a38664fc7 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -109,11 +109,11 @@
 // SPI Ports
 #define USE_SPI
 
-#define USE_SPI_DEVICE_1 //MPU6500
+#define USE_SPI_DEVICE_1 //Gyro
 #define SPI1_NSS_PIN            PA4
 #define SPI1_SCK_PIN            PA5
 #define SPI1_MISO_PIN           PA6
-#define SPI1_MOSI_PIN             PA7
+#define SPI1_MOSI_PIN           PA7
 
 /*
 #define USE_SPI_DEVICE_2 //Free
@@ -138,7 +138,7 @@
 #define VBAT_ADC_PIN            PC1
 #define RSSI_ADC_GPIO_PIN       PC0
 
-
+#define USE_DSHOT
 #define LED_STRIP
 
 // Default configuration
@@ -156,4 +156,4 @@
 #define TARGET_IO_PORTD         (BIT(2))
 
 #define USABLE_TIMER_CHANNEL_COUNT 7
-#define USED_TIMERS             (TIM_N(2) | TIM_N(3) | TIM_N(8))
+#define USED_TIMERS             (TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8))
diff --git a/src/main/target/link/stm32_flash_f103_128k.ld b/src/main/target/link/stm32_flash_f103_128k.ld
index 48441c237c..eaeaa48843 100644
--- a/src/main/target/link/stm32_flash_f103_128k.ld
+++ b/src/main/target/link/stm32_flash_f103_128k.ld
@@ -12,9 +12,11 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */
-  RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 20K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 126K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
+
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 20K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f103_128k_opbl.ld b/src/main/target/link/stm32_flash_f103_128k_opbl.ld
index a8fad3a5f4..c25b881d98 100644
--- a/src/main/target/link/stm32_flash_f103_128k_opbl.ld
+++ b/src/main/target/link/stm32_flash_f103_128k_opbl.ld
@@ -12,10 +12,12 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH (rx)      : ORIGIN = 0x08003000, LENGTH = 126K - 0x03000 /* last 2kb used for config storage first 12k for OP Bootloader*/
+    /* First 12K (0x3000 bytes) used for OP Bootloader, last 2K used for config storage */
+    FLASH (rx)        : ORIGIN = 0x08003000, LENGTH = 114K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
 
-  RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 20K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 20K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f103_256k.ld b/src/main/target/link/stm32_flash_f103_256k.ld
index 0f0c2fe88c..b95a15f070 100644
--- a/src/main/target/link/stm32_flash_f103_256k.ld
+++ b/src/main/target/link/stm32_flash_f103_256k.ld
@@ -12,9 +12,11 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
-  RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 48K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 252K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0803F000, LENGTH = 4K
+
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 48K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f103_64k.ld b/src/main/target/link/stm32_flash_f103_64k.ld
index a72e2b6082..32ac49552f 100644
--- a/src/main/target/link/stm32_flash_f103_64k.ld
+++ b/src/main/target/link/stm32_flash_f103_64k.ld
@@ -12,9 +12,11 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 62K /* last 2kb used for config storage */
-  RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 20K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 62K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0800F800, LENGTH = 2K
+
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 20K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld
index a13c304581..8d948e1d45 100644
--- a/src/main/target/link/stm32_flash_f303_128k.ld
+++ b/src/main/target/link/stm32_flash_f303_128k.ld
@@ -12,9 +12,12 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH  (rx)     : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */
-  RAM    (xrw)    : ORIGIN = 0x20000000, LENGTH = 40K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 126K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
+
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
+    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld
index 2db0e7713f..2f10b692d5 100644
--- a/src/main/target/link/stm32_flash_f303_256k.ld
+++ b/src/main/target/link/stm32_flash_f303_256k.ld
@@ -12,9 +12,12 @@
 /* Specify the memory areas. */
 MEMORY
 {
-  FLASH  (rx)     : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
-  RAM    (xrw)    : ORIGIN = 0x20000000, LENGTH = 40K
-  MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 252K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0803F000, LENGTH = 4K
+
+    RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
+    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld
index a792a296c6..7bd4c75660 100644
--- a/src/main/target/link/stm32_flash_f405.ld
+++ b/src/main/target/link/stm32_flash_f405.ld
@@ -12,10 +12,18 @@
 /* Entry Point */
 ENTRY(Reset_Handler)
 
+/*
+0x08000000 to 0x08100000 1024K full flash,
+0x08000000 to 0x080FC000 1008K firmware,
+0x080FC000 to 0x08100000   16K config,
+*/
+
 /* Specify the memory areas */
 MEMORY
 {
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 1M
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 1008K
+    FLASH_CONFIG (r)  : ORIGIN = 0x080FC000, LENGTH = 16K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
     BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld
index a9e2b7dd84..27d7544d89 100644
--- a/src/main/target/link/stm32_flash_f405_opbl.ld
+++ b/src/main/target/link/stm32_flash_f405_opbl.ld
@@ -12,21 +12,22 @@
 /* Entry Point */
 ENTRY(Reset_Handler)
 
-/* Specify the memory areas */
-
 /*
-0x08000000 to 0x08100000 1024kb full flash,
-0x08000000 to 0x08020000 128kb OPBL,
-0x08020000 to 0x08080000 384kb firmware,
-0x08080000 to 0x080A0000 128kb config,
+0x08000000 to 0x08100000 1024K full flash,
+0x08000000 to 0x08004000   16K OPBL,
+0x08004000 to 0x080FC000  992K firmware,
+0x080FC000 to 0x08100000   16K config,
 */
 
+/* Specify the memory areas */
 MEMORY
 {
-	FLASH (rx)      : ORIGIN = 0x08020000, LENGTH = 0x000A0000 
-  CCM (rwx)       : ORIGIN = 0x10000000, LENGTH = 64K
-	RAM (xrw)       : ORIGIN = 0x20000000, LENGTH = 0x00020000
-	MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)      : ORIGIN = 0x08004000, LENGTH = 992K
+    FLASH_CONFIG (r): ORIGIN = 0x080FC000, LENGTH = 16K
+
+    RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 128K
+    CCM (rwx)       : ORIGIN = 0x10000000, LENGTH = 64K
+    MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
\ No newline at end of file
diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld
index d145ff3afc..b7439b60d3 100644
--- a/src/main/target/link/stm32_flash_f411.ld
+++ b/src/main/target/link/stm32_flash_f411.ld
@@ -13,17 +13,20 @@
 ENTRY(Reset_Handler)
 
 /*
-0x08000000 to 0x08080000 512kb full flash,
-0x08000000 to 0x08060000 384kb firmware,
-0x08060000 to 0x08080000 128kb config,
+0x08000000 to 0x08080000 512K full flash,
+0x08000000 to 0x0807C000 469K firmware,
+0x0807C000 to 0x08080000  16K config,
 */
 
 /* Specify the memory areas */
 MEMORY
 {
-	FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 0x00080000 
-	RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 0x00020000
-	MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 496K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0807C000, LENGTH = 16K
+
+    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
+    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 64K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld
index 5787e6d20b..e329397e61 100644
--- a/src/main/target/link/stm32_flash_f411_opbl.ld
+++ b/src/main/target/link/stm32_flash_f411_opbl.ld
@@ -13,18 +13,21 @@
 ENTRY(Reset_Handler)
 
 /*
-0x08000000 to 0x08080000 512kb full flash,
-0x08000000 to 0x08010000 64kb OPBL,
-0x08010000 to 0x08060000 320kb firmware,
-0x08060000 to 0x08080000 128kb config,
+0x08000000 to 0x08080000 512K full flash,
+0x08000000 to 0x08004000  16K OPBL,
+0x08010000 to 0x0807C000 480K firmware,
+0x0807C000 to 0x08080000  16K config,
 */
 
 /* Specify the memory areas */
 MEMORY
 {
-	FLASH (rx)      : ORIGIN = 0x08010000, LENGTH = 0x00070000 
-	RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 0x00020000
-	MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
+    FLASH (rx)        : ORIGIN = 0x08004000, LENGTH = 480K
+    FLASH_CONFIG (r)  : ORIGIN = 0x0807C000, LENGTH = 16K
+
+    RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
+    CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 64K
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
 INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld
index 62270ea0f0..6c60e5c2c8 100644
--- a/src/main/target/link/stm32_flash_f745.ld
+++ b/src/main/target/link/stm32_flash_f745.ld
@@ -15,7 +15,9 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 1M
+    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 1008K
+    FLASH_CONFIG (r)  : ORIGIN = 0x080FC000, LENGTH = 16K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 320K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index dcf448edaf..a5f619d6dd 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -142,7 +142,7 @@ CRC:            (uint8_t)
 Payload:
 int32_t     Latitude ( degree / 10`000`000 )
 int32_t     Longitude (degree / 10`000`000 )
-uint16_t    Groundspeed ( km/h / 100 )
+uint16_t    Groundspeed ( km/h / 10 )
 uint16_t    GPS heading ( degree / 100 )
 uint16      Altitude ( meter ­ 1000m offset )
 uint8_t     Satellites in use ( counter )
@@ -154,7 +154,7 @@ void crsfFrameGps(sbuf_t *dst)
     crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
     crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
     crsfSerialize32(dst, GPS_coord[LON]);
-    crsfSerialize16(dst, GPS_speed * 36); // GPS_speed is in 0.1m/s
+    crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
     crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
     //Send real GPS altitude only if it's reliable (there's a GPS fix)
     const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
@@ -215,39 +215,6 @@ typedef enum {
     CRSF_RF_POWER_2000_mW = 6,
 } crsrRfPower_e;
 
-/*
-0x14 Link statistics
-Uplink is the connection from the ground to the UAV and downlink the opposite direction.
-Payload:
-uint8_t     UplinkRSSI Ant.1(dBm*­1)
-uint8_t     UplinkRSSI Ant.2(dBm*­1)
-uint8_t     Uplink Package success rate / Link quality ( % )
-int8_t      Uplink SNR ( db )
-uint8_t     Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
-uint8_t     RF Mode ( enum 4fps = 0 , 50fps, 150hz)
-uint8_t     Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
-uint8_t     Downlink RSSI ( dBm * ­-1 )
-uint8_t     Downlink package success rate / Link quality ( % )
-int8_t      Downlink SNR ( db )
-*/
-
-void crsfFrameLinkStatistics(sbuf_t *dst)
-{
-    // use sbufWrite since CRC does not include frame length
-    sbufWriteU8(dst, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_LINK_STATISTICS);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-    crsfSerialize8(dst, 0);
-}
-
 /*
 0x1E Attitude
 Payload:
@@ -256,7 +223,7 @@ int16_t     Roll angle ( rad / 10000 )
 int16_t     Yaw angle ( rad / 10000 )
 */
 
-#define DECIDEGREES_TO_RADIANS10000(angle) (1000.0f * (angle) * RAD)
+#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
 
 void crsfFrameAttitude(sbuf_t *dst)
 {
@@ -301,14 +268,10 @@ void crsfFrameFlightMode(sbuf_t *dst)
 #define BV(x)  (1 << (x)) // bit value
 
 // schedule array to decide how often each type of frame is sent
-#define CRSF_SCHEDULE_COUNT     5
-static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = {
-    BV(CRSF_FRAME_ATTITUDE),
-    BV(CRSF_FRAME_BATTERY_SENSOR),
-    BV(CRSF_FRAME_LINK_STATISTICS),
-    BV(CRSF_FRAME_FLIGHT_MODE),
-    BV(CRSF_FRAME_GPS),
-};
+#define CRSF_SCHEDULE_COUNT_MAX     5
+static uint8_t crsfScheduleCount;
+static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+
 
 static void processCrsf(void)
 {
@@ -328,11 +291,6 @@ static void processCrsf(void)
         crsfFrameBatterySensor(dst);
         crsfFinalize(dst);
     }
-    if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) {
-        crsfInitializeFrame(dst);
-        crsfFrameLinkStatistics(dst);
-        crsfFinalize(dst);
-    }
     if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
         crsfInitializeFrame(dst);
         crsfFrameFlightMode(dst);
@@ -345,7 +303,7 @@ static void processCrsf(void)
         crsfFinalize(dst);
     }
 #endif
-    crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT;
+    crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
 
 void initCrsfTelemetry(void)
@@ -353,6 +311,15 @@ void initCrsfTelemetry(void)
     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
     // and feature is enabled, if so, set CRSF telemetry enabled
     crsfTelemetryEnabled = crsfRxIsActive();
+    int index = 0;
+    crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
+    crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
+    crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
+    if (feature(FEATURE_GPS)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
+    }
+    crsfScheduleCount = (uint8_t)index;
+
  }
 
 bool checkCrsfTelemetryState(void)
@@ -396,9 +363,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
     case CRSF_FRAME_BATTERY_SENSOR:
         crsfFrameBatterySensor(sbuf);
         break;
-    case CRSF_FRAME_LINK_STATISTICS:
-        crsfFrameLinkStatistics(sbuf);
-        break;
     case CRSF_FRAME_FLIGHT_MODE:
         crsfFrameFlightMode(sbuf);
         break;
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 3b6c5d4cca..d6a126a29e 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -23,7 +23,6 @@ typedef enum {
     CRSF_FRAME_START = 0,
     CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
     CRSF_FRAME_BATTERY_SENSOR,
-    CRSF_FRAME_LINK_STATISTICS,
     CRSF_FRAME_FLIGHT_MODE,
     CRSF_FRAME_GPS,
     CRSF_FRAME_COUNT
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index f8663dcf7e..411089db02 100755
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -170,16 +170,23 @@ void configureMAVLinkTelemetryPort(void)
 
 void checkMAVLinkTelemetryState(void)
 {
-    bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
+    if (portConfig && telemetryCheckRxPortShared(portConfig)) {
+        if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) {
+            mavlinkPort = telemetrySharedPort;
+            mavlinkTelemetryEnabled = true;
+        }
+    } else {
+        bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
 
-    if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
-        return;
+        if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
+            return;
+        }
+
+        if (newTelemetryEnabledValue)
+            configureMAVLinkTelemetryPort();
+        else
+            freeMAVLinkTelemetryPort();
     }
-
-    if (newTelemetryEnabledValue)
-        configureMAVLinkTelemetryPort();
-    else
-        freeMAVLinkTelemetryPort();
 }
 
 void mavlinkSendSystemStatus(void)
@@ -430,7 +437,7 @@ void mavlinkSendHUDAndHeartbeat(void)
         mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
 
     uint8_t mavSystemType;
-    switch(masterConfig.mixerMode)
+    switch(masterConfig.mixerConfig.mixerMode)
     {
         case MIXER_TRI:
             mavSystemType = MAV_TYPE_TRICOPTER;
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index 214722f12e..9355c703db 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -59,4 +59,4 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing);
 
 void telemetryUseConfig(telemetryConfig_t *telemetryConfig);
 
-#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM)
+#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c
index 78455c00d8..7c8a1abe8b 100644
--- a/src/main/vcpf4/usbd_cdc_vcp.c
+++ b/src/main/vcpf4/usbd_cdc_vcp.c
@@ -156,7 +156,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len)
  * Output         : None.
  * Return         : None.
  *******************************************************************************/
-uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength)
+uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
 {
     VCP_DataTx(ptrBuffer, sendLength);
     return sendLength;
diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h
index c6544ab797..7ef4535883 100644
--- a/src/main/vcpf4/usbd_cdc_vcp.h
+++ b/src/main/vcpf4/usbd_cdc_vcp.h
@@ -36,7 +36,7 @@
 
 __ALIGN_BEGIN USB_OTG_CORE_HANDLE  USB_OTG_dev __ALIGN_END;
 
-uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength);  // HJI
+uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength);
 uint32_t CDC_Send_FreeBytes(void);
 uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len);       // HJI
 uint32_t CDC_Receive_BytesAvailable(void);
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index 028b0e75ac..6f1762c572 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -68,10 +68,11 @@ uint8_t crfsCrc(uint8_t *frame, int frameLen)
     }
     return crc;
 }
+
 /*
 int32_t     Latitude ( degree / 10`000`000 )
 int32_t     Longitude (degree / 10`000`000 )
-uint16_t    Groundspeed ( km/h / 100 )
+uint16_t    Groundspeed ( km/h / 10 )
 uint16_t    GPS heading ( degree / 100 )
 uint16      Altitude ( meter ­ 1000m offset )
 uint8_t     Satellites in use ( counter )
@@ -81,8 +82,7 @@ uint16_t GPS_distanceToHome;        // distance to home point in meters
 uint16_t GPS_altitude;              // altitude in m
 uint16_t GPS_speed;                 // speed in 0.1m/s
 uint16_t GPS_ground_course = 0;     // degrees * 10
-
- */
+*/
 #define FRAME_HEADER_FOOTER_LEN 4
 
 TEST(TelemetryCrsfTest, TestGPS)
@@ -112,7 +112,7 @@ TEST(TelemetryCrsfTest, TestGPS)
     GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
     ENABLE_STATE(GPS_FIX);
     GPS_altitude = 2345;              // altitude in m
-    GPS_speed = 163;                 // speed in 0.1m/s, 16.3 m/s = 58.68 km/h
+    GPS_speed = 163;                 // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
     GPS_numSat = 9;
     GPS_ground_course = 1479;     // degrees * 10
     frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
@@ -121,7 +121,7 @@ TEST(TelemetryCrsfTest, TestGPS)
     longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10];
     EXPECT_EQ(1630000000, longitude);
     groundSpeed = frame[11] << 8 | frame[12];
-    EXPECT_EQ(5868, groundSpeed);
+    EXPECT_EQ(587, groundSpeed);
     GPSheading = frame[13] << 8 | frame[14];
     EXPECT_EQ(14790, GPSheading);
     altitude = frame[15] << 8 | frame[16];
@@ -169,18 +169,6 @@ TEST(TelemetryCrsfTest, TestBattery)
     EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
 }
 
-TEST(TelemetryCrsfTest, TestLinkStatistics)
-{
-    uint8_t frame[CRSF_FRAME_SIZE_MAX];
-
-    int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS);
-    EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
-    EXPECT_EQ(12, frame[1]); // length
-    EXPECT_EQ(0x14, frame[2]); // type
-    EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]);
-}
-
 TEST(TelemetryCrsfTest, TestAttitude)
 {
     uint8_t frame[CRSF_FRAME_SIZE_MAX];
@@ -303,6 +291,8 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 
 uint32_t micros(void) {return 0;}
 
+bool feature(uint32_t) {return true;}
+
 uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
 uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
 uint8_t serialRead(serialPort_t *) {return 0;}