Merge remote-tracking branch 'origin/development' into dzikuvx-d-boost

docs/Cli.md

@@ -329,6 +329,9 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  osd_dist_alarm       | 1000  | Value above which to make the OSD distance from home indicator blink (meters) |
 |  osd_alt_alarm        | 100   | Value above which to make the OSD relative altitude indicator blink (meters) |
 |  osd_neg_alt_alarm    | 5    | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
+|  osd_gforce_alarm     | 5    | Value above which the OSD g force indicator will blink (g) |
+|  osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) |
+|  osd_gforce_axis_alarm_max | 5  | Value above which the OSD axis g force indicators will blink (g) |
 |  osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) |
@@ -409,8 +412,8 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  gyro_lpf_type  | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
 |  acc_lpf_hz  | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 |  acc_lpf_type  | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
-|  dterm_lpf_hz  | 40 |  |
+|  dterm_lpf_hz  | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
-|  yaw_lpf_hz  | 30 |  |
+|  yaw_lpf_hz  | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 |  gyro_stage2_lowpass_hz  | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
 |  pidsum_limit  | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
 |  yaw_p_limit  | 300 |  |
@@ -471,3 +474,6 @@ A shorter form is also supported to enable and disable functions using `serial <
 | nav_mc_pos_expo | 10 | Expo for PosHold control |
 | osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon |
 | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default  should allow the noisiest baro to complete calibration [cm]. | 
+| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. | 
+| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used | 
+| use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** | 

make/source.mk

@@ -93,6 +93,7 @@ COMMON_SRC = \
             io/beeper.c \
             io/lights.c \
             io/pwmdriver_i2c.c \
+            io/esc_serialshot.c \
             io/piniobox.c \
             io/serial.c \
             io/serial_4way.c \

src/main/blackbox/blackbox.c

@@ -360,9 +360,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"hwHealthStatus",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"powerSupplyImpedance",  -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"sagCompensatedVBat",    -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
-    {"wind",                   0, SIGNED,   PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"wind",                   0, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
-    {"wind",                   1, SIGNED,   PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"wind",                   1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
-    {"wind",                   2, SIGNED,   PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"wind",                   2, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
     {"IMUTemperature",        -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
 #ifdef USE_BARO
     {"baroTemperature",       -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},

src/main/build/debug.h

@@ -61,7 +61,6 @@ typedef enum {
     DEBUG_FPORT,
     DEBUG_ALWAYS,
     DEBUG_STAGE2,
-    DEBUG_WIND_ESTIMATOR,
     DEBUG_SAG_COMP_VOLTAGE,
     DEBUG_VIBE,
     DEBUG_CRUISE,

src/main/cms/cms_menu_osd.c

@@ -244,6 +244,11 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("WIND HOR", OSD_WIND_SPEED_HORIZONTAL),
     OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL),
 
+    OSD_ELEMENT_ENTRY("G-FORCE", OSD_GFORCE),
+    OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE),
+    OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE),
+    OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE),
+
     OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE),
 #ifdef USE_BARO
     OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE),

src/main/common/maths.h

@@ -55,6 +55,10 @@
 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
 #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
 
+#define CENTIMETERS_TO_CENTIFEET(cm)            (cm * (328 / 100.0))
+#define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
+#define CENTIMETERS_TO_METERS(cm)               (cm / 100)
+
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
     ( __extension__ ({                                      \

src/main/drivers/osd_symbols.h

@@ -63,7 +63,8 @@
 
 //                                0x21 // 033 ASCII !
 
-#define SYM_TRIP_DIST             0x22 // 034 Icon total distance
+#define SYM_TRIP_DIST             0x22 // 034 Trip distance
+#define SYM_TOTAL                 0x22 // 034 Total
 
 //                                0x23 // 035 ASCII #
 

src/main/drivers/pwm_mapping.c

@@ -205,15 +205,8 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
                 continue;
             }
 
-            if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->pwmProtocolType, init->enablePWMOutput)) {
+            if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, motorConfig()->motorPwmRate, init->enablePWMOutput)) {
-                if (init->useFastPwm) {
+                pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR;
-                    pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM;
-                } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) {
-                    pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM;
-                } else {
-                    pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ;
-                }
-
                 pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.motorCount;
                 pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr;
 
@@ -232,7 +225,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
             }
 
             if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) {
-                pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM;
+                pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO;
                 pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount;
                 pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr;
 

src/main/drivers/pwm_mapping.h

@@ -57,7 +57,6 @@ typedef struct drv_pwm_config_s {
     bool useUART3;
     bool useUART6;
     bool useVbat;
-    bool useFastPwm;
     bool useSoftSerial;
     bool useLEDStrip;
 #ifdef USE_RANGEFINDER
@@ -66,8 +65,6 @@ typedef struct drv_pwm_config_s {
     bool useServoOutputs;
     uint16_t servoPwmRate;
     uint16_t servoCenterPulse;
-    uint8_t pwmProtocolType;
-    uint16_t motorPwmRate;
     rangefinderIOConfig_t rangefinderIOConfig;
 } drv_pwm_config_t;
 
@@ -75,9 +72,6 @@ typedef enum {
     PWM_PF_NONE = 0,
     PWM_PF_MOTOR = (1 << 0),
     PWM_PF_SERVO = (1 << 1),
-    PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
-    PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
-    PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4),
     PWM_PF_PPM = (1 << 5),
     PWM_PF_PWM = (1 << 6)
 } pwmPortFlags_e;

src/main/drivers/pwm_output.c

@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+#include "build/debug.h"
+
 #include "common/log.h"
 #include "common/maths.h"
 
@@ -32,6 +34,7 @@
 #include "drivers/io_pca9685.h"
 
 #include "io/pwmdriver_i2c.h"
+#include "io/esc_serialshot.h"
 
 #include "config/feature.h"
 
@@ -59,9 +62,8 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function poi
 
 typedef struct {
     TCH_t * tch;
-    pwmWriteFuncPtr pwmWritePtr;
     bool configured;
-    uint16_t value;                 // Used to keep track of last motor value
+    uint16_t value;
 
     // PWM parameters
     volatile timCCR_t *ccr;         // Shortcut for timer CCR register
@@ -74,16 +76,23 @@ typedef struct {
 #endif
 } pwmOutputPort_t;
 
+typedef struct {
+    pwmOutputPort_t *   pwmPort;        // May be NULL if motor doesn't use the PWM port
+    uint16_t            value;          // Used to keep track of last motor value
+} pwmOutputMotor_t;
+
 static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
 
-static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
+static pwmOutputMotor_t motors[MAX_PWM_MOTORS];
 static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
 
-#ifdef USE_DSHOT
+static motorPwmProtocolTypes_e initMotorProtocol;
+static pwmWriteFuncPtr         motorWritePtr = NULL;    // Function to write value to motors
 
-static bool isProtocolDshot = false;
+
-static timeUs_t dshotMotorUpdateIntervalUs = 0;
+#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
-static timeUs_t dshotMotorLastUpdateUs;
+static timeUs_t digitalMotorUpdateIntervalUs = 0;
+static timeUs_t digitalMotorLastUpdateUs;
 #endif
 
 #ifdef BEEPER_PWM
@@ -110,20 +119,34 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
     *p->ccr = 0;
 }
 
-static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
+static pwmOutputPort_t *pwmOutAllocatePort(void)
 {
     if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
         LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
         return NULL;
     }
 
+    pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
+
+    p->tch = NULL;
+    p->configured = false;
+
+    return p;
+}
+
+static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
+{
     // Attempt to allocate TCH
     TCH_t * tch = timerGetTCH(timHw);
     if (tch == NULL) {
         return NULL;
     }
 
-    pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
+    // Allocate motor output port
+    pwmOutputPort_t *p = pwmOutAllocatePort();
+    if (p == NULL) {
+        return NULL;
+    }
 
     const IO_t io = IOGetByTag(timHw->tag);
     IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount);
@@ -143,13 +166,15 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t
 
 static void pwmWriteStandard(uint8_t index, uint16_t value)
 {
-    *motors[index]->ccr = lrintf((value * motors[index]->pulseScale) + motors[index]->pulseOffset);
+    if (motors[index].pwmPort) {
+        *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset);
+    }
 }
 
 void pwmWriteMotor(uint8_t index, uint16_t value)
 {
-    if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) {
+    if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) {
-        motors[index]->pwmWritePtr(index, value);
+        motorWritePtr(index, value);
     }
 }
 
@@ -157,7 +182,9 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 {
     for (int index = 0; index < motorCount; index++) {
         // Set the compare register to 0, which stops the output pulsing if the timer overflows
-        *motors[index]->ccr = 0;
+        if (motors[index].pwmPort) {
+            *(motors[index].pwmPort->ccr) = 0;
+        }
     }
 }
 
@@ -221,7 +248,7 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
 
     // Keep track of motor update interval
     const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz;
-    dshotMotorUpdateIntervalUs = MAX(dshotMotorUpdateIntervalUs, motorIntervalUs);
+    digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs);
 
     // Configure timer DMA
     if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
@@ -233,12 +260,6 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
     return port;
 }
 
-static void pwmWriteDshot(uint8_t index, uint16_t value)
-{
-    // DMA operation might still be running. Cache value for future use
-    motors[index]->value = value;
-}
-
 static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
 {
     for (int i = 0; i < 16; i++) {
@@ -265,71 +286,125 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
 
     return packet;
 }
+#endif
 
-void pwmCompleteDshotUpdate(uint8_t motorCount)
+#ifdef USE_SERIALSHOT
+static void motorConfigSerialShot(uint16_t motorPwmRateHz)
 {
-    // Get latest REAL time
+    // Keep track of motor update interval
-    timeUs_t currentTimeUs = micros();
+    const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz;
+    digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs);
 
-    // Enforce motor update rate
+    // Kick off SerialShot driver initalization
-    if (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) {
+    serialshotInitialize();
-        return;
+}
-    }
+#endif
 
-    dshotMotorLastUpdateUs = currentTimeUs;
+#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
-
+static void pwmWriteDigital(uint8_t index, uint16_t value)
-    // Generate DMA buffers
+{
-    for (int index = 0; index < motorCount; index++) {
+    // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate()
-        if (motors[index] && motors[index]->configured) {
+    // DSHOT and some other digital protocols use 11-bit throttle range [0;2047]
-            // TODO: ESC telemetry
+    motors[index].value = constrain(value, 0, 2047);
-            uint16_t packet = prepareDshotPacket(motors[index]->value, false);
-
-            loadDmaBufferDshot(motors[index]->dmaBuffer, packet);
-            timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE);
-        }
-    }
-
-    // Start DMA on all timers
-    for (int index = 0; index < motorCount; index++) {
-        if (motors[index] && motors[index]->configured) {
-            timerPWMStartDMA(motors[index]->tch);
-        }
-    }
 }
 
 bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
 {
-    return isProtocolDshot;
+    // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config
+    // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used
+    return (initMotorProtocol == PWM_TYPE_DSHOT150) ||
+           (initMotorProtocol == PWM_TYPE_DSHOT300) ||
+           (initMotorProtocol == PWM_TYPE_DSHOT600) ||
+           (initMotorProtocol == PWM_TYPE_DSHOT1200);
+}
+
+bool FAST_CODE NOINLINE isMotorProtocolSerialShot(void)
+{
+    return (initMotorProtocol == PWM_TYPE_SERIALSHOT);
+}
+
+bool FAST_CODE NOINLINE isMotorProtocolDigital(void)
+{
+    return isMotorProtocolDshot() || isMotorProtocolSerialShot();
+}
+
+void pwmCompleteMotorUpdate(void)
+{
+    // Get motor count from mixer
+    int motorCount = getMotorCount();
+
+    // Get latest REAL time
+    timeUs_t currentTimeUs = micros();
+
+    // Enforce motor update rate
+    if (!isMotorProtocolDigital() || (digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
+        return;
+    }
+
+    digitalMotorLastUpdateUs = currentTimeUs;
+
+#ifdef USE_DSHOT
+    if (isMotorProtocolDshot()) {
+        // Generate DMA buffers
+        for (int index = 0; index < motorCount; index++) {
+            if (motors[index].pwmPort && motors[index].pwmPort->configured) {
+                // TODO: ESC telemetry
+                uint16_t packet = prepareDshotPacket(motors[index].value, false);
+
+                loadDmaBufferDshot(motors[index].pwmPort->dmaBuffer, packet);
+                timerPWMPrepareDMA(motors[index].pwmPort->tch, DSHOT_DMA_BUFFER_SIZE);
+            }
+        }
+
+        // Start DMA on all timers
+        for (int index = 0; index < motorCount; index++) {
+            if (motors[index].pwmPort && motors[index].pwmPort->configured) {
+                timerPWMStartDMA(motors[index].pwmPort->tch);
+            }
+        }
+    }
+#endif
+
+#ifdef USE_SERIALSHOT
+    if (isMotorProtocolSerialShot()) {
+        for (int index = 0; index < motorCount; index++) {
+            serialshotUpdateMotor(index, motors[index].value);
+        }
+
+        serialshotSendUpdate();
+    }
+#endif
 }
 #endif
 
-bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, motorPwmProtocolTypes_e proto, bool enableOutput)
+bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, bool enableOutput)
 {
-    pwmOutputPort_t * port = NULL;
+    // Keep track of initial motor protocol
-    pwmWriteFuncPtr pwmWritePtr;
+    initMotorProtocol = motorConfig()->motorPwmProtocol;
 
 #ifdef BRUSHED_MOTORS
-    proto = PWM_TYPE_BRUSHED;   // Override proto
+    initMotorProtocol = PWM_TYPE_BRUSHED;   // Override proto
 #endif
 
-    switch (proto) {
+    switch (initMotorProtocol) {
     case PWM_TYPE_BRUSHED:
-        port = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput);
-        pwmWritePtr = pwmWriteStandard;
+        motorWritePtr = pwmWriteStandard;
         break;
+
     case PWM_TYPE_ONESHOT125:
-        port = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput);
-        pwmWritePtr = pwmWriteStandard;
+        motorWritePtr = pwmWriteStandard;
         break;
 
     case PWM_TYPE_ONESHOT42:
-        port = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput);
-        pwmWritePtr = pwmWriteStandard;
+        motorWritePtr = pwmWriteStandard;
         break;
 
     case PWM_TYPE_MULTISHOT:
-        port = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput);
-        pwmWritePtr = pwmWriteStandard;
+        motorWritePtr = pwmWriteStandard;
         break;
 
 #ifdef USE_DSHOT
@@ -337,28 +412,33 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
     case PWM_TYPE_DSHOT600:
     case PWM_TYPE_DSHOT300:
     case PWM_TYPE_DSHOT150:
-        port = motorConfigDshot(timerHardware, proto, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, initMotorProtocol, motorPwmRateHz, enableOutput);
-        if (port) {
+        motorWritePtr = pwmWriteDigital;
-            isProtocolDshot = true;
+        break;
-            pwmWritePtr = pwmWriteDshot;
+#endif
-        }
+
+#ifdef USE_SERIALSHOT
+    case PWM_TYPE_SERIALSHOT:
+        // This is hacky. Our motor output flow is:   init() -> pwmInit() -> pwmMotorConfig() 
+        // This is decoupled from mixer, so if the board doesn't define any PWM motor output the pwmMotorConfig() won't get called
+        // We rely on the fact that all FCs define hardware PWM motor outputs. To make this bullet-proof we need to change the 
+        // init sequence to originate from the mixer and allocate timers only if necessary
+        motorConfigSerialShot(motorPwmRateHz);
+        // Make sure pwmMotorConfig fails and doesn't mark timer as occupied by a motor
+        motors[motorIndex].pwmPort = NULL;
+        // Serialshot uses the same throttle interpretation as DSHOT, so we use the same write function here
+        motorWritePtr = pwmWriteDigital;
         break;
 #endif
 
     default:
     case PWM_TYPE_STANDARD:
-        port = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput);
+        motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput);
-        pwmWritePtr = pwmWriteStandard;
+        motorWritePtr = pwmWriteStandard;
         break;
     }
 
-    if (port) {
+    return (motors[motorIndex].pwmPort != NULL);
-        port->pwmWritePtr = pwmWritePtr;
-        motors[motorIndex] = port;
-        return true;
-    }
-
-    return false;
 }
 
 bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)

src/main/drivers/pwm_output.h

@@ -30,19 +30,20 @@ typedef enum {
     PWM_TYPE_DSHOT300,
     PWM_TYPE_DSHOT600,
     PWM_TYPE_DSHOT1200,
+    PWM_TYPE_SERIALSHOT,
 } motorPwmProtocolTypes_e;
 
 void pwmWriteMotor(uint8_t index, uint16_t value);
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
-void pwmCompleteDshotUpdate(uint8_t motorCount);
+void pwmCompleteMotorUpdate(void);
-bool isMotorProtocolDshot(void);
+bool isMotorProtocolDigital(void);
 
 void pwmWriteServo(uint8_t index, uint16_t value);
 
 void pwmDisableMotors(void);
 void pwmEnableMotors(void);
 struct timerHardware_s;
-bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, motorPwmProtocolTypes_e proto, bool enableOutput);
+bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, bool enableOutput);
 bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
 void pwmWriteBeeper(bool onoffBeep);
 void beeperPwmInit(ioTag_t tag, uint16_t frequency);

src/main/drivers/timer_def_stm32f4xx.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#define timerDMASafeType_t  uint16_t
+#define timerDMASafeType_t  uint32_t
 
 #define DEF_TIM_DMAMAP__D(dma, stream, channel)         DMA_TAG(dma, stream, channel)
 #define DEF_TIM_DMAMAP__NONE                            DMA_NONE

src/main/fc/config.c

@@ -297,6 +297,11 @@ void validateAndFixConfig(void)
     case PWM_TYPE_DSHOT1200:
         motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 32000);
         break;
+#endif
+#ifdef USE_SERIALSHOT
+    case PWM_TYPE_SERIALSHOT:   // 2-4 kHz
+        motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 4000);
+        break;
 #endif
     }
 #endif

src/main/fc/fc_core.c

@@ -659,6 +659,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
         flightTime += cycleTime;
+        updateAccExtremes();
     }
 
     taskGyro(currentTimeUs);
@@ -753,7 +754,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 #endif
 
 #ifdef USE_DSHOT
-    pwmCompleteDshotUpdate(getMotorCount());
+    pwmCompleteMotorUpdate();
 #endif
 }
 

src/main/fc/fc_init.c

@@ -317,16 +317,7 @@ void init(void)
     pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
     pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
 
-    pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol;
-#ifndef BRUSHED_MOTORS
-    pwm_params.useFastPwm = (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT125) ||
-                            (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT42) ||
-                            (motorConfig()->motorPwmProtocol == PWM_TYPE_MULTISHOT);
-#endif
-    pwm_params.motorPwmRate = motorConfig()->motorPwmRate;
-
     if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
-        pwm_params.useFastPwm = false;
         featureClear(FEATURE_3D);
     }
 
@@ -351,9 +342,6 @@ void init(void)
 
     mixerPrepare();
 
-    if (!pwm_params.useFastPwm)
-        motorControlEnable = true;
-
     addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE);
     systemState |= SYSTEM_STATE_MOTORS_READY;
 

src/main/fc/fc_tasks.c

@@ -153,8 +153,6 @@ void taskUpdateCompass(timeUs_t currentTimeUs)
 #ifdef USE_BARO
 void taskUpdateBaro(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
-
     if (!sensors(SENSOR_BARO)) {
         return;
     }

src/main/fc/settings.yaml

@@ -32,7 +32,7 @@ tables:
   - name: blackbox_device
     values: ["SERIAL", "SPIFLASH", "SDCARD"]
   - name: motor_pwm_protocol
-    values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"]
+    values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"]
   - name: failsafe_procedure
     values: ["SET-THR", "DROP", "RTH", "NONE"]
   - name: current_sensor
@@ -54,7 +54,7 @@ tables:
   - name: nav_user_control_mode
     values: ["ATTI", "CRUISE"]
   - name: nav_rth_alt_mode
-    values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST"]
+    values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"]
   - name: osd_unit
     values: ["IMPERIAL", "METRIC", "UK"]
     enum: osd_unit_e
@@ -76,7 +76,7 @@ tables:
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
     values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
-      "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE",
+      "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", "D_BOOST"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
@@ -1656,6 +1656,18 @@ groups:
         field: neg_alt_alarm
         min: 0
         max: 10000
+      - name: osd_gforce_alarm
+        field: gforce_alarm
+        min: -20
+        max: 20
+      - name: osd_gforce_axis_alarm_min
+        field: gforce_axis_alarm_min
+        min: -20
+        max: 20
+      - name: osd_gforce_axis_alarm_max
+        field: gforce_axis_alarm_max
+        min: -20
+        max: 20
       - name: osd_imu_temp_alarm_min
         field: imu_temp_alarm_min
         min: -550

src/main/flight/imu.c

@@ -580,6 +580,7 @@ void imuCheckVibrationLevels(void)
     DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
     DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
     DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
+    // DEBUG_VIBE values 4-7 are used by NAV estimator
 }
 
 void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)

src/main/flight/mixer.c

@@ -175,7 +175,7 @@ void FAST_CODE NOINLINE writeMotors(void)
 
 #ifdef USE_DSHOT
         // If we use DSHOT we need to convert motorValue to DSHOT ranges
-        if (isMotorProtocolDshot()) {
+        if (isMotorProtocolDigital()) {
             const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue;
 
             if (feature(FEATURE_3D)) {

src/main/flight/rth_estimator.c

@@ -20,6 +20,18 @@
 
 #if defined(USE_ADC) && defined(USE_GPS)
 
+/* INPUTS:
+ *   - heading degrees
+ *   - horizontalWindSpeed
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns same unit as horizontalWindSpeed
+ */
+static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
+    return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
+}
+
+#ifdef USE_WIND_ESTIMATOR
 /* INPUTS:
  *   - forwardSpeed (same unit as horizontalWindSpeed)
  *   - heading degrees
@@ -44,17 +56,6 @@ static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, f
     return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading)));
 }
 
-/* INPUTS:
- *   - heading degrees
- *   - horizontalWindSpeed
- *   - windHeading degrees
- * OUTPUT:
- *   returns same unit as horizontalWindSpeed
- */
-static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
-    return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
-}
-
 /* INPUTS:
  *   - forwardSpeed (same unit as horizontalWindSpeed)
  *   - heading degrees
@@ -66,17 +67,17 @@ static float forwardWindSpeed(float heading, float horizontalWindSpeed, float wi
 static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
     return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
 }
+#endif
 
 // returns degrees
-static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) {
+static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
     return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
 }
 
-// altitudeChange is in meters
+// pitch in degrees
-// idle_power and cruise_power are in deciWatt
+// output in Watt
-// output is in Watt
+static float estimatePitchPower(float pitch) {
-static float estimateRTHAltitudeChangePower(float altitudeChange) {
+    int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
-    uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle;
     altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
     const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
     return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
@@ -88,7 +89,7 @@ static float estimateRTHAltitudeChangePower(float altitudeChange) {
 // output is in seconds
 static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) {
     // Assuming increase in throttle keeps air speed at cruise speed
-    const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
+    const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
     return altitudeChange / estimatedVerticalSpeed;
 }
 
@@ -100,17 +101,22 @@ static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalW
 // output is in meters
 static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) {
     // Assuming increase in throttle keeps air speed at cruise speed
-    float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
+    const float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
     return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed;
 }
 
 // altitudeChange is in m
 // verticalWindSpeed is in m/s
 // output is in Wh
-static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
+static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
-    return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
+    const float RTHInitialAltitudeChangePower = estimatePitchPower(altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle);
+    return RTHInitialAltitudeChangePower * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
 }
 
+// horizontalWindSpeed is in m/s
+// windHeading is in degrees
+// verticalWindSpeed is in m/s
+// altitudeChange is in m
 // returns distance in m
 // *heading is in degrees
 static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
@@ -128,8 +134,17 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan
     }
 }
 
-// returns mWh
+// distanceToHome is in meters
-static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
+// output in Watt
+static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
+    const float timeToHome = distanceToHome / speedToHome; // seconds
+    const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - RTHAltitude()) : 0);
+    const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
+    return estimatePitchPower(pitchToHome) * timeToHome / 3600;
+}
+
+// returns Wh
+static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
     // Fixed wing only for now
     if (!STATE(FIXED_WING))
         return -1;
@@ -141,35 +156,40 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
        ))
         return -1;
 
+    const float RTH_initial_altitude_change = MAX(0, (RTHAltitude() - getEstimatedActualPosition(Z)) / 100);
+
+    float RTH_heading; // degrees
 #ifdef USE_WIND_ESTIMATOR
     uint16_t windHeading; // centidegrees
     const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
     const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
     const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
+
+    const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
+    const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees);
 #else
     UNUSED(takeWindIntoAccount);
-    const float horizontalWindSpeed = 0; // m/s
+    const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading);
-    const float windHeadingDegrees = 0;
+    const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100;
-    const float verticalWindSpeed = 0;
 #endif
 
-    const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100;
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100));
-    float RTH_heading; // degrees
-    const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
-    const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees);
-
-    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100));
     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
+#ifdef USE_WIND_ESTIMATOR
     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
+#endif
 
     if (RTH_speed <= 0)
-        return -2; // wind is too strong
+        return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost)
 
-    const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds
+#ifdef USE_WIND_ESTIMATOR
-    const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh
+    const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
-    const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh
+#else
-    const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh
+    const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
+#endif
+    const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh
+    const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh
 
     if (remaining_energy_before_rth < 0) // No energy left = No time left
         return 0;
@@ -178,31 +198,28 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
 }
 
 // returns seconds
-int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
+float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
 
-    const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
+    const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
 
     // error: return error code directly
     if (remainingEnergyBeforeRTH < 0)
         return remainingEnergyBeforeRTH;
 
-    const int32_t averagePower = calculateAveragePower();
+    const float averagePower = (float)calculateAveragePower() / 100;
 
     if (averagePower == 0)
         return -1;
 
-    const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower;
+    const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower;
-
-    if (time_before_rth > 0x7FFFFFFF) // int32 overflow
-        return -1;
 
     return time_before_rth;
 }
 
 // returns meters
-int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
+float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 
-    const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
+    const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
 
     // error: return error code directly
     if (remainingFlightTimeBeforeRTH < 0)

src/main/flight/rth_estimator.h

@@ -1,5 +1,5 @@
 
 #if defined(USE_ADC) && defined(USE_GPS)
-int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
+float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
-int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
+float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
 #endif

src/main/flight/wind_estimator.c

@@ -163,10 +163,6 @@ void updateWindEstimator(timeUs_t currentTimeUs)
             estimatedWind[X] = estimatedWind[X] * 0.95f + wind[X] * 0.05f;
             estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f;
             estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f;
-
-            DEBUG_SET(DEBUG_WIND_ESTIMATOR, 0, estimatedWind[X]);
-            DEBUG_SET(DEBUG_WIND_ESTIMATOR, 1, estimatedWind[Y]);
-            DEBUG_SET(DEBUG_WIND_ESTIMATOR, 2, estimatedWind[Z]);
         }
         lastUpdateUs = currentTimeUs;
         hasValidWindEstimate = true;

src/main/io/esc_serialshot.c

@@ -0,0 +1,116 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/crc.h"
+
+#include "io/serial.h"
+#include "io/esc_serialshot.h"
+
+#if defined(USE_SERIALSHOT)
+
+#define SERIALSHOT_UART_BAUD            921600
+#define SERIALSHOT_PKT_DEFAULT_HEADER   (0x00)  // Default header (motors 1-4, regular 4-in-1 ESC)
+
+
+typedef struct __attribute__((packed)) {
+    uint8_t hdr;            // Header/version marker
+    uint8_t motorData[6];   // 12 bit per motor
+    uint8_t crc;            // CRC8/DVB-T of hdr & motorData
+} serialShortPacket_t;
+
+
+static serialShortPacket_t txPkt;
+static uint16_t motorValues[4];
+static serialPort_t * escPort = NULL;
+static serialPortConfig_t * portConfig;
+
+bool serialshotInitialize(void)
+{
+    // Avoid double initialization
+    if (escPort) {
+        return true;
+    }
+
+    portConfig = findSerialPortConfig(FUNCTION_SERIALSHOT);
+    if (!portConfig) {
+        return false;
+    }
+
+    escPort = openSerialPort(portConfig->identifier, FUNCTION_SERIALSHOT, NULL, NULL, SERIALSHOT_UART_BAUD, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR);
+    if (!escPort) {
+        return false;
+    }
+
+    return true;
+}
+
+void serialshotUpdateMotor(int index, uint16_t value)
+{
+    if (index < 0 && index > 3) {
+        return;
+    }
+
+    motorValues[index] = value;
+}
+
+void serialshotSendUpdate(void)
+{
+    // Check if the port is initialized
+    if (!escPort) {
+        return;
+    }
+
+    // Skip update if previous one is not yet fully sent
+    // This helps to avoid buffer overflow and evenyually the data corruption
+    if (!isSerialTransmitBufferEmpty(escPort)) {
+        return;
+    }
+
+    txPkt.hdr = SERIALSHOT_PKT_DEFAULT_HEADER;
+
+    txPkt.motorData[0] = motorValues[0] & 0x00FF;
+    txPkt.motorData[1] = motorValues[1] & 0x00FF;
+    txPkt.motorData[2] = motorValues[2] & 0x00FF;
+    txPkt.motorData[3] = motorValues[3] & 0x00FF;
+    txPkt.motorData[4] = (((motorValues[0] & 0xF00) >> 8) << 0) | (((motorValues[1] & 0xF00) >> 8) << 4);
+    txPkt.motorData[5] = (((motorValues[2] & 0xF00) >> 8) << 0) | (((motorValues[3] & 0xF00) >> 8) << 4);
+
+    txPkt.crc = crc8_dvb_s2(0x00, txPkt.hdr);
+    txPkt.crc = crc8_dvb_s2_update(txPkt.crc, txPkt.motorData, sizeof(txPkt.motorData));
+
+    serialWriteBuf(escPort, (const uint8_t *)&txPkt, sizeof(txPkt));
+}
+
+#endif

src/main/io/esc_serialshot.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+bool serialshotInitialize(void);
+void serialshotUpdateMotor(int index, uint16_t value);
+void serialshotSendUpdate(void);

src/main/io/osd.c

@@ -84,6 +84,7 @@
 
 #include "rx/rx.h"
 
+#include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/diagnostics.h"
@@ -98,14 +99,13 @@
 #define VIDEO_BUFFER_CHARS_PAL    480
 #define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL)
 
-#define CENTIMETERS_TO_CENTIFEET(cm)            (cm * (328 / 100.0))
-#define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
-#define CENTIMETERS_TO_METERS(cm)               (cm / 100)
 #define FEET_PER_MILE                           5280
 #define FEET_PER_KILOFEET                       1000 // Used for altitude
 #define METERS_PER_KILOMETER                    1000
 #define METERS_PER_MILE                         1609
 
+#define GFORCE_FILTER_TC 0.2
+
 #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
 
 #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms
@@ -137,6 +137,8 @@ static unsigned currentLayout = 0;
 static int layoutOverride = -1;
 static bool hasExtendedFont = false; // Wether the font supports characters > 256
 static timeMs_t layoutOverrideUntil = 0;
+static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT];
+static float GForce, GForceAxis[XYZ_AXIS_COUNT];
 
 typedef struct statistic_s {
     uint16_t max_speed;
@@ -188,7 +190,7 @@ static displayPort_t *osdDisplayPort;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8);
 
 static int digitCount(int32_t value)
 {
@@ -294,20 +296,22 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist)
 {
     switch ((osd_unit_e)osdConfig()->units) {
     case OSD_UNIT_IMPERIAL:
-        if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) {
+        if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) {
-            buff[0] = SYM_DIST_MI;
+            buff[3] = SYM_DIST_MI;
         } else {
-            buff[0] = SYM_DIST_FT;
+            buff[3] = SYM_DIST_FT;
         }
+        buff[4] = '\0';
         break;
     case OSD_UNIT_UK:
         FALLTHROUGH;
     case OSD_UNIT_METRIC:
-        if (osdFormatCentiNumber(buff + 1, dist, METERS_PER_KILOMETER, 0, 3, 3)) {
+        if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) {
-            buff[0] = SYM_DIST_KM;
+            buff[3] = SYM_DIST_KM;
         } else {
-            buff[0] = SYM_DIST_M;
+            buff[3] = SYM_DIST_M;
         }
+        buff[4] = '\0';
         break;
     }
 }
@@ -426,23 +430,25 @@ static void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         case OSD_UNIT_UK:
             FALLTHROUGH;
         case OSD_UNIT_IMPERIAL:
-            if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) {
+            if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) {
                 // Scaled to kft
-                buff[0] = SYM_ALT_KFT;
+                buff[3] = SYM_ALT_KFT;
             } else {
                 // Formatted in feet
-                buff[0] = SYM_ALT_FT;
+                buff[3] = SYM_ALT_FT;
             }
+            buff[4] = '\0';
             break;
         case OSD_UNIT_METRIC:
             // alt is alredy in cm
-            if (osdFormatCentiNumber(buff+1, alt, 1000, 0, 2, 3)) {
+            if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) {
                 // Scaled to km
-                buff[0] = SYM_ALT_KM;
+                buff[3] = SYM_ALT_KM;
             } else {
                 // Formatted in m
-                buff[0] = SYM_ALT_M;
+                buff[3] = SYM_ALT_M;
             }
+            buff[4] = '\0';
             break;
     }
 }
@@ -1163,8 +1169,10 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
     displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4));
+    digits = MIN(digits, 4);
-    strcat(buff, "V");
+    osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits);
+    buff[digits] = SYM_VOLT;
+    buff[digits+1] = '\0';
     if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
@@ -1241,34 +1249,40 @@ static bool osdDrawSingleElement(uint8_t item)
         return true;
 
     case OSD_CURRENT_DRAW:
-        buff[0] = SYM_AMP;
+
-        osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3);
+        osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3);
+        buff[3] = SYM_AMP;
+        buff[4] = '\0';
         break;
 
     case OSD_MAH_DRAWN:
-        buff[0] = SYM_MAH;
+        tfp_sprintf(buff, "%4d", (int)getMAhDrawn());
-        tfp_sprintf(buff + 1, "%-4d", (int)getMAhDrawn());
+        buff[4] = SYM_MAH;
+        buff[5] = '\0';
         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
         break;
 
     case OSD_WH_DRAWN:
-        buff[0] = SYM_WH;
+        osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
-        osdFormatCentiNumber(buff + 1, getMWhDrawn() / 10, 0, 2, 0, 3);
         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
+        buff[3] = SYM_WH;
+        buff[4] = '\0';
         break;
 
     case OSD_BATTERY_REMAINING_CAPACITY:
-        buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH);
 
         if (currentBatteryProfile->capacity.value == 0)
-            tfp_sprintf(buff + 1, "NA");
+            tfp_sprintf(buff, "  NA");
         else if (!batteryWasFullWhenPluggedIn())
-            tfp_sprintf(buff + 1, "NF");
+            tfp_sprintf(buff, "  NF");
         else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH)
-            tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity());
+            tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity());
         else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
             osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3);
 
+        buff[3] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
+        buff[4] = '\0';
+
         if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical))
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
 
@@ -1341,7 +1355,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP;
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
-            buff[1] = 0;
+            buff[1] = '\0';
             break;
         }
 
@@ -1374,7 +1388,7 @@ static bool osdDrawSingleElement(uint8_t item)
         break;
 
     case OSD_TRIP_DIST:
-        buff[0] = SYM_TRIP_DIST;
+        buff[0] = SYM_TOTAL;
         osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance());
         break;
 
@@ -1390,8 +1404,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else {
                 buff[1] = buff[2] = buff[3] = '-';
             }
-            buff[4] = SYM_DEGREES;
+            buff[4] = '\0';
-            buff[5] = '\0';
             break;
         }
 
@@ -1492,8 +1505,8 @@ static bool osdDrawSingleElement(uint8_t item)
             int32_t alt = osdGetAltitudeMsl();
             osdFormatAltitudeSymbol(buff, alt);
             break;
-        }		
+        }
-		
+
     case OSD_ONTIME:
         {
             osdFormatOnTime(buff);
@@ -1555,12 +1568,13 @@ static bool osdDrawSingleElement(uint8_t item)
         }
         buff[0] = SYM_TRIP_DIST;
         if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
-            buff[1] = SYM_DIST_M;
+            buff[4] = SYM_DIST_M;
-            strcpy(buff + 2, "---");
+            buff[5] = '\0';
+            strcpy(buff + 1, "---");
         } else if (distanceMeters == -2) {
             // Wind is too strong to come back with cruise throttle
-            buff[1] = SYM_DIST_M;
+            buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL;
-            buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL;
+            buff[4] = SYM_DIST_M;
             buff[5] = '\0';
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         } else {
@@ -2054,8 +2068,9 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_POWER:
         {
-            buff[0] = SYM_WATT;
+            osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3);
-            osdFormatCentiNumber(buff + 1, getPower(), 0, 2, 0, 3);
+            buff[3] = SYM_WATT;
+            buff[4] = '\0';
             break;
         }
 
@@ -2306,6 +2321,27 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_GFORCE:
+        {
+            osdFormatCentiNumber(buff, GForce, 0, 2, 0, 3);
+            if (GForce > osdConfig()->gforce_alarm * 100) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+            break;
+        }
+
+    case OSD_GFORCE_X:
+    case OSD_GFORCE_Y:
+    case OSD_GFORCE_Z:
+        {
+            float GForceValue = GForceAxis[item - OSD_GFORCE_X];
+            osdFormatCentiNumber(buff, GForceValue, 0, 2, 0, 4);
+            if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+            break;
+        }
+
     case OSD_DEBUG:
         {
             // Longest representable string is -32768, hence 6 characters
@@ -2571,7 +2607,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
     osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
     osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
-    osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG;
+    osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
     osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
@@ -2661,6 +2697,11 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
     osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7);
 
+    osdConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4);
+    osdConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5);
+    osdConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6);
+    osdConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7);
+
     // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
     osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
 
@@ -2677,6 +2718,9 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->neg_alt_alarm = 5;
     osdConfig->imu_temp_alarm_min = -200;
     osdConfig->imu_temp_alarm_max = 600;
+    osdConfig->gforce_alarm = 5;
+    osdConfig->gforce_axis_alarm_min = -5;
+    osdConfig->gforce_axis_alarm_max = 5;
 #ifdef USE_BARO
     osdConfig->baro_temp_alarm_min = -200;
     osdConfig->baro_temp_alarm_max = 600;
@@ -2935,6 +2979,19 @@ static void osdShowStats(void)
     tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
     displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
+    const float max_gforce = accGetMeasuredMaxG();
+    displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE      :");
+    osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3);
+    displayWrite(osdDisplayPort, statValuesX, top++, buff);
+
+    const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
+    displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
+    osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4);
+    strcat(buff,"/");
+    displayWrite(osdDisplayPort, statValuesX, top, buff);
+    osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3);
+    displayWrite(osdDisplayPort, statValuesX + 5, top++, buff);
+
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
 }
@@ -2981,8 +3038,33 @@ static void osdShowArmed(void)
     }
 }
 
+static void osdFilterData(timeUs_t currentTimeUs) {
+    static timeUs_t lastRefresh = 0;
+    float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6;
+
+    GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
+    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
+
+    if (lastRefresh) {
+        GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT);
+        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT);
+    } else {
+        pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0);
+        pt1FilterReset(&GForceFilter, GForce);
+
+        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
+            pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0);
+            pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]);
+        }
+    }
+
+    lastRefresh = currentTimeUs;
+}
+
 static void osdRefresh(timeUs_t currentTimeUs)
 {
+    osdFilterData(currentTimeUs);
+
 #ifdef USE_CMS
     if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) {
 #else

src/main/io/osd.h

@@ -137,6 +137,10 @@ typedef enum {
     OSD_PLUS_CODE,
     OSD_MAP_SCALE,
     OSD_MAP_REFERENCE,
+    OSD_GFORCE,
+    OSD_GFORCE_X,
+    OSD_GFORCE_Y,
+    OSD_GFORCE_Z,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -180,6 +184,9 @@ typedef struct osdConfig_s {
     uint16_t neg_alt_alarm; // abs(negative altitude) in m
     int16_t imu_temp_alarm_min;
     int16_t imu_temp_alarm_max;
+    float gforce_alarm;
+    float gforce_axis_alarm_min;
+    float gforce_axis_alarm_max;
 #ifdef USE_BARO
     int16_t baro_temp_alarm_min;
     int16_t baro_temp_alarm_max;

src/main/io/serial.h

@@ -49,6 +49,7 @@ typedef enum {
     FUNCTION_LOG                 = (1 << 15), // 32768
     FUNCTION_RANGEFINDER         = (1 << 16), // 65536
     FUNCTION_VTX_FFPV            = (1 << 17), // 131072
+    FUNCTION_SERIALSHOT          = (1 << 18), // 262144
 } serialPortFunction_e;
 
 typedef enum {

src/main/navigation/navigation.c

@@ -1119,6 +1119,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
                 initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
             }
 
+            posControl.rthInitialHomeDistance = posControl.homeDistance;
+
+            if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
+                setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+            }
+            else {
+                setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+            }
+
             return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_RTH_HEAD_HOME
         }
         else {
@@ -1144,8 +1153,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
 
                 if (navConfig()->general.flags.rth_tail_first) {
                     setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
-                }
+                } else {
-                else {
                     setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
                 }
             }
@@ -1179,13 +1187,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
             return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
         }
         else {
-            // Update XYZ-position target
+            if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
-            if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
+                fpVector3_t pos;
-                setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+                uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0;
-            }
+                uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome;
-            else {
+                uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart);
-                setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+                float RTHStartAltitude = posControl.homeWaypointAbove.pos.z;
+                float RTHFinalAltitude = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
+                pos.z = RTHStartAltitude - scaleRange(distanceToLoiterTraveled, 0, distanceToLoiterToTravelFromRTHStart, 0, RTHStartAltitude - RTHFinalAltitude);
+                setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
             }
+
             return NAV_FSM_EVENT_NONE;
         }
     }

src/main/navigation/navigation.h

@@ -63,11 +63,13 @@ enum {
 };
 
 enum {
-    NAV_RTH_NO_ALT          = 0,            // Maintain current altitude
+    NAV_RTH_NO_ALT                       = 0, // Maintain current altitude
-    NAV_RTH_EXTRA_ALT       = 1,            // Maintain current altitude + predefined safety margin
+    NAV_RTH_EXTRA_ALT                    = 1, // Maintain current altitude + predefined safety margin
-    NAV_RTH_CONST_ALT       = 2,            // Climb/descend to predefined altitude
+    NAV_RTH_CONST_ALT                    = 2, // Climb/descend to predefined altitude
-    NAV_RTH_MAX_ALT         = 3,            // Track maximum altitude and climb to it when RTH
+    NAV_RTH_MAX_ALT                      = 3, // Track maximum altitude and climb to it when RTH
-    NAV_RTH_AT_LEAST_ALT    = 4,            // Climb to predefined altitude if below it
+    NAV_RTH_AT_LEAST_ALT                 = 4, // Climb to predefined altitude if below it
+    NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT  = 5, // Climb to predefined altitude if below it,
+                                              // descend linearly to reach home at predefined altitude if above it
 };
 
 enum {
@@ -400,6 +402,7 @@ bool loadNonVolatileWaypointList(void);
 bool saveNonVolatileWaypointList(void);
 
 float RTHAltitude(void);
+int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
 
 /* Geodetic functions */
 typedef enum {

src/main/navigation/navigation_fixedwing.c

@@ -432,6 +432,11 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
     return throttleSpeedAdjustment;
 }
 
+int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
+{
+    return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
+}
+
 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 {
     int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
@@ -447,7 +452,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         // PITCH >0 dive, <0 climb
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
-        int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle;
+        int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection);
 
 #ifdef NAV_FIXED_WING_LANDING
         if (navStateFlags & NAV_CTL_LAND) {

src/main/navigation/navigation_pos_estimator.c

@@ -25,6 +25,7 @@
 #if defined(USE_NAV)
 
 #include "build/build_config.h"
+#include "build/debug.h"
 
 #include "common/axis.h"
 #include "common/log.h"
@@ -354,8 +355,37 @@ static bool gravityCalibrationComplete(void)
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
 
-static void updateIMUTopic(void)
+static void updateIMUEstimationWeight(const float dt)
 {
+    bool isAccClipped = accIsClipped();
+
+    // If accelerometer measurement is clipped - drop the acc weight to zero
+    // and gradually restore weight back to 1.0 over time
+    if (isAccClipped) {
+        posEstimator.imu.accWeightFactor = 0.0f;
+    }
+    else {
+        const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
+        posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
+    }
+
+    // DEBUG_VIBE[0-3] are used in IMU
+    DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
+}
+
+float navGetAccelerometerWeight(void)
+{
+    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
+
+    return accWeightScaled;
+}
+
+static void updateIMUTopic(timeUs_t currentTimeUs)
+{
+    const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
+    posEstimator.imu.lastUpdateTime = currentTimeUs;
+
     if (!isImuReady()) {
         posEstimator.imu.accelNEU.x = 0;
         posEstimator.imu.accelNEU.y = 0;
@@ -364,6 +394,9 @@ static void updateIMUTopic(void)
         restartGravityCalibration();
     }
     else {
+        /* Update acceleration weight based on vibration levels and clipping */
+        updateIMUEstimationWeight(dt);
+
         fpVector3_t accelBF;
 
         /* Read acceleration data in body frame */
@@ -435,12 +468,6 @@ static bool navIsHeadingUsable(void)
     }
 }
 
-float navGetAccelerometerWeight(void)
-{
-    // TODO(digitalentity): consider accelerometer health in weight calculation
-    return positionEstimationConfig()->w_xyz_acc_p;
-}
-
 static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 {
     /* Figure out if we have valid position data from our data sources */
@@ -768,6 +795,7 @@ void initializePositionEstimator(void)
     posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
     posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
 
+    posEstimator.imu.lastUpdateTime = 0;
     posEstimator.gps.lastUpdateTime = 0;
     posEstimator.baro.lastUpdateTime = 0;
     posEstimator.surface.lastUpdateTime = 0;
@@ -778,6 +806,8 @@ void initializePositionEstimator(void)
     posEstimator.est.flowCoordinates[X] = 0;
     posEstimator.est.flowCoordinates[Y] = 0;
 
+    posEstimator.imu.accWeightFactor = 0;
+
     restartGravityCalibration();
 
     for (axis = 0; axis < 3; axis++) {
@@ -806,7 +836,7 @@ void FAST_CODE NOINLINE updatePositionEstimator(void)
     const timeUs_t currentTimeUs = micros();
 
     /* Read updates from IMU, preprocess */
-    updateIMUTopic();
+    updateIMUTopic(currentTimeUs);
 
     /* Update estimate */
     updateEstimatedTopic(currentTimeUs);

src/main/navigation/navigation_pos_estimator_private.h

@@ -51,6 +51,8 @@
 #define INAV_BARO_AVERAGE_HZ                1.0f
 #define INAV_SURFACE_AVERAGE_HZ             1.0f
 
+#define INAV_ACC_CLIPPING_RC_CONSTANT           (0.010f)    // Reduce acc weight for ~10ms after clipping
+
 #define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
 #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
 #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
@@ -126,9 +128,11 @@ typedef struct {
 } navPositionEstimatorESTIMATE_t;
 
 typedef struct {
+     timeUs_t               lastUpdateTime;
     fpVector3_t             accelNEU;
     fpVector3_t             accelBias;
     float                   calibratedGravityCMSS;
+    float                   accWeightFactor;
     zeroCalibrationScalar_t gravityCalibration;
 } navPosisitonEstimatorIMU_t;
 

src/main/navigation/navigation_private.h

@@ -324,6 +324,7 @@ typedef struct {
     navWaypointPosition_t       homePosition;       // Special waypoint, stores original yaw (heading when launched)
     navWaypointPosition_t       homeWaypointAbove;  // NEU-coordinates and initial bearing + desired RTH altitude
     navigationHomeFlags_t       homeFlags;
+    uint32_t                    rthInitialHomeDistance;  // Distance to home after RTH has been initiated and the initial climb/descent is done
 
     uint32_t                    homeDistance;   // cm
     int32_t                     homeDirection;  // deg*100

src/main/sensors/acceleration.c

@@ -339,6 +339,11 @@ bool accInit(uint32_t targetLooptime)
     acc.accClipCount = 0;
     accInitFilters();
 
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        acc.extremes[axis].min = 100;
+        acc.extremes[axis].max = -100;
+    }
+
     if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
         acc.dev.accAlign = accelerometerConfig()->acc_align;
     }
@@ -504,7 +509,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
 }
 
 /*
- * Calculate measured acceleration in body frame in g
+ * Calculate measured acceleration in body frame in m/s^2
  */
 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
 {
@@ -513,6 +518,19 @@ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
     }
 }
 
+/*
+ * Return g's
+ */
+const acc_extremes_t* accGetMeasuredExtremes(void)
+{
+    return (const acc_extremes_t *)&acc.extremes;
+}
+
+float accGetMeasuredMaxG(void)
+{
+    return acc.maxG;
+}
+
 void accUpdate(void)
 {
     if (!acc.dev.readFn(&acc.dev)) {
@@ -541,8 +559,12 @@ void accUpdate(void)
 
     // Before filtering check for clipping and vibration levels
     if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
+        acc.isClipped = true;
         acc.accClipCount++;
     }
+    else {
+        acc.isClipped = false;
+    }
 
     // Calculate vibration levels
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -558,7 +580,6 @@ void accUpdate(void)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
     }
-    
 
 #ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_hz) {
@@ -570,6 +591,18 @@ void accUpdate(void)
 
 }
 
+// Record extremes: min/max for each axis and acceleration vector modulus
+void updateAccExtremes(void)
+{
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        if (acc.accADCf[axis] < acc.extremes[axis].min) acc.extremes[axis].min = acc.accADCf[axis];
+        if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
+    }
+
+    float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
+    if (gforce > acc.maxG) acc.maxG = gforce;
+}
+
 void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 {
     accVibeLevels->x = sqrtf(acc.accVibeSq[X]);
@@ -587,6 +620,11 @@ uint32_t accGetClipCount(void)
     return acc.accClipCount;
 }
 
+bool accIsClipped(void)
+{
+    return acc.isClipped;
+}
+
 void accSetCalibrationValues(void)
 {
     if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&

src/main/sensors/acceleration.h

@@ -49,12 +49,20 @@ typedef enum {
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 
+typedef struct {
+    float min;
+    float max;
+} acc_extremes_t;
+
 typedef struct acc_s {
     accDev_t dev;
     uint32_t accTargetLooptime;
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
     float accVibeSq[XYZ_AXIS_COUNT];
     uint32_t accClipCount;
+    bool isClipped;
+    acc_extremes_t extremes[XYZ_AXIS_COUNT];
+    float maxG;
 } acc_t;
 
 extern acc_t acc;
@@ -76,9 +84,13 @@ bool accInit(uint32_t accTargetLooptime);
 bool accIsCalibrationComplete(void);
 void accStartCalibration(void);
 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
+const acc_extremes_t* accGetMeasuredExtremes(void);
+float accGetMeasuredMaxG(void);
+void updateAccExtremes(void);
 void accGetVibrationLevels(fpVector3_t *accVibeLevels);
 float accGetVibrationLevel(void);
 uint32_t accGetClipCount(void);
+bool accIsClipped(void);
 void accUpdate(void);
 void accSetCalibrationValues(void);
 void accInitFilters(void);

src/main/target/MATEKF405/target.h

@@ -204,6 +204,7 @@
 #define TARGET_IO_PORTD         (BIT(2))
 
 #define USE_DSHOT
+#define USE_SERIALSHOT
 
 #define MAX_PWM_OUTPUT_PORTS       6
 

src/main/target/MATEKF411/target.h

@@ -151,6 +151,7 @@
 #define USE_SPEKTRUM_BIND
 #define BIND_PIN                PA10 //  RX1
 
+#define USE_SERIALSHOT
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define TARGET_IO_PORTA         0xffff

src/main/target/MATEKF722/target.h

@@ -160,4 +160,5 @@
 #define TARGET_IO_PORTD         (BIT(2))
 
 #define MAX_PWM_OUTPUT_PORTS        7
-#define USE_DSHOT
+#define USE_DSHOT
+#define USE_SERIALSHOT

src/main/target/MATEKF722SE/target.h

@@ -195,4 +195,5 @@
 #define TARGET_IO_PORTD 0xffff
 
 #define MAX_PWM_OUTPUT_PORTS        8
-#define USE_DSHOT
+#define USE_DSHOT
+#define USE_SERIALSHOT

src/main/target/OMNIBUSF7NXT/target.h

@@ -174,6 +174,7 @@
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define USE_DSHOT
+#define USE_SERIALSHOT
 
 // Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    6