diff --git a/docs/Cli.md b/docs/Cli.md
index 5c184f77be..9566b0ea5a 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -251,13 +251,39 @@ Re-apply any new defaults as desired.
 |  blackbox_rate_num  | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations |
 |  blackbox_rate_denom  | 1 | Blackbox logging rate denominator. See blackbox_rate_num. |
 |  blackbox_device  | SPIFLASH | Selection of where to write blackbox data |
+|  ledstrip_visual_beeper  | OFF |  |
+|  osd_video_system     | 0     |  |
+|  osd_row_shiftdown    | 0     |  |
+|  osd_units            | 0     |  |
+|  osd_rssi_alarm       | 20    |  |
+|  osd_cap_alarm        | 2200  |  |
+|  osd_time_alarm       | 10    |  |
+|  osd_alt_alarm        | 100   |  |
+|  osd_main_voltage_pos | 0     |  |
+|  osd_rssi_pos         | 0     |  |
+|  osd_flytimer_pos     | 0     |  |
+|  osd_ontime_pos       | 0     |  |
+|  osd_flymode_pos      | 0     |  |
+|  osd_throttle_pos     | 0     |  |
+|  osd_vtx_channel_pos  | 0     |  |
+|  osd_crosshairs       | 0     |  |
+|  osd_artificial_horizon  | 0  |  |
+|  osd_current_draw_pos | 0     |  |
+|  osd_mah_drawn_pos    | 0     |  |
+|  osd_craft_name_pos   | 0     |  |
+|  osd_gps_speed_pos    | 0     |  |
+|  osd_gps_sats_pos     | 0     |  |
+|  osd_altitude_pos     | 0     |  |
+|  osd_pid_roll_pos     | 0     |  |
+|  osd_pid_pitch_pos    | 0     |  |
+|  osd_pid_yaw_pos      | 0     |  |
+|  osd_power_pos        | 0     |  |
 |  magzero_x  | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_y  | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_z  | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. |
 |  acczero_x  | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
 |  acczero_y  | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
 |  acczero_z  | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
-|  ledstrip_visual_beeper  | OFF |  |
 |  accgain_x  | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
 |  accgain_y  | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
 |  accgain_z  | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
@@ -287,7 +313,8 @@ Re-apply any new defaults as desired.
 |  default_rate_profile  | 0 | Default = profile number |
 |  mag_declination  | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl  is turned on and you aquirre valid GPS fix. |
 |  mag_hold_rate_limit  | 90 | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
-| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH               |
+| `mag_calibration_time` | 30 | Adjust how long time the Calibration of mag will last. |
+| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH               |
 | `mc_i_pitch` | 30 | Multicopter rate stabilisation I-gain for PITCH               |
 | `mc_d_pitch` | 23 | Multicopter rate stabilisation D-gain for PITCH               |
 | `mc_p_roll`  | 40 | Multicopter rate stabilisation P-gain for ROLL                |
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index d93813a35e..e88669dead 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -517,7 +517,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 
         case FLIGHT_LOG_FIELD_CONDITION_SONAR:
 #ifdef SONAR
-            return feature(FEATURE_SONAR);
+            return sensors(SENSOR_SONAR);
 #else
             return false;
 #endif
@@ -1135,7 +1135,7 @@ static void loadMainState(timeUs_t currentTimeUs)
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->gyroADC[i] = gyro.gyroADC[i];
+        blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
@@ -1338,7 +1338,7 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("rcRate:%d",                             100); //For compatibility reasons write rc_rate 100
         BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                        motorConfig()->minthrottle);
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                        motorConfig()->maxthrottle);
-        BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x",                       castFloatBytesToInt(gyro.dev.scale));
+        BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x",                       castFloatBytesToInt(1.0f));
         BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u",                             acc.dev.acc_1G);
 
         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index c9eac2165e..08bf1f4020 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -87,7 +87,8 @@
 #define PG_MODE_ACTIVATION_OPERATOR_CONFIG 1003
 #define PG_OSD_CONFIG 1004
 #define PG_BEEPER_CONFIG 1005
-#define PG_INAV_END 1005
+#define PG_RANGEFINDER_CONFIG 1006
+#define PG_INAV_END 1006
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index 8bf54eb3d4..1562376f12 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -50,6 +50,7 @@ typedef struct gyroDev_s {
     extiCallbackRec_t exti;
     float scale;                                            // scalefactor
     int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+    int16_t gyroZero[XYZ_AXIS_COUNT];
     uint8_t lpf;
     gyroRateKHz_e gyroRateKHz;
     uint8_t mpuDividerDrops;
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index c68a42f9dc..c68fa6a02c 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -267,20 +267,17 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
 }
 #endif
 
-mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
+void mpuDetect(gyroDev_t *gyro)
 {
-    bool ack;
-    uint8_t sig;
-    uint8_t inquiryResult;
-
     // MPU datasheet specifies 30ms.
     delay(35);
 
 #ifndef USE_I2C
-    ack = false;
-    sig = 0;
+    uint8_t sig = 0;
+    bool ack = false;
 #else
-    ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
+    uint8_t sig;
+    bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
 #endif
     if (ack) {
         gyro->mpuConfiguration.read = mpuReadRegisterI2C;
@@ -290,33 +287,30 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
         bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
         UNUSED(detectedSpiSensor);
 #endif
-
-        return &gyro->mpuDetectionResult;
+        return;
     }
 
     gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
 
     // If an MPU3050 is connected sig will contain 0.
+    uint8_t inquiryResult;
     ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
     inquiryResult &= MPU_INQUIRY_MASK;
     if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
         gyro->mpuDetectionResult.sensor = MPU_3050;
         gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
-        return &gyro->mpuDetectionResult;
+        return;
     }
 
     sig &= MPU_INQUIRY_MASK;
 
     if (sig == MPUx0x0_WHO_AM_I_CONST) {
-
         gyro->mpuDetectionResult.sensor = MPU_60x0;
-
         mpu6050FindRevision(gyro);
     } else if (sig == MPU6500_WHO_AM_I_CONST) {
         gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
     }
-
-    return &gyro->mpuDetectionResult;
+    return;
 }
 
 void mpuGyroInit(gyroDev_t *gyro)
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index 1e6ef622f3..4e0daef6f5 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -169,8 +169,13 @@ typedef enum {
     MPU_65xx_I2C,
     MPU_65xx_SPI,
     MPU_9250_SPI,
+    ICM_20601_SPI,
+    ICM_20602_SPI,
+    ICM_20608_SPI,
+    ICM_20649_SPI,
+    ICM_20679_SPI,
     ICM_20689_SPI
-} detectedMPUSensor_e;
+} mpuSensor_e;
 
 typedef enum {
     MPU_HALF_RESOLUTION,
@@ -178,7 +183,7 @@ typedef enum {
 } mpu6050Resolution_e;
 
 typedef struct mpuDetectionResult_s {
-    detectedMPUSensor_e sensor;
+    mpuSensor_e sensor;
     mpu6050Resolution_e resolution;
 } mpuDetectionResult_t;
 
@@ -187,7 +192,7 @@ void mpuGyroInit(struct gyroDev_s *gyro);
 struct accDev_s;
 bool mpuAccRead(struct accDev_s *acc);
 bool mpuGyroRead(struct gyroDev_s *gyro);
-mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
+void mpuDetect(struct gyroDev_s *gyro);
 bool mpuCheckDataReady(struct gyroDev_s *gyro);
 void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
 
diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c
index 27c8ce67d2..7b4d6875c4 100644
--- a/src/main/drivers/light_ws2811strip_hal.c
+++ b/src/main/drivers/light_ws2811strip_hal.c
@@ -91,23 +91,23 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
     /* Set hdma_tim instance */
     hdma_tim.Instance = WS2811_DMA_STREAM;
 
-    uint32_t channelAddress = 0;
+    //uint32_t channelAddress = 0;
     switch (WS2811_TIMER_CHANNEL) {
         case TIM_CHANNEL_1:
             timDMASource = TIM_DMA_ID_CC1;
-            channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
+            //channelAddress = (uint32_t)(&WS2811_TIMER->CCR1);
             break;
         case TIM_CHANNEL_2:
             timDMASource = TIM_DMA_ID_CC2;
-            channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
+            //channelAddress = (uint32_t)(&WS2811_TIMER->CCR2);
             break;
         case TIM_CHANNEL_3:
             timDMASource = TIM_DMA_ID_CC3;
-            channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
+            //channelAddress = (uint32_t)(&WS2811_TIMER->CCR3);
             break;
         case TIM_CHANNEL_4:
             timDMASource = TIM_DMA_ID_CC4;
-            channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
+            //channelAddress = (uint32_t)(&WS2811_TIMER->CCR4);
             break;
     }
 
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index b6f327e61a..91039f51ce 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -32,8 +32,6 @@
 #include "pwm_rx.h"
 #include "pwm_mapping.h"
 
-void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput);
-void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
 
 /*
     Configuration maps
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index ecdd0768a1..54d59c7e55 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -32,3 +32,7 @@ void pwmWriteServo(uint8_t index, uint16_t value);
 
 void pwmDisableMotors(void);
 void pwmEnableMotors(void);
+struct timerHardware_s;
+void pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput);
+void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
+
diff --git a/src/main/drivers/pwm_output_hal.c b/src/main/drivers/pwm_output_hal.c
index dbe14c10f8..87d2a7c2bc 100644
--- a/src/main/drivers/pwm_output_hal.c
+++ b/src/main/drivers/pwm_output_hal.c
@@ -181,8 +181,9 @@ bool isMotorBrushed(uint16_t motorPwmRate)
     return (motorPwmRate > 500);
 }
 
-void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto)
+void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput)
 {
+    UNUSED(enableOutput);
     uint32_t timerMhzCounter;
     pwmWriteFuncPtr pwmWritePtr;
 
@@ -221,8 +222,9 @@ void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
 }
 
 #ifdef USE_SERVOS
-void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
+void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
 {
+    UNUSED(enableOutput);
     servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
 }
 
diff --git a/src/main/drivers/rangefinder.h b/src/main/drivers/rangefinder.h
index b0b6ee7074..a217d75810 100644
--- a/src/main/drivers/rangefinder.h
+++ b/src/main/drivers/rangefinder.h
@@ -17,12 +17,33 @@
 
 #pragma once
 
+#include "common/time.h"
+
+#include "io.h"
+
 #define RANGEFINDER_OUT_OF_RANGE (-1)
 
-typedef struct rangefinder_s {
+struct rangefinderDev_s;
+
+typedef struct rangefinderHardwarePins_s {
+    ioTag_t triggerTag;
+    ioTag_t echoTag;
+} rangefinderHardwarePins_t;
+
+typedef void (*rangefinderOpInitFuncPtr)(void);
+typedef void (*rangefinderOpStartFuncPtr)(void);
+typedef int32_t (*rangefinderOpReadFuncPtr)(void);
+
+typedef struct rangefinderDev_s {
+    timeMs_t delayMs;
     int16_t maxRangeCm;
+
     // these are full detection cone angles, maximum tilt is half of this
     int16_t detectionConeDeciDegrees; // detection cone angle as in device spec
     int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
-} rangefinder_t;
 
+    // function pointers
+    rangefinderOpInitFuncPtr init;
+    rangefinderOpStartFuncPtr update;
+    rangefinderOpReadFuncPtr read;
+} rangefinderDev_t;
diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
index 70cb89c28b..f61a1a18bb 100644
--- a/src/main/drivers/sonar_hcsr04.c
+++ b/src/main/drivers/sonar_hcsr04.c
@@ -48,8 +48,7 @@
  *
  */
 
-STATIC_UNIT_TESTED volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
-sonarHcsr04Hardware_t sonarHcsr04Hardware;
+static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
 
 #ifdef USE_EXTI
 static extiCallbackRec_t hcsr04_extiCallbackRec;
@@ -75,46 +74,8 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb)
 }
 #endif
 
-void hcsr04_set_sonar_hardware(void)
+void hcsr04_init(void)
 {
-#if !defined(UNIT_TEST)
-
-#ifdef STM32F10X
-    // enable AFIO for EXTI support
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
-#endif
-
-#if defined(STM32F3) || defined(STM32F4)
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
-
-    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
-#endif
-
-    // trigger pin
-    triggerIO = IOGetByTag(sonarHcsr04Hardware.triggerTag);
-    IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0);
-    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
-
-    // echo pin
-    echoIO = IOGetByTag(sonarHcsr04Hardware.echoTag);
-    IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0);
-    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
-
-#ifdef USE_EXTI
-    EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
-    EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
-    EXTIEnable(echoIO, true);
-#endif
-
-#endif // UNIT_TEST
-}
-
-void hcsr04_init(rangefinder_t *rangefinder)
-{
-    rangefinder->maxRangeCm = HCSR04_MAX_RANGE_CM;
-    rangefinder->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
-    rangefinder->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
 }
 
 /*
@@ -156,4 +117,48 @@ int32_t hcsr04_get_distance(void)
     }
     return distance;
 }
+
+bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins)
+{
+#ifdef STM32F10X
+    // enable AFIO for EXTI support
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
+#endif
+
+#if defined(STM32F3) || defined(STM32F4)
+    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
+
+    /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
+#endif
+
+    // trigger pin
+    triggerIO = IOGetByTag(sonarHardwarePins->triggerTag);
+    IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0);
+    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
+
+    // echo pin
+    echoIO = IOGetByTag(sonarHardwarePins->echoTag);
+    IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0);
+    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
+
+#ifdef USE_EXTI
+    EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
+    EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
+    EXTIEnable(echoIO, true);
+#endif
+
+    dev->delayMs = 100;
+    dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
+    dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
+    dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+    dev->init = &hcsr04_init;
+    dev->update = &hcsr04_start_reading;
+    dev->read = &hcsr04_get_distance;
+
+    /* FIXME: Do actual hardware detection - see if HC-SR04 is alive */
+    return true;
+}
+
 #endif
diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h
index 452234322f..79178044d7 100644
--- a/src/main/drivers/sonar_hcsr04.h
+++ b/src/main/drivers/sonar_hcsr04.h
@@ -17,13 +17,6 @@
 
 #pragma once
 
-typedef struct sonarHcsr04Hardware_s {
-    ioTag_t triggerTag;
-    ioTag_t echoTag;
-} sonarHcsr04Hardware_t;
+#include "drivers/rangefinder.h"
 
-struct rangefinder_s;
-void hcsr04_set_sonar_hardware(void);
-void hcsr04_init(struct rangefinder_s *rangefinder);
-void hcsr04_start_reading(void);
-int32_t hcsr04_get_distance(void);
+bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonarHardwarePins);
diff --git a/src/main/drivers/sonar_srf10.c b/src/main/drivers/sonar_srf10.c
index a60554bedd..eb584062bd 100644
--- a/src/main/drivers/sonar_srf10.c
+++ b/src/main/drivers/sonar_srf10.c
@@ -114,22 +114,8 @@ static uint8_t i2c_srf10_read_byte(uint8_t i2cRegister)
     return byte;
 }
 
-bool srf10_detect()
+static void srf10_init(void)
 {
-    const uint8_t value = i2c_srf10_read_byte(SRF10_READ_Unused);
-    if (value == SRF10_READ_Unused_ReturnValue) {
-        return true;
-    }
-    return false;
-}
-
-void srf10_init(rangefinder_t *rangefinder)
-{
-    rangefinder->maxRangeCm = SRF10_MAX_RANGE_CM;
-    rangefinder->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
-    rangefinder->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
-    // set up the SRF10 hardware for a range of 6m
-    minimumFiringIntervalMs = SRF10_MinimumFiringIntervalFor600cmRangeMs;
     i2c_srf10_send_byte(SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max);
     i2c_srf10_send_byte(SRF10_WRITE_RangeRegister, SRF10_RangeValue6m);
     // initiate first ranging command
@@ -141,7 +127,7 @@ void srf10_init(rangefinder_t *rangefinder)
  * Start a range reading
  * Called periodically by the scheduler
  */
-void srf10_start_reading(void)
+static void srf10_start_reading(void)
 {
     // check if there is a measurement outstanding, 0xFF is returned if no measurement
     const uint8_t revision = i2c_srf10_read_byte(SRF10_READ_SoftwareRevision);
@@ -154,7 +140,7 @@ void srf10_start_reading(void)
             srf10measurementCm = RANGEFINDER_OUT_OF_RANGE;
         }
     }
-    const uint32_t timeNowMs = millis();
+    const timeMs_t timeNowMs = millis();
     if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) {
         // measurement repeat interval should be greater than minimumFiringIntervalMs
         // to avoid interference between connective measurements.
@@ -166,8 +152,28 @@ void srf10_start_reading(void)
 /**
  * Get the distance that was measured by the last pulse, in centimeters.
  */
-int32_t srf10_get_distance(void)
+static int32_t srf10_get_distance(void)
 {
     return srf10measurementCm;
 }
+
+bool srf10Detect(rangefinderDev_t *dev)
+{
+    const uint8_t value = i2c_srf10_read_byte(SRF10_READ_Unused);
+    if (value == SRF10_READ_Unused_ReturnValue) {
+        dev->delayMs = SRF10_MinimumFiringIntervalFor600cmRangeMs + 10; // set up the SRF10 hardware for a range of 6m + margin of 10ms
+        dev->maxRangeCm = SRF10_MAX_RANGE_CM;
+        dev->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
+        dev->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+        dev->init = &srf10_init;
+        dev->update = &srf10_start_reading;
+        dev->read = &srf10_get_distance;
+
+        return true;
+    }
+    else {
+        return false;
+    }
+}
 #endif
diff --git a/src/main/drivers/sonar_srf10.h b/src/main/drivers/sonar_srf10.h
index e64040b94d..b390b708c2 100644
--- a/src/main/drivers/sonar_srf10.h
+++ b/src/main/drivers/sonar_srf10.h
@@ -17,9 +17,4 @@
 
 #pragma once
 
-struct rangefinder_s;
-bool srf10_detect();
-void srf10_init(struct rangefinder_s *rangefinder);
-void srf10_start_reading(void);
-int32_t srf10_get_distance(void);
-
+bool srf10Detect(rangefinderDev_t *dev);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 8a581f9579..b00b2b3f54 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -102,6 +102,7 @@ extern uint8_t __config_end;
 #include "sensors/diagnostics.h"
 #include "sensors/gyro.h"
 #include "sensors/pitotmeter.h"
+#include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
 
 #include "telemetry/frsky.h"
@@ -131,8 +132,6 @@ static void cliBootlog(char *cmdline);
 static char cliBuffer[64];
 static uint32_t bufferIndex = 0;
 
-static const char* const emptyName = "-";
-
 #ifndef USE_QUAD_MIXER_ONLY
 // sync this with mixerMode_e
 static const char * const mixerNames[] = {
@@ -147,10 +146,10 @@ static const char * const mixerNames[] = {
 
 // sync this with features_e
 static const char * const featureNames[] = {
-    "RX_PPM", "VBAT", "UNUSED_1", "RX_SERIAL", "MOTOR_STOP",
-    "SERVO_TILT", "SOFTSERIAL", "GPS", "UNUSED_3",
-    "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
-    "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "UNUSED_2",
+    "RX_PPM", "VBAT", "", "RX_SERIAL", "MOTOR_STOP",
+    "SERVO_TILT", "SOFTSERIAL", "GPS", "",
+    "", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
+    "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
     "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", "OSD", NULL
 };
@@ -160,6 +159,7 @@ static const char * const featureNames[] = {
 static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"};
 // sync with accelerationSensor_e
 static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"};
+#if (FLASH_SIZE > 64)
 // sync with baroSensor_e
 static const char * const lookupTableBaroHardware[] = { "NONE", "AUTO", "BMP085", "MS5611", "BMP280", "FAKE"};
 // sync with magSensor_e
@@ -169,7 +169,6 @@ static const char * const lookupTableRangefinderHardware[] = { "NONE", "HCSR04",
 // sync with pitotSensor_e
 static const char * const lookupTablePitotHardware[] = { "NONE", "AUTO", "MS4525", "FAKE"};
 
-#if (FLASH_SIZE > 64)
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
     "GYRO", "ACC", "BARO", "MAG", "SONAR", "PITOT", "GPS", "GPS+MAG", NULL
@@ -515,6 +514,11 @@ static const clivalue_t valueTable[] = {
     { "accgain_y",                  VAR_INT16  | MASTER_VALUE, .config.minmax = { 1,  8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) },
     { "accgain_z",                  VAR_INT16  | MASTER_VALUE, .config.minmax = { 1,  8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) },
 
+// PG_RANGEFINDER_CONFIG
+#ifdef SONAR
+    { "rangefinder_hardware",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
+#endif
+
 // PG_COMPASS_CONFIG
 #ifdef MAG
     { "align_mag",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) },
@@ -524,6 +528,7 @@ static const clivalue_t valueTable[] = {
     { "magzero_y",                  VAR_INT16  | MASTER_VALUE, .config.minmax = { INT16_MIN,  INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Y]) },
     { "magzero_z",                  VAR_INT16  | MASTER_VALUE, .config.minmax = { INT16_MIN,  INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Z]) },
     { "mag_hold_rate_limit",        VAR_UINT8  | MASTER_VALUE, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN,  MAG_HOLD_RATE_LIMIT_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hold_rate_limit) },
+    { "mag_calibration_time",       VAR_UINT8  | MASTER_VALUE, .config.minmax = { 30,  120 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magCalibrationTimeLimit) },
 #endif
 
 // PG_BAROMETER_CONFIG
@@ -862,6 +867,10 @@ static const clivalue_t valueTable[] = {
     { "osd_gps_speed_pos",          VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SPEED]) },
     { "osd_gps_sats_pos",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SATS]) },
     { "osd_altitude_pos",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ALTITUDE]) },
+    { "osd_pid_roll_pos",           VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_PIDS]) },
+    { "osd_pid_pitch_pos",          VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_PIDS]) },
+    { "osd_pid_yaw_pos",            VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_YAW_PIDS]) },
+    { "osd_power_pos",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POS_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_POWER]) },
 #endif
 // PG_SYSTEM_CONFIG
     { "i2c_overclock",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, i2c_overclock) },
@@ -2569,6 +2578,8 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig,
     for (uint32_t i = 0; ; i++) { // disable all feature first
         if (featureNames[i] == NULL)
             break;
+        if (featureNames[i][0] == '\0')
+            continue;
         const char *format = "feature -%s\r\n";
         cliDefaultPrintf(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]);
         cliDumpPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]);
@@ -2576,6 +2587,8 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig,
     for (uint32_t i = 0; ; i++) {  // reenable what we want.
         if (featureNames[i] == NULL)
             break;
+        if (featureNames[i][0] == '\0')
+            continue;
         const char *format = "feature %s\r\n";
         if (defaultMask & (1 << i)) {
             cliDefaultPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]);
@@ -2596,6 +2609,8 @@ static void cliFeature(char *cmdline)
         for (uint32_t i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
+            if (featureNames[i][0] == '\0')
+                continue;
             if (mask & (1 << i))
                 cliPrintf("%s ", featureNames[i]);
         }
@@ -2605,6 +2620,8 @@ static void cliFeature(char *cmdline)
         for (uint32_t i = 0; ; i++) {
             if (featureNames[i] == NULL)
                 break;
+            if (featureNames[i][0] == '\0')
+                continue;
             cliPrintf("%s ", featureNames[i]);
         }
         cliPrint("\r\n");
@@ -2632,12 +2649,6 @@ static void cliFeature(char *cmdline)
                     cliPrint("unavailable\r\n");
                     break;
                 }
-#endif
-#ifndef SONAR
-                if (mask & FEATURE_SONAR) {
-                    cliPrint("unavailable\r\n");
-                    break;
-                }
 #endif
                 if (remove) {
                     featureClear(mask);
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index baf9c84120..ad9fa0514c 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -47,6 +47,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/rangefinder.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"
@@ -175,7 +176,7 @@ void validateAndFixConfig(void)
 {
 #if defined(SONAR)
     // FIXME: Completely disable sonar
-    featureClear(FEATURE_SONAR);
+    rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
 #endif
 
 #ifdef USE_GYRO_NOTCH_1
@@ -306,13 +307,13 @@ void validateAndFixConfig(void)
 #endif
 
 #if defined(NAZE) && defined(SONAR)
-    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
+    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
 
 #if defined(OLIMEXINO) && defined(SONAR)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
+    if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
@@ -325,20 +326,20 @@ void validateAndFixConfig(void)
 
 #if defined(CC3D)
 #if defined(CC3D_PPM1)
-#if defined(SONAR) && defined(USE_SOFTSERIAL1)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
-        featureClear(FEATURE_SONAR);
-    }
-#endif
+    #if defined(SONAR) && defined(USE_SOFTSERIAL1)
+        if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
+            rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
+        }
+    #endif
 #else
-#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
-    // shared pin
-    if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
-       featureClear(FEATURE_SONAR);
-       featureClear(FEATURE_SOFTSERIAL);
-       featureClear(FEATURE_RSSI_ADC);
-    }
-#endif
+    #if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
+        // shared pin
+        if (((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
+           rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
+           featureClear(FEATURE_SOFTSERIAL);
+           featureClear(FEATURE_RSSI_ADC);
+        }
+    #endif
 #endif // CC3D_PPM1
 #endif // CC3D
 
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index a880e5cbb8..ba9bbba32f 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -49,8 +49,8 @@ typedef enum {
     FEATURE_SERVO_TILT = 1 << 5,
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,
-    FEATURE_UNUSED_3 = 1 << 8,          // was FAILSAFE
-    FEATURE_SONAR = 1 << 9,
+    FEATURE_UNUSED_3 = 1 << 8,          // was FEATURE_FAILSAFE
+    FEATURE_UNUSED_4 = 1 << 9,          // was FEATURE_SONAR
     FEATURE_TELEMETRY = 1 << 10,
     FEATURE_CURRENT_METER = 1 << 11,
     FEATURE_3D = 1 << 12,
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 0599143708..a7942c5ff7 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -60,7 +60,6 @@
 #include "drivers/bus_spi.h"
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
-#include "drivers/sonar_hcsr04.h"
 #include "drivers/sdcard.h"
 #include "drivers/gyro_sync.h"
 #include "drivers/io.h"
@@ -101,7 +100,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/initialisation.h"
-#include "sensors/sonar.h"
+#include "sensors/rangefinder.h"
 
 #include "telemetry/telemetry.h"
 
@@ -242,14 +241,16 @@ void init(void)
 
 #ifdef USE_SERVOS
     servosInit();
+    mixerUpdateStateFlags();    // This needs to be called early to allow pwm mapper to use information about FIXED_WING state
 #endif
 
     drv_pwm_config_t pwm_params;
     memset(&pwm_params, 0, sizeof(pwm_params));
 
 #ifdef SONAR
-    if (feature(FEATURE_SONAR)) {
-        const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(batteryConfig()->currentMeterType);
+    // HC-SR04 has a dedicated connection to FC and require two pins
+    if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) {
+        const rangefinderHardwarePins_t *sonarHardware = sonarGetHardwarePins();
         if (sonarHardware) {
             pwm_params.useSonar = true;
             pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
@@ -259,10 +260,7 @@ void init(void)
 #endif
 
     // when using airplane/wing mixer, servo/motor outputs are remapped
-    if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
-        pwm_params.airplane = true;
-    else
-        pwm_params.airplane = false;
+    pwm_params.airplane = STATE(FIXED_WING);
 #if defined(USE_UART2) && defined(STM32F10X)
     pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
 #endif
@@ -392,14 +390,14 @@ void init(void)
 
 #if defined(SONAR) && defined(USE_SOFTSERIAL1)
 #if defined(FURYF3) || defined(OMNIBUS) || defined(SPRACINGF3MINI)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
+    if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
     }
 #endif
 #endif
 
 #if defined(SONAR) && defined(USE_SOFTSERIAL2) && defined(SPRACINGF3)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
+    if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
     }
 #endif
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 8856be8846..65e15049e9 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -273,14 +273,12 @@ static void initActiveBoxIds(void)
     if (feature(FEATURE_SERVO_TILT))
         activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
 
-    bool isFixedWing = mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE;
-
 #ifdef GPS
-    if (sensors(SENSOR_BARO) || (isFixedWing && feature(FEATURE_GPS))) {
+    if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
     }
-    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
         activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
@@ -289,7 +287,7 @@ static void initActiveBoxIds(void)
     }
 #endif
 
-    if (isFixedWing) {
+    if (STATE(FIXED_WING)) {
         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
         activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
@@ -300,7 +298,7 @@ static void initActiveBoxIds(void)
      * FLAPERON mode active only in case of airplane and custom airplane. Activating on
      * flying wing can cause bad thing
      */
-    if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
+    if (STATE(FLAPERON_AVAILABLE)) {
         activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
     }
 #endif
@@ -587,7 +585,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
                 sbufWriteU16(dst, acc.accADC[i] / scale);
             }
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, gyro.gyroADC[i]);
+                sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
             }
             for (int i = 0; i < 3; i++) {
                 sbufWriteU16(dst, mag.magADC[i]);
@@ -644,11 +642,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_ALTITUDE:
 #if defined(NAV)
-        sbufWriteU32(dst, (uint32_t)lrintf(getEstimatedActualPosition(Z)));
-        sbufWriteU16(dst, (uint32_t)lrintf(getEstimatedActualVelocity(Z)));
+        sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
+        sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
 #else
         sbufWriteU32(dst, 0);
         sbufWriteU16(dst, 0);
+#endif
+#if defined(BARO)
+        sbufWriteU32(dst, baroGetLatestAltitude());
+#else
+        sbufWriteU32(dst, 0);
 #endif
         break;
 
@@ -1148,7 +1151,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #else
         sbufWriteU8(dst, 0);
 #endif
-        sbufWriteU8(dst, 0);    // rangefinder hardware
+#ifdef SONAR
+        sbufWriteU8(dst, rangefinderConfig()->rangefinder_hardware);
+#else
+        sbufWriteU8(dst, 0);
+#endif
         sbufWriteU8(dst, 0);    // optical flow hardware
         break;
 
@@ -1836,6 +1843,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #ifndef USE_QUAD_MIXER_ONLY
     case MSP_SET_MIXER:
         mixerConfigMutable()->mixerMode = sbufReadU8(src);
+        mixerUpdateStateFlags();    // Required for correct preset functionality
         break;
 #endif
 
@@ -1893,6 +1901,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         sbufReadU8(src); // mixerMode ignored
 #else
         mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
+        mixerUpdateStateFlags();    // Required for correct preset functionality
 #endif
 
         featureClearAll();
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 65543d78d4..7a5cd47116 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -89,7 +89,7 @@ attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclinat
 
 static imuRuntimeConfig_t imuRuntimeConfig;
 
-static float gyroScale;
+static const float gyroScale = (M_PIf / 180.0f);  // gyro output scaled to rad per second
 
 static bool gpsHeadingInitialized = false;
 
@@ -118,7 +118,7 @@ void imuUpdateGyroscope(uint32_t gyroUpdateDeltaUs)
     const float gyroUpdateDelta = gyroUpdateDeltaUs * 1e-6f;
 
     for (int axis = 0; axis < 3; axis++) {
-        imuAccumulatedRate[axis] += gyro.gyroADC[axis] * gyroScale * gyroUpdateDelta;
+        imuAccumulatedRate[axis] += gyro.gyroADCf[axis] * gyroScale * gyroUpdateDelta;
     }
 
     imuAccumulatedRateTime += gyroUpdateDelta;
@@ -164,7 +164,6 @@ void imuConfigure(void)
 void imuInit(void)
 {
     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
-    gyroScale = gyro.dev.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         imuAccelInBodyFrame.A[axis] = 0;
@@ -550,7 +549,7 @@ static void imuUpdateMeasuredRotationRate(void)
     imuAccumulatedRateTime = 0.0f;
 #else
     for (axis = 0; axis < 3; axis++) {
-        imuMeasuredRotationBF.A[axis] = gyro.gyroADC[axis] * gyroScale;
+        imuMeasuredRotationBF.A[axis] = gyro.gyroADCf[axis] * gyroScale;
     }
 #endif
 }
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index b6c5f627e9..84cd754c28 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -304,6 +304,27 @@ bool isMixerEnabled(mixerMode_e mixerMode)
 }
 
 #ifdef USE_SERVOS
+void mixerUpdateStateFlags(void)
+{
+    const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
+
+    // set flag that we're on something with wings
+    if (currentMixerMode == MIXER_FLYING_WING ||
+        currentMixerMode == MIXER_AIRPLANE ||
+        currentMixerMode == MIXER_CUSTOM_AIRPLANE
+    ) {
+        ENABLE_STATE(FIXED_WING);
+    } else {
+        DISABLE_STATE(FIXED_WING);
+    }
+
+    if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
+        ENABLE_STATE(FLAPERON_AVAILABLE);
+    } else {
+        DISABLE_STATE(FLAPERON_AVAILABLE);
+    }
+}
+
 void mixerUsePWMIOConfiguration(void)
 {
     int i;
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 5109047a45..545d5ae048 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -123,6 +123,7 @@ extern bool motorLimitReached;
 void writeAllMotors(int16_t mc);
 void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerUsePWMIOConfiguration(void);
+void mixerUpdateStateFlags(void);
 void mixerResetDisarmedMotors(void);
 void mixTable(void);
 void writeMotors(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 608ebe318a..57bce991e5 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -684,7 +684,7 @@ void pidController(void)
 
     for (int axis = 0; axis < 3; axis++) {
         // Step 1: Calculate gyro rates
-        pidState[axis].gyroRate = gyro.gyroADC[axis] * gyro.dev.scale;
+        pidState[axis].gyroRate = gyro.gyroADCf[axis];
 
         // Step 2: Read target
         float rateTarget;
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index f0400d599d..81ee7562f8 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -199,21 +199,6 @@ int servoDirection(int servoIndex, int inputSource)
 void servosInit(void)
 {
     const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
-    // set flag that we're on something with wings
-    if (currentMixerMode == MIXER_FLYING_WING ||
-        currentMixerMode == MIXER_AIRPLANE ||
-        currentMixerMode == MIXER_CUSTOM_AIRPLANE
-    ) {
-        ENABLE_STATE(FIXED_WING);
-    } else {
-        DISABLE_STATE(FIXED_WING);
-    }
-
-    if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
-        ENABLE_STATE(FLAPERON_AVAILABLE);
-    } else {
-        DISABLE_STATE(FLAPERON_AVAILABLE);
-    }
 
     minServoIndex = servoMixers[currentMixerMode].minServoIndex;
     maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 6cd319fe09..3db6910d9d 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -269,6 +269,11 @@ int32_t baroCalculateAltitude(void)
     return baro.BaroAlt;
 }
 
+int32_t baroGetLatestAltitude(void)
+{
+    return baro.BaroAlt;
+}
+
 bool baroIsHealthy(void)
 {
     return true;
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 1e329857df..59c877c583 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -52,4 +52,5 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 uint32_t baroUpdate(void);
 bool baroIsReady(void);
 int32_t baroCalculateAltitude(void);
+int32_t baroGetLatestAltitude(void);
 bool baroIsHealthy(void);
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index d6b48a01cb..f673fb5222 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -66,7 +66,8 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
     .mag_align = ALIGN_DEFAULT,
     .mag_hardware = MAG_HARDWARE_DEFAULT,
     .mag_declination = 0,
-    .mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT
+    .mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT,
+    .magCalibrationTimeLimit = 30
 );
 
 #ifdef MAG
@@ -309,7 +310,7 @@ void compassUpdate(timeUs_t currentTimeUs)
     }
 
     if (calStartedAt != 0) {
-        if ((currentTimeUs - calStartedAt) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
+        if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
             LED0_TOGGLE;
 
             float diffMag = 0;
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 3c1cbca348..71640f9532 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -57,7 +57,8 @@ typedef struct compassConfig_s {
     sensor_align_e mag_align;               // mag alignment
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
     flightDynamicsTrims_t magZero;
-    uint8_t mag_hold_rate_limit;            //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
+    uint8_t mag_hold_rate_limit;            // Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
+    uint8_t magCalibrationTimeLimit;        // Time for compass calibration (seconds)
 } compassConfig_t;
 
 PG_DECLARE(compassConfig_t, compassConfig);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 853ee1a5b6..7ae66855d9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -69,9 +69,14 @@
 
 gyro_t gyro; // gyro sensor object
 
-STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
+typedef struct gyroCalibration_s {
+    int32_t g[XYZ_AXIS_COUNT];
+    stdev_t var[XYZ_AXIS_COUNT];
+    uint16_t calibratingG;
+} gyroCalibration_t;
 
-static uint16_t calibratingG = 0;
+STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration;
+static int32_t gyroADC[XYZ_AXIS_COUNT];
 
 static filterApplyFnPtr softLpfFilterApplyFn;
 static void *softLpfFilter[XYZ_AXIS_COUNT];
@@ -115,12 +120,8 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
 }
 #endif
 
-STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
+STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
 {
-    dev->mpuIntExtiConfig =  extiConfig;
-
-    gyroSensor_e gyroHardware = GYRO_AUTODETECT;
-
     dev->gyroAlign = ALIGN_DEFAULT;
 
     switch(gyroHardware) {
@@ -196,8 +197,8 @@ STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfi
 #ifdef GYRO_MPU6500_ALIGN
             dev->gyroAlign = GYRO_MPU6500_ALIGN;
 #endif
-                break;
-            }
+            break;
+        }
 #endif
         ; // fallthrough
 
@@ -209,20 +210,18 @@ STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfi
         }
 #endif
         ; // fallthrough
+    default:
     case GYRO_NONE:
         gyroHardware = GYRO_NONE;
     }
 
     addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);
 
-    if (gyroHardware == GYRO_NONE) {
-        return false;
+    if (gyroHardware != GYRO_NONE) {
+        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
+        sensorsSet(SENSOR_GYRO);
     }
-
-    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
-    sensorsSet(SENSOR_GYRO);
-
-    return true;
+    return gyroHardware;
 }
 
 bool gyroInit(void)
@@ -235,8 +234,8 @@ bool gyroInit(void)
 #else
     const extiConfig_t *extiConfig = NULL;
 #endif
-
-    if (!gyroDetect(&gyro.dev, extiConfig)) {
+    gyro.dev.mpuIntExtiConfig =  extiConfig;
+    if (gyroDetect(&gyro.dev, GYRO_AUTODETECT) == GYRO_NONE) {
         return false;
     }
     // After refactoring this function is always called after gyro sampling rate is known, so
@@ -298,83 +297,90 @@ void gyroInitFilters(void)
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
-    calibratingG = calibrationCyclesRequired;
+    gyroCalibration.calibratingG = calibrationCyclesRequired;
+}
+
+STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration)
+{
+    return gyroCalibration->calibratingG == 0;
 }
 
 bool gyroIsCalibrationComplete(void)
 {
-    return calibratingG == 0;
+    return gyroCalibration.calibratingG == 0;
 }
 
-static bool isOnFinalGyroCalibrationCycle(void)
+static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
 {
-    return calibratingG == 1;
+    return gyroCalibration->calibratingG == 1;
 }
 
-static bool isOnFirstGyroCalibrationCycle(void)
+static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
 {
-    return calibratingG == CALIBRATING_GYRO_CYCLES;
+    return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES;
 }
 
-STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
 {
-    static int32_t g[3];
-    static stdev_t var[3];
-
     for (int axis = 0; axis < 3; axis++) {
-
         // Reset g[axis] at start of calibration
-        if (isOnFirstGyroCalibrationCycle()) {
-            g[axis] = 0;
-            devClear(&var[axis]);
+        if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
+            gyroCalibration->g[axis] = 0;
+            devClear(&gyroCalibration->var[axis]);
         }
 
         // Sum up CALIBRATING_GYRO_CYCLES readings
-        g[axis] += gyro.gyroADC[axis];
-        devPush(&var[axis], gyro.gyroADC[axis]);
+        gyroCalibration->g[axis] += dev->gyroADCRaw[axis];
+        devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]);
 
-        // Reset global variables to prevent other code from using un-calibrated data
-        gyro.gyroADC[axis] = 0;
-        gyroZero[axis] = 0;
+        // gyroZero is set to zero until calibration complete
+        dev->gyroZero[axis] = 0;
 
-        if (isOnFinalGyroCalibrationCycle()) {
-            float dev = devStandardDeviation(&var[axis]);
-            // check deviation and startover in case the model was moved
-            if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
+        if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
+            const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
+            // check deviation and start over if the model was moved
+            if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
                 gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
                 return;
             }
-            gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
+            // calibration complete, so set the gyro zero values
+            dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
         }
     }
 
-    if (isOnFinalGyroCalibrationCycle()) {
+    if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
         beeper(BEEPER_GYRO_CALIBRATED);
     }
-    calibratingG--;
-
+    gyroCalibration->calibratingG--;
 }
 
 void gyroUpdate(void)
 {
     // range: +/- 8192; +/- 2000 deg/sec
-    if (!gyro.dev.read(&gyro.dev)) {
+    if (gyro.dev.read(&gyro.dev)) {
+        if (isCalibrationComplete(&gyroCalibration)) {
+            // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
+            gyroADC[X] = (int32_t)gyro.dev.gyroADCRaw[X] - (int32_t)gyro.dev.gyroZero[X];
+            gyroADC[Y] = (int32_t)gyro.dev.gyroADCRaw[Y] - (int32_t)gyro.dev.gyroZero[Y];
+            gyroADC[Z] = (int32_t)gyro.dev.gyroADCRaw[Z] - (int32_t)gyro.dev.gyroZero[Z];
+            alignSensors(gyroADC, gyro.dev.gyroAlign);
+        } else {
+            performGyroCalibration(&gyro.dev, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
+            // Reset gyro values to zero to prevent other code from using uncalibrated data
+            gyro.gyroADCf[X] = 0.0f;
+            gyro.gyroADCf[Y] = 0.0f;
+            gyro.gyroADCf[Z] = 0.0f;
+            // still calibrating, so no need to further process gyro data
+            return;
+        }
+    } else {
+        // no gyro reading to process
         return;
     }
 
-    // Prepare a copy of int32_t gyroADC for mangling to prevent overflow
-    gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
-    gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
-    gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
-
-    if (!gyroIsCalibrationComplete()) {
-        performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
-    }
-
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        gyro.gyroADC[axis] -= gyroZero[axis];
-        float gyroADCf = (float)gyro.gyroADC[axis];
+        float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
@@ -388,8 +394,6 @@ void gyroUpdate(void)
 #ifdef USE_GYRO_NOTCH_2
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
 #endif
-        gyro.gyroADC[axis] = lrintf(gyroADCf);
+        gyro.gyroADCf[axis] = gyroADCf;
     }
-
-    alignSensors(gyro.gyroADC, gyro.dev.gyroAlign);
 }
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 5da48c275b..c5ef13e2a6 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -36,7 +36,7 @@ typedef enum {
 typedef struct gyro_s {
     gyroDev_t dev;
     uint32_t targetLooptime;
-    int32_t gyroADC[XYZ_AXIS_COUNT];
+    float gyroADCf[XYZ_AXIS_COUNT];
 } gyro_t;
 
 extern gyro_t gyro;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 515053e41b..4f896e7640 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -67,8 +67,7 @@ bool sensorsAutodetect(void)
 #endif
 
 #ifdef SONAR
-    const rangefinderType_e rangefinderType = rangefinderDetect();
-    rangefinderInit(rangefinderType);
+    rangefinderInit();
 #endif
 
     if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index 3578397a82..2628b5b018 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -22,14 +22,14 @@
 
 #include <platform.h>
 
-#ifdef SONAR
-
 #include "build/build_config.h"
 
 #include "common/maths.h"
 #include "common/utils.h"
 
 #include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
 
 #include "drivers/io.h"
 #include "drivers/logging.h"
@@ -44,139 +44,96 @@
 #include "sensors/rangefinder.h"
 #include "sensors/battery.h"
 
-// Sonar measurements are in cm, a value of RANGEFINDER_OUT_OF_RANGE indicates sonar is not in range.
-// Inclination is adjusted by imu
+rangefinder_t rangefinder;
 
-extern sonarHcsr04Hardware_t sonarHcsr04Hardware;
+#ifdef SONAR
+PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
 
-static int16_t rangefinderMaxRangeCm;
-static int16_t rangefinderMaxAltWithTiltCm;
-static int16_t rangefinderCfAltCm; // Complimentary Filter altitude
-STATIC_UNIT_TESTED int16_t rangefinderMaxTiltDeciDegrees;
-static rangefinderFunctionPointers_t rangefinderFunctionPointers;
-static float rangefinderMaxTiltCos;
+PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
+    .rangefinder_hardware = RANGEFINDER_NONE,
+);
 
-static int32_t calculatedAltitude;
+const rangefinderHardwarePins_t * sonarGetHardwarePins(void)
+{
+    static rangefinderHardwarePins_t rangefinderHardwarePins;
+
+#if defined(SONAR_PWM_TRIGGER_PIN)
+    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
+    if (feature(FEATURE_SOFTSERIAL)
+            || feature(FEATURE_RX_PARALLEL_PWM )
+            || (feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC)) {
+        rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
+        rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
+    } else {
+        rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
+        rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
+    }
+#elif defined(SONAR_TRIGGER_PIN)
+    rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
+    rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
+#else
+#error Sonar not defined for target
+#endif
+    return &rangefinderHardwarePins;
+}
 
 /*
  * Detect which rangefinder is present
  */
-rangefinderType_e rangefinderDetect(void)
+static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
 {
-    rangefinderType_e rangefinderType = RANGEFINDER_NONE;
-    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
-        rangefinderType = RANGEFINDER_HCSR04;
-    }
+    rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
+    requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
+
+    switch (rangefinderHardwareToUse) {
+        case RANGEFINDER_HCSR04:
+            {
+                const rangefinderHardwarePins_t *sonarHardwarePins = sonarGetHardwarePins();
+                if (hcsr04Detect(dev, sonarHardwarePins)) {   // FIXME: Do actual detection if HC-SR04 is plugged in
+                    rangefinderHardware = RANGEFINDER_HCSR04;
+                }
+            }
+            break;
+
+        case RANGEFINDER_SRF10:
 #ifdef USE_SONAR_SRF10
-    if (srf10_detect()) {
-        // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04
-        rangefinderType = RANGEFINDER_SRF10;
-    }
+            if (srf10Detect(dev)) {
+                rangefinderHardware = RANGEFINDER_SRF10;
+            }
 #endif
+            break;
 
-    addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0);
-
-    requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType;   // FIXME: Make rangefinder type selectable from CLI
-    detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType;
-
-    if (rangefinderType != RANGEFINDER_NONE) {
-        sensorsSet(SENSOR_SONAR);
+        case RANGEFINDER_NONE:
+            rangefinderHardware = RANGEFINDER_NONE;
+            break;
     }
 
-    return rangefinderType;
+    addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderHardware, 0, 0, 0);
+
+    if (rangefinderHardware == RANGEFINDER_NONE) {
+        sensorsClear(SENSOR_SONAR);
+        return false;
+    }
+
+    detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
+    sensorsSet(SENSOR_SONAR);
+    return true;
 }
 
-static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(currentSensor_e currentSensor)
+bool rangefinderInit(void)
 {
-#if defined(SONAR_PWM_TRIGGER_PIN)
-#endif
-#if !defined(UNIT_TEST)
-#endif
-#if defined(UNIT_TEST)
-   UNUSED(currentSensor);
-   return 0;
-#elif defined(SONAR_PWM_TRIGGER_PIN)
-    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
-    if (feature(FEATURE_SOFTSERIAL)
-            || feature(FEATURE_RX_PARALLEL_PWM )
-            || (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) {
-        sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
-        sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
-    } else {
-        sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
-        sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN);
+    if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
+        return false;
     }
-#elif defined(SONAR_TRIGGER_PIN)
-    UNUSED(currentSensor);
-    sonarHcsr04Hardware.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
-    sonarHcsr04Hardware.echoTag = IO_TAG(SONAR_ECHO_PIN);
-#else
-#error Sonar not defined for target
-#endif
-    return &sonarHcsr04Hardware;
+
+    rangefinder.dev.init();
+    rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+    rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
+    rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
+
+    return true;
 }
 
-STATIC_UNIT_TESTED void rangefinderSetFunctionPointers(rangefinderType_e rangefinderType)
-{
-
-    switch (rangefinderType) {
-    default:
-    case RANGEFINDER_NONE:
-        break;
-    case RANGEFINDER_HCSR04:
-        rangefinderFunctionPointers.init = hcsr04_init;
-        rangefinderFunctionPointers.update = hcsr04_start_reading;
-        rangefinderFunctionPointers.read = hcsr04_get_distance;
-        break;
-#ifdef USE_SONAR_SRF10
-    case RANGEFINDER_SRF10:
-        rangefinderFunctionPointers.init = srf10_init;
-        rangefinderFunctionPointers.update = srf10_start_reading;
-        rangefinderFunctionPointers.read = srf10_get_distance;
-        break;
-#endif
-    }
-}
-
-/*
- * Get the HCSR04 sonar hardware configuration.
- * NOTE: sonarInit() must be subsequently called before using any of the sonar functions.
- */
-const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor)
-{
-    // Return the configuration for the HC-SR04 hardware.
-    // Unfortunately the I2C bus is not initialised at this point
-    // so cannot detect if another sonar device is present
-    return sonarGetHardwareConfigurationForHCSR04(currentSensor);
-}
-
-void rangefinderInit(rangefinderType_e rangefinderType)
-{
-    calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
-
-    if (rangefinderType == RANGEFINDER_HCSR04) {
-        hcsr04_set_sonar_hardware();
-    }
-#ifndef UNIT_TEST
-    rangefinderSetFunctionPointers(rangefinderType);
-#endif
-
-    rangefinder_t rangefinder;
-    if (rangefinderType == RANGEFINDER_NONE) {
-        memset(&rangefinder, 0, sizeof(rangefinder));
-    } else {
-        rangefinderFunctionPointers.init(&rangefinder);
-    }
-    rangefinderMaxRangeCm = rangefinder.maxRangeCm;
-    rangefinderCfAltCm = rangefinderMaxRangeCm / 2;
-    rangefinderMaxTiltDeciDegrees =  rangefinder.detectionConeExtendedDeciDegrees / 2;
-    rangefinderMaxTiltCos = cos_approx(rangefinderMaxTiltDeciDegrees / 10.0f * RAD);
-    rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinderMaxTiltCos;
-}
-
-
 static int32_t applyMedianFilter(int32_t newReading)
 {
     #define DISTANCE_SAMPLES_MEDIAN 5
@@ -200,8 +157,8 @@ static int32_t applyMedianFilter(int32_t newReading)
  */
 void rangefinderUpdate(void)
 {
-    if (rangefinderFunctionPointers.update) {
-        rangefinderFunctionPointers.update();
+    if (rangefinder.dev.update) {
+        rangefinder.dev.update();
     }
 }
 
@@ -210,11 +167,20 @@ void rangefinderUpdate(void)
  */
 int32_t rangefinderRead(void)
 {
-    if (rangefinderFunctionPointers.read) {
-        const int32_t distance = rangefinderFunctionPointers.read();
-        return applyMedianFilter(distance);
+    if (rangefinder.dev.read) {
+        const int32_t distance = rangefinder.dev.read();
+        if (distance >= 0) {
+            rangefinder.rawAltitude = applyMedianFilter(distance);
+        }
+        else {
+            rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+        }
     }
-    return 0;
+    else {
+        rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
+    }
+
+    return rangefinder.rawAltitude;
 }
 
 /**
@@ -226,12 +192,12 @@ int32_t rangefinderRead(void)
 int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle)
 {
     // calculate sonar altitude only if the ground is in the sonar cone
-    if (cosTiltAngle < rangefinderMaxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
-        calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
+    if (cosTiltAngle < rangefinder.maxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) {
+        rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
     } else {
-        calculatedAltitude = rangefinderDistance * cosTiltAngle;
+        rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle;
     }
-    return calculatedAltitude;
+    return rangefinder.calculatedAltitude;
 }
 
 /**
@@ -240,7 +206,7 @@ int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltA
  */
 int32_t rangefinderGetLatestAltitude(void)
 {
-    return calculatedAltitude;
+    return rangefinder.calculatedAltitude;
 }
 
 bool rangefinderIsHealthy(void)
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index 19939518e6..ec39d28592 100644
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@@ -18,6 +18,8 @@
 #pragma once
 
 #include <stdint.h>
+#include "config/parameter_group.h"
+#include "drivers/rangefinder.h"
 
 typedef enum {
     RANGEFINDER_NONE    = 0,
@@ -25,22 +27,30 @@ typedef enum {
     RANGEFINDER_SRF10   = 2,
 } rangefinderType_e;
 
-struct rangefinder_s;
-typedef void (*rangefinderInitFunctionPtr)(struct rangefinder_s *rangefinderRange);
-typedef void (*rangefinderUpdateFunctionPtr)(void);
-typedef int32_t (*rangefinderReadFunctionPtr)(void);
+typedef struct rangefinderConfig_s {
+    uint8_t rangefinder_hardware;
+} rangefinderConfig_t;
+
+PG_DECLARE(rangefinderConfig_t, rangefinderConfig);
+
+typedef struct rangefinder_s {
+    rangefinderDev_t dev;
+    float maxTiltCos;
+    int32_t rawAltitude;
+    int32_t calculatedAltitude;
+} rangefinder_t;
+
+extern rangefinder_t rangefinder;
+
+const rangefinderHardwarePins_t * sonarGetHardwarePins(void);
+
+bool rangefinderInit(void);
+
 
-typedef struct rangefinderFunctionPointers_s {
-    rangefinderInitFunctionPtr init;
-    rangefinderUpdateFunctionPtr update;
-    rangefinderReadFunctionPtr read;
-} rangefinderFunctionPointers_t;
 
-rangefinderType_e rangefinderDetect(void);
 int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle);
 int32_t rangefinderGetLatestAltitude(void);
-rangefinderType_e rangefinderDetect(void);
-void rangefinderInit(rangefinderType_e rangefinderType);
+
 void rangefinderUpdate(void);
 int32_t rangefinderRead(void);
 bool rangefinderIsHealthy(void);
diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h
deleted file mode 100644
index 68138b5804..0000000000
--- a/src/main/sensors/sonar.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#pragma once
-
-#include "drivers/sonar_hcsr04.h"
-#include "drivers/rangefinder.h"
-#include "sensors/battery.h"
-
-typedef enum {
-    SONAR_NONE = 0,
-    SONAR_HCSR04,
-    SONAR_SRF10
-} sonarHardwareType_e;
-
-typedef enum {
-    SONAR_HCSR04_PINS_RC,
-    SONAR_HCSR04_PINS_PWM,
-} sonarHCSR04Pins_e;
-
-struct sonarRange_s;
-typedef void (*sonarInitFunctionPtr)(struct sonarRange_s *sonarRange);
-typedef void (*sonarUpdateFunctionPtr)(void);
-typedef int32_t (*sonarReadFunctionPtr)(void);
-
-typedef struct sonarFunctionPointers_s {
-    sonarInitFunctionPtr init;
-    sonarUpdateFunctionPtr update;
-    sonarReadFunctionPtr read;
-} sonarFunctionPointers_t;
-
-const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor);
-int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
-int32_t sonarGetLatestAltitude(void);
-void sonarInit(void);
-void sonarUpdate(void);
-int32_t sonarRead(void);
-
diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c
index 00ae132b9a..eef481973d 100644
--- a/src/main/target/BLUEJAYF4/hardware_revision.c
+++ b/src/main/target/BLUEJAYF4/hardware_revision.c
@@ -29,6 +29,7 @@
 #include "drivers/flash_m25p16.h"
 #include "hardware_revision.h"
 
+#if 0
 static const char * const hardwareRevisionNames[] = {
     "Unknown",
     "BlueJay rev1",
@@ -36,6 +37,7 @@ static const char * const hardwareRevisionNames[] = {
     "BlueJay rev3",
     "BlueJay rev3a"
 };
+#endif
 
 uint8_t hardwareRevision = UNKNOWN;
 
diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index cc57481ecc..03d9460a73 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -27,6 +27,7 @@ extern "C" {
     #include "build/build_config.h"
     #include "build/debug.h"
     #include "common/axis.h"
+    #include "common/maths.h"
     #include "common/utils.h"
     #include "drivers/accgyro_fake.h"
     #include "drivers/logging_codes.h"
@@ -36,9 +37,16 @@ extern "C" {
     #include "sensors/acceleration.h"
     #include "sensors/sensors.h"
 
-    STATIC_UNIT_TESTED bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig);
-    STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold);
-    extern int32_t gyroZero[XYZ_AXIS_COUNT];
+    typedef struct gyroCalibration_s {
+        int32_t g[XYZ_AXIS_COUNT];
+        stdev_t var[XYZ_AXIS_COUNT];
+        uint16_t calibratingG;
+    } gyroCalibration_t;
+    extern gyroCalibration_t gyroCalibration;
+
+    STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
+    STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold);
+    STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration);
 }
 
 #include "unittest_macros.h"
@@ -46,14 +54,15 @@ extern "C" {
 
 TEST(SensorGyro, Detect)
 {
-    const bool detected = gyroDetect(&gyro.dev, NULL);
-    EXPECT_EQ(true, detected);
+    const gyroSensor_e detected = gyroDetect(&gyro.dev, GYRO_AUTODETECT);
+    EXPECT_EQ(GYRO_FAKE, detected);
     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
 }
 
 TEST(SensorGyro, Init)
 {
-    gyroInit();
+    const bool initialised = gyroInit();
+    EXPECT_EQ(true, initialised);
     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
 }
 
@@ -75,30 +84,54 @@ TEST(SensorGyro, Calibrate)
     fakeGyroSet(5, 6, 7);
     const bool read = gyro.dev.read(&gyro.dev);
     EXPECT_EQ(true, read);
-    gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
-    gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
-    gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
-    EXPECT_EQ(5, gyro.gyroADC[X]);
-    EXPECT_EQ(6, gyro.gyroADC[Y]);
-    EXPECT_EQ(7, gyro.gyroADC[Z]);
+    EXPECT_EQ(5, gyro.dev.gyroADCRaw[X]);
+    EXPECT_EQ(6, gyro.dev.gyroADCRaw[Y]);
+    EXPECT_EQ(7, gyro.dev.gyroADCRaw[Z]);
     static const int gyroMovementCalibrationThreshold = 32;
-    gyroZero[X] = 8;
-    gyroZero[Y] = 9;
-    gyroZero[Z] = 10;
-    performGyroCalibration(gyroMovementCalibrationThreshold);
-    EXPECT_EQ(0, gyroZero[X]);
-    EXPECT_EQ(0, gyroZero[Y]);
-    EXPECT_EQ(0, gyroZero[Z]);
-    EXPECT_EQ(false, gyroIsCalibrationComplete());
-    while (!gyroIsCalibrationComplete()) {
-        performGyroCalibration(gyroMovementCalibrationThreshold);
-        gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
-        gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
-        gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
+    gyro.dev.gyroZero[X] = 8;
+    gyro.dev.gyroZero[Y] = 9;
+    gyro.dev.gyroZero[Z] = 10;
+    performGyroCalibration(&gyro.dev, &gyroCalibration, gyroMovementCalibrationThreshold);
+    EXPECT_EQ(0, gyro.dev.gyroZero[X]);
+    EXPECT_EQ(0, gyro.dev.gyroZero[Y]);
+    EXPECT_EQ(0, gyro.dev.gyroZero[Z]);
+    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    while (!isCalibrationComplete(&gyroCalibration)) {
+        performGyroCalibration(&gyro.dev, &gyroCalibration, gyroMovementCalibrationThreshold);
     }
-    EXPECT_EQ(5, gyroZero[X]);
-    EXPECT_EQ(6, gyroZero[Y]);
-    EXPECT_EQ(7, gyroZero[Z]);
+    EXPECT_EQ(5, gyro.dev.gyroZero[X]);
+    EXPECT_EQ(6, gyro.dev.gyroZero[Y]);
+    EXPECT_EQ(7, gyro.dev.gyroZero[Z]);
+}
+
+TEST(SensorGyro, Update)
+{
+    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    gyroInit();
+    fakeGyroSet(5, 6, 7);
+    gyroUpdate();
+    EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
+    while (!isCalibrationComplete(&gyroCalibration)) {
+        gyroUpdate();
+    }
+    EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration));
+    EXPECT_EQ(5, gyro.dev.gyroZero[X]);
+    EXPECT_EQ(6, gyro.dev.gyroZero[Y]);
+    EXPECT_EQ(7, gyro.dev.gyroZero[Z]);
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
+    gyroUpdate();
+    // expect zero values since gyro is calibrated
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
+    EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
+    fakeGyroSet(15, 26, 97);
+    gyroUpdate();
+    EXPECT_FLOAT_EQ(10 * gyro.dev.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
+    EXPECT_FLOAT_EQ(20 * gyro.dev.scale, gyro.gyroADCf[Y]);
+    EXPECT_FLOAT_EQ(90 * gyro.dev.scale, gyro.gyroADCf[Z]);
 }