Merge branch 'master' into avs-automatic-servo-trim

# Conflicts:
#	src/main/build/debug.h
#	src/main/fc/settings.yaml

docs/Settings.md

@@ -103,7 +103,6 @@
 | failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF |  |  | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
-| flip_over_after_crash_power_factor | 65 | 0 | 100 | flip over after crash power factor |
 | fpv_mix_degrees | 0 | 0 | 50 |  |
 | frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
@@ -148,6 +147,9 @@
 | gps_provider | UBLOX |  |  | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 | gps_sbas_mode | NONE |  |  | Which SBAS mode to be used |
 | gps_ublox_use_galileo | OFF |  |  | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter |
 | gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
 | gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF |
 | gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF |
@@ -289,7 +291,7 @@
 | nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) |
 | nav_fw_land_dive_angle | 2 | -20 | 20 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
 | nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
-| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
+| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
 | nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] |
 | nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
 | nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
@@ -424,6 +426,7 @@
 | osd_right_sidebar_scroll_step | 0 |  | 255 | Same as left_sidebar_scroll_step, but for the right sidebar |
 | osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) |
 | osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink |
+| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) |
 | osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
 | osd_sidebar_scroll_arrows | OFF |  |  |  |
 | osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) |
@@ -522,6 +525,7 @@
 | tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. |
 | tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. |
 | tri_unarmed_servo | ON |  |  | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor |
 | tz_automatic_dst | OFF |  |  | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. |
 | tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
 | vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |

src/main/build/debug.h

@@ -83,7 +83,8 @@ typedef enum {
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
-    DEBUG_SMITH_COMPENSATOR,
+    DEBUG_GYRO_ALPHA_BETA_GAMMA,
+    DEBUG_SMITH_PREDICTOR,
     DEBUG_AUTOTRIM,
     DEBUG_COUNT
 } debugType_e;

src/main/common/filter.c

@@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y1 = y1;
     filter->y2 = y2;
 }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
+    // beta, gamma, and eta gains all derived from
+    // http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
+
+    const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
+    filter->xk = 0.0f;
+    filter->vk = 0.0f;
+    filter->ak = 0.0f;
+    filter->jk = 0.0f;
+    filter->a = alpha;
+    filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
+    filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
+    filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
+	filter->dT = dT;
+	filter->dT2 = dT * dT;
+    filter->dT3 = dT * dT * dT;
+    pt1FilterInit(&filter->boostFilter, 100, dT);
+
+    const float boost = boostGain * 100;
+
+    filter->boost = (boost * boost / 10000) * 0.003;
+    filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
+}
+
+FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
+    //xk - current system state (ie: position)
+	//vk - derivative of system state (ie: velocity)
+    //ak - derivative of system velociy (ie: acceleration)
+    //jk - derivative of system acceleration (ie: jerk)
+    float rk;   // residual error
+
+    // give the filter limited history
+    filter->xk *= filter->halfLife;
+    filter->vk *= filter->halfLife;
+    filter->ak *= filter->halfLife;
+    filter->jk *= filter->halfLife;
+
+    // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
+    filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
+    
+    // update (estimated) velocity (also estimated dterm from measurement)
+    filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
+    filter->ak += filter->dT * filter->jk;
+    
+    // what is our residual error (measured - estimated)
+    rk = input - filter->xk;
+
+    // artificially boost the error to increase the response of the filter
+    rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
+    if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
+        rk = (input - filter->xk) / filter->a;
+    }
+    filter->rk = rk; // for logging
+
+    // update our estimates given the residual error.
+    filter->xk += filter->a * rk;
+    filter->vk += filter->b / filter->dT * rk;
+    filter->ak += filter->g / (2.0f * filter->dT2) * rk;
+    filter->jk += filter->e / (6.0f * filter->dT3) * rk;
+
+	return filter->xk;
+}
+
+#endif

src/main/common/filter.h

@@ -56,6 +56,18 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef struct alphaBetaGammaFilter_s {
+    float a, b, g, e;
+    float ak; // derivative of system velociy (ie: acceleration)
+    float vk; // derivative of system state (ie: velocity)
+    float xk; // current system state (ie: position)
+    float jk; // derivative of system acceleration (ie: jerk)
+    float rk; // residual error
+    float dT, dT2, dT3;
+    float halfLife, boost;
+    pt1Filter_t boostFilter;
+} alphaBetaGammaFilter_t;
+
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
@@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
+float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);

src/main/drivers/serial_softserial.c

@@ -169,7 +169,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 
 static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud)
 {
-    uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim);
+    uint32_t baseClock = timerClock(tch->timHw->tim);
     uint32_t clock = baseClock;
     uint32_t timerPeriod;
 

src/main/drivers/serial_usb_vcp.c

@@ -216,6 +216,12 @@ void usbVcpInitHardware(void)
 
     /* Start Device Process */
     USBD_Start(&USBD_Device);
+    
+#ifdef STM32H7
+    HAL_PWREx_EnableUSBVoltageDetector();
+    delay(100); // Cold boot failures observed without this, even when USB cable is not connected
+#endif
+
 #else
     Set_System();
     Set_USBClock();

src/main/drivers/system_stm32h7xx.c

@@ -51,10 +51,13 @@ bool isMPUSoftReset(void)
         return false;
 }
 
-#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800)
 uint32_t systemBootloaderAddress(void)
 {
-    return SYSMEMBOOT_VECTOR_TABLE[1];
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+    return 0x1ff09800;
+#else
+#error Unknown MCU
+#endif
 }
 
 void systemClockSetup(uint8_t cpuUnderclock)

src/main/drivers/timer.c

@@ -240,7 +240,7 @@ uint32_t timerGetBaseClock(TCH_t * tch)
 
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw)
 {
-    return SystemCoreClock / timerClockDivisor(timHw->tim);
+    return timerClock(timHw->tim);
 }
 
 bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)

src/main/drivers/timer.h

@@ -157,7 +157,7 @@ typedef enum {
     TYPE_TIMER
 } channelType_t;
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim);
+uint32_t timerClock(TIM_TypeDef *tim);
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw);
 
 const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag);

src/main/drivers/timer_stm32f30x.c

@@ -48,10 +48,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [16] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM1_TRG_COM_TIM17_IRQn },
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);

src/main/drivers/timer_stm32f4xx.c

@@ -95,16 +95,16 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
 #endif
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
 #if defined (STM32F411xE)
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 #elif defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F446xx)
     if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
-        return 1;
+        return SystemCoreClock;
     } else {
-        return 2;
+        return SystemCoreClock / 2;
     }
 #else
     #error "No timer clock defined correctly for the MCU"

src/main/drivers/timer_stm32f7xx.c

@@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM14, .rcc = RCC_APB1(TIM14), .irq = TIM8_TRG_COM_TIM14_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);

src/main/drivers/timer_stm32h7xx.c

@@ -48,16 +48,48 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM17, .rcc = RCC_APB2(TIM17),  .irq = TIM17_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
-    UNUSED(tim);
+    int timpre;
-    return SystemCoreClock;
+    uint32_t pclk;
+    uint32_t ppre;
+
+    // Implement the table:
+    // RM0433 (Rev 6) Table 52.
+    // RM0455 (Rev 3) Table 55.
+    // "Ratio between clock timer and pclk"
+    // (Tables are the same, just D2 or CD difference)
+
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+#define PERIPH_PRESCALER(bus) ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE ## bus) >> RCC_D2CFGR_D2PPRE ## bus ## _Pos)
+#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
+#define PERIPH_PRESCALER(bus) ((RCC->CDCFGR2 & RCC_CDCFGR2_CDPPRE ## bus) >> RCC_CDCFGR2_CDPPRE ## bus ## _Pos)
+#else
+#error Unknown MCU type
+#endif
+
+    if (tim == TIM1 || tim == TIM8 || tim == TIM15 || tim == TIM16 || tim == TIM17) {
+        // Timers on APB2
+        pclk = HAL_RCC_GetPCLK2Freq();
+        ppre = PERIPH_PRESCALER(2);
+    } else {
+        // Timers on APB1
+        pclk = HAL_RCC_GetPCLK1Freq();
+        ppre = PERIPH_PRESCALER(1);
+    }
+
+    timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0;
+
+    int index = (timpre << 3) | ppre;
+
+    static uint8_t periphToKernel[16] = { // The mutiplier table
+        1, 1, 1, 1, 2, 2, 2, 2, // TIMPRE = 0
+        1, 1, 1, 1, 2, 4, 4, 4  // TIMPRE = 1
+    };
+
+    return pclk * periphToKernel[index];
+
+#undef PERIPH_PRESCALER
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
@@ -66,8 +98,6 @@ _TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
 _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
 _TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
 _TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
-//_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9);
-//_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11);
 _TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12);
 _TIM_IRQ_HANDLER(TIM15_IRQHandler, 15);
 _TIM_IRQ_HANDLER(TIM16_IRQHandler, 16);

src/main/fc/fc_core.c

@@ -420,9 +420,9 @@ void disarm(disarmReason_t disarmReason)
         }
 #endif
 #ifdef USE_DSHOT
-        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+        if (FLIGHT_MODE(TURTLE_MODE)) {
             sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
-            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+            DISABLE_FLIGHT_MODE(TURTLE_MODE);
         }
 #endif
         statsOnDisarm();
@@ -494,15 +494,31 @@ void tryArm(void)
 {
     updateArmingStatus();
 
+#ifdef USE_DSHOT
+    if (
+            STATE(MULTIROTOR) &&
+            IS_RC_MODE_ACTIVE(BOXTURTLE) &&
+            emergencyArmingCanOverrideArmingDisabled() &&
+            isMotorProtocolDshot() &&
+            !ARMING_FLAG(ARMED) &&
+            !FLIGHT_MODE(TURTLE_MODE)
+            ) {
+        sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+        ENABLE_ARMING_FLAG(ARMED);
+        enableFlightMode(TURTLE_MODE);
+        return;
+    }
+#endif
+
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
-        !isArmingDisabled() || 
+        !isArmingDisabled() ||
-        emergencyArmingIsEnabled() || 
+        emergencyArmingIsEnabled() ||
         LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)
     ) {
-#else 
+#else
     if (
-        !isArmingDisabled() || 
+        !isArmingDisabled() ||
         emergencyArmingIsEnabled()
     ) {
 #endif
@@ -510,21 +526,6 @@ void tryArm(void)
             return;
         }
 
-#ifdef USE_DSHOT
-        if (
-                STATE(MULTIROTOR) &&
-                IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
-                emergencyArmingCanOverrideArmingDisabled() &&
-                isMotorProtocolDshot() &&
-                !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
-                ) {
-            sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
-            ENABLE_ARMING_FLAG(ARMED);
-            enableFlightMode(FLIP_OVER_AFTER_CRASH);
-            return;
-        }
-#endif
-
 #if defined(USE_NAV)
         // If nav_extra_arming_safety was bypassed we always
         // allow bypassing it even without the sticks set

src/main/fc/fc_msp_box.c

@@ -89,7 +89,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
-    { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
+    { BOXTURTLE, "TURTLE", 52 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -316,7 +316,7 @@ void initActiveBoxIds(void)
 
 #ifdef USE_DSHOT
     if(STATE(MULTIROTOR) && isMotorProtocolDshot())
-        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+        activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
 #endif
 }
 

src/main/fc/rc_modes.h

@@ -69,7 +69,7 @@ typedef enum {
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
-    BOXFLIPOVERAFTERCRASH = 43,
+    BOXTURTLE        = 43,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/runtime_config.h

@@ -98,7 +98,7 @@ typedef enum {
     NAV_COURSE_HOLD_MODE  = (1 << 12),
     FLAPERON              = (1 << 13),
     TURN_ASSISTANT        = (1 << 14),
-    FLIP_OVER_AFTER_CRASH = (1 << 15),
+    TURTLE_MODE           = (1 << 15),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;

src/main/fc/settings.yaml

@@ -94,7 +94,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
       "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
-      "SMITH_COMPENSATOR", "AUTOTRIM"]
+      "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -111,7 +111,7 @@ tables:
     enum: navRTHAllowLanding_e
   - name: bat_capacity_unit
     values: ["MAH", "MWH"]
-    enum: batCapacityUnit_e    
+    enum: batCapacityUnit_e
   - name: bat_voltage_source
     values: ["RAW", "SAG_COMP"]
     enum: batVoltageSource_e
@@ -296,6 +296,27 @@ groups:
         min: 0
         max: 1
         default_value: 0
+      - name: gyro_abg_alpha
+        description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0
+        field: alphaBetaGammaAlpha
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 1
+      - name: gyro_abg_boost
+        description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.35
+        field: alphaBetaGammaBoost
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 2
+      - name: gyro_abg_half_life
+        description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.5
+        field: alphaBetaGammaHalfLife
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 10
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
@@ -851,10 +872,10 @@ groups:
         min: 4
         max: 255
         default_value: 14
-      - name: flip_over_after_crash_power_factor
+      - name: turtle_mode_power_factor
-        field: flipOverAfterPowerFactor
+        field: turtleModePowerFactor
-        default_value: 65
+        default_value: 55
-        description: "flip over after crash power factor"
+        description: "Turtle mode power factor"
         condition: "USE_DSHOT"
         min: 0
         max: 100
@@ -2670,7 +2691,7 @@ groups:
         min: 0
         max: 60000
       - name: nav_fw_launch_climb_angle
-        description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
+        description: "Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
         default_value: 18
         field: fw.launch_climb_angle
         min: 5
@@ -3288,6 +3309,13 @@ groups:
         default_value: 0
         description: Same as left_sidebar_scroll_step, but for the right sidebar
 
+      - name: osd_sidebar_height
+        field: sidebar_height
+        min: 0
+        max: 5
+        default_value: 3
+        description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
+
       - name: osd_home_position_arm_screen
         type: bool
         default_value: ON

src/main/flight/mixer.c

@@ -116,13 +116,12 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
 #ifdef USE_DSHOT
-    .flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT,
+    .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
 #endif
 );
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
-#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
 #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
 
 typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
@@ -328,10 +327,10 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
-static void applyFlipOverAfterCrashModeToMotors(void) {
+static void applyTurtleModeToMotors(void) {
 
     if (ARMING_FLAG(ARMED)) {
-        const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
+        const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f;
         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@@ -394,13 +393,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) {
 
             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 
-            float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
+            motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle);
-
-            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
-            motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
-                    motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
-
-            motor[i] = motorOutput;
         }
     } else {
         // Disarmed mode
@@ -532,8 +525,8 @@ static int getReversibleMotorsThrottleDeadband(void)
 void FAST_CODE mixTable(const float dT)
 {
 #ifdef USE_DSHOT
-    if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+    if (FLIGHT_MODE(TURTLE_MODE)) {
-        applyFlipOverAfterCrashModeToMotors();
+        applyTurtleModeToMotors();
         return;
     }
 #endif

src/main/flight/mixer.h

@@ -87,7 +87,7 @@ typedef struct motorConfig_s {
     float throttleIdle;                     // Throttle IDLE value based on min_command, max_throttle, in percent
     float throttleScale;                    // Scaling factor for throttle.
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
-    uint8_t flipOverAfterPowerFactor;       // Power factor from 0 to 100% of flip over after crash
+    uint8_t turtleModePowerFactor;          // Power factor from 0 to 100% of flip over after crash
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

src/main/flight/pid.c

@@ -1103,9 +1103,9 @@ void FAST_CODE pidController(float dT)
 #endif
 
 #ifdef USE_SMITH_PREDICTOR
-        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate);
         pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
-        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate);
 #endif
 
         DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);

src/main/io/osd.c

@@ -193,7 +193,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 1);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)
@@ -1770,6 +1770,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "!FS!";
             else if (FLIGHT_MODE(MANUAL_MODE))
                 p = "MANU";
+            else if (FLIGHT_MODE(TURTLE_MODE))
+                p = "TURT";
             else if (FLIGHT_MODE(NAV_RTH_MODE))
                 p = isWaypointMissionRTHActive() ? "WRTH" : "RTH ";
             else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
@@ -1790,8 +1792,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
-                p = "TURT";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;
@@ -1963,9 +1963,10 @@ static bool osdDrawSingleElement(uint8_t item)
                         wp2.lon = posControl.waypointList[j].lon;
                         wp2.alt = posControl.waypointList[j].alt;
                         fpVector3_t poi;
-                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3));
+                        int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude();
                         while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
-                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i);
                     }
                 }
             }
@@ -2811,6 +2812,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT,
     .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
     .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
+    .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT,
     .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
     .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
     .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,

src/main/io/osd.h

@@ -369,7 +369,8 @@ typedef struct osdConfig_s {
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
     int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
     uint8_t crsf_lq_format;
-    uint8_t telemetry; 				        // use telemetry on displayed pixel line 0
+    uint8_t sidebar_height;             // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
+    uint8_t telemetry; 				    // use telemetry on displayed pixel line 0
 
 } osdConfig_t;
 

src/main/io/osd_grid.c

@@ -295,7 +295,7 @@ void osdGridDrawSidebars(displayPort_t *display)
     uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
 
     const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
-    const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS;
+    const int hudheight = osdConfig()->sidebar_height;
 
     // Arrows
     if (osdConfig()->sidebar_scroll_arrows) {
@@ -315,11 +315,12 @@ void osdGridDrawSidebars(displayPort_t *display)
     // Draw AH sides
     int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
     int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
-    for (int y = -hudheight; y <= hudheight; y++) {
+    if (osdConfig()->sidebar_height) {
-        displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
+        for (int y = -hudheight; y <= hudheight; y++) {
-        displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+            displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
+            displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+        }
     }
-
     // AH level indicators
     displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
     displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);

src/main/navigation/navigation.c

@@ -236,7 +236,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 static navigationFSMEvent_t nextForNonGeoStates(void);
 static bool isWaypointMissionValid(void);
 
@@ -1502,7 +1502,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             if (STATE(MULTIROTOR)) {
                 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
                 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
-                                           &posControl.waypointList[posControl.activeWaypointIndex]);
+                                           &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
             }
             return nextForNonGeoStates();
 
@@ -1875,6 +1875,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
             break;
 
         case RTH_HOME_FINAL_LAND:
+            // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
+            if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
+                posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100;   // 100 -> m to cm
+                if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
+                    posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt;  // correct to relative if absolute SL altitude datum used
+                }
+            }
             break;
     }
 
@@ -2890,7 +2897,7 @@ void resetSafeHomes(void)
 }
 #endif
 
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
 {
     gpsLocation_t wpLLH;
 
@@ -2898,7 +2905,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
     wpLLH.lon = waypoint->lon;
     wpLLH.alt = waypoint->alt;
 
-    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
+    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
 }
 
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
@@ -2912,10 +2919,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
     setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 }
 
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
+{
+    return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
+}
+
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
 {
     fpVector3_t localPos;
-    mapWaypointToLocalPosition(&localPos, waypoint);
+    mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
     calculateAndSetActiveWaypointToLocalPosition(&localPos);
 }
 
@@ -3321,7 +3333,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
     // Don't allow arming if first waypoint is farther than configured safe distance
     if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
         fpVector3_t startingWaypointPos;
-        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
+        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
 
         const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
 

src/main/navigation/navigation.h

@@ -481,6 +481,11 @@ typedef enum {
     GEO_ORIGIN_RESET_ALTITUDE
 } geoOriginResetMode_e;
 
+typedef enum {
+    NAV_WP_TAKEOFF_DATUM,
+    NAV_WP_MSL_DATUM
+} geoAltitudeDatumFlag_e;
+
 // geoSetOrigin stores the location provided in llh as a GPS origin in the
 // provided origin parameter. resetMode indicates wether all origin coordinates
 // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
@@ -501,6 +506,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh
 // the provided origin is valid and the conversion could be performed.
 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
+// Select absolute or relative altitude based on WP mission flag setting
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 
 /* Distance/bearing calculation */
 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);

src/main/sensors/gyro.c

@@ -104,7 +104,14 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+
+STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
+STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
+
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@@ -130,6 +137,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    .alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
+    .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
+    .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
+#endif
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -284,6 +296,24 @@ static void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
         }
     }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    
+    abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
+
+    if (gyroConfig()->alphaBetaGammaAlpha > 0) {
+        abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
+            alphaBetaGammaFilterInit(
+                &abgFilter[axis], 
+                gyroConfig()->alphaBetaGammaAlpha, 
+                gyroConfig()->alphaBetaGammaBoost, 
+                gyroConfig()->alphaBetaGammaHalfLife, 
+                getLooptime() * 1e-6f
+            );
+        }
+    } 
+#endif
 }
 
 bool gyroInit(void)
@@ -458,6 +488,12 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
 
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
+        gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
+#endif
+
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);

src/main/sensors/gyro.h

@@ -82,6 +82,11 @@ typedef struct gyroConfig_s {
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    float alphaBetaGammaAlpha;
+    float alphaBetaGammaBoost;
+    float alphaBetaGammaHalfLife;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/target/MATEKH743/target.c

@@ -33,8 +33,29 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500,   DEVHW_MPU6500,  IMU2_SPI_BUS,   IMU2
 //BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6000,   DEVHW_MPU6000,  IMU2_SPI_BUS,   IMU2_CS_PIN,    IMU2_EXTI_PIN,  1,  DEVFLAGS_NONE,  IMU2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM2,  CH1, PA0,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S1
-    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S2
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S2
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S3  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S4
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S9
+    DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
+
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S12
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 0),    // LED_2812
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
+
+    DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PPM, 0, 0),   // RX6 PPM
+    DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_ANY, 0, 0),   // TX6    
+    DEF_TIM(TIM16, CH1, PB8,  TIM_USE_ANY, 0, 0),   // RX4
+    DEF_TIM(TIM17, CH1, PB9,  TIM_USE_ANY, 0, 0),   // TX4
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/common.h

@@ -84,6 +84,7 @@
 #define USE_PITOT_ADC
 #define USE_PITOT_VIRTUAL
 
+#define USE_ALPHA_BETA_GAMMA_FILTER
 #define USE_DYNAMIC_FILTERS
 #define USE_GYRO_KALMAN
 #define USE_SMITH_PREDICTOR