Add remaining flight time/distance calculations and OSD items

make/source.mk

@@ -74,6 +74,7 @@ COMMON_SRC = \
             flight/mixer.c \
             flight/pid.c \
             flight/pid_autotune.c \
+            flight/rth_estimator.c \
             flight/servos.c \
             flight/wind_estimator.c \
             io/beeper.c \

src/main/build/debug.h

@@ -16,6 +16,8 @@
  */
 
 #include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
 
 #define DEBUG16_VALUE_COUNT 4
 extern int16_t debug[DEBUG16_VALUE_COUNT];
@@ -63,6 +65,7 @@ typedef enum {
     DEBUG_SAG_COMP_VOLTAGE,
     DEBUG_VIBE,
     DEBUG_CRUISE,
+    DEBUG_REM_FLIGHT_TIME,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cms/cms_menu_osd.c

@@ -157,6 +157,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("ON TIME", OSD_ONTIME),
     OSD_ELEMENT_ENTRY("FLY TIME", OSD_FLYTIME),
     OSD_ELEMENT_ENTRY("ON/FLY TIME", OSD_ONTIME_FLYTIME),
+    OSD_ELEMENT_ENTRY("REM. TIME", OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH),
+    OSD_ELEMENT_ENTRY("REM. DIST", OSD_REMAINING_DISTANCE_BEFORE_RTH),
     OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME),
     OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE),
     OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME),

src/main/common/maths.h

@@ -153,7 +153,9 @@ float cos_approx(float x);
 float atan2_approx(float y, float x);
 float acos_approx(float x);
 #define tan_approx(x)       (sin_approx(x) / cos_approx(x))
+#define asin_approx(x)      (M_PIf / 2 - acos_approx(x))
 #else
+#define asin_approx(x)      asinf(x)
 #define sin_approx(x)       sinf(x)
 #define cos_approx(x)       cosf(x)
 #define atan2_approx(y,x)   atan2f(y,x)

src/main/fc/fc_core.c

@@ -94,6 +94,7 @@ enum {
 #define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro
 
 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
+static timeUs_t flightTime = 0;
 
 float dT;
 
@@ -704,6 +705,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
+    if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) {
+        flightTime += cycleTime;
+    }
+
 #ifdef USE_ASYNC_GYRO_PROCESSING
     if (getAsyncMode() == ASYNC_MODE_NONE) {
         taskGyro(currentTimeUs);
@@ -825,3 +830,8 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
     processRx(currentTimeUs);
     isRXDataNew = true;
 }
+
+// returns seconds
+float getFlightTime() {
+    return (float)(flightTime / 1000) / 1000;
+}

src/main/fc/fc_core.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "common/time.h"
+
 typedef enum disarmReason_e {
     DISARM_NONE         = 0,
     DISARM_TIMEOUT      = 1,
@@ -29,6 +31,7 @@ typedef enum disarmReason_e {
     DISARM_REASON_COUNT
 } disarmReason_t;
 
+
 void handleInflightCalibrationStickPosition(void);
 
 void disarm(disarmReason_t disarmReason);
@@ -36,3 +39,4 @@ void tryArm(void);
 disarmReason_t getDisarmReason(void);
 
 bool isCalibrating(void);
+float getFlightTime();

src/main/fc/settings.yaml

@@ -67,7 +67,7 @@ tables:
   - name: i2c_speed
     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", "VIBE", "CRUISE"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -536,6 +536,17 @@ groups:
         field: voltageSource
         table: bat_voltage_source
         type: uint8_t
+      - name: cruise_power
+        field: cruise_power
+        min: 0
+        max: 4294967295
+      - name: idle_power
+        field: idle_power
+        min: 0
+        max: 65535
+      - name: rth_energy_margin
+        min: 0
+        max: 100
 
   - name: PG_BATTERY_PROFILES
     type: batteryProfile_t
@@ -1311,6 +1322,10 @@ groups:
         field: fw.loiter_radius
         min: 0
         max: 10000
+      - name: nav_fw_cruise_speed
+        field: fw.cruise_speed
+        min: 0
+        max: 65535
 
       - name: nav_fw_land_dive_angle
         field: fw.land_dive_angle
@@ -1531,6 +1546,10 @@ groups:
         min: 0
         max: 1
 
+      - name: osd_estimations_wind_compensation
+        field: estimations_wind_compensation
+        type: bool
+
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
     headers: ["fc/config.h"]

src/main/fc/stats.c

@@ -30,8 +30,14 @@ PG_RESET_TEMPLATE(statsConfig_t, statsConfig,
 
 static uint32_t arm_millis;
 static uint32_t arm_distance_cm;
+
 #ifdef USE_ADC
 static uint32_t arm_mWhDrawn;
+static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh
+
+uint32_t getFlyingEnergy() {
+    return flyingEnergy;
+}
 #endif
 
 void statsOnArm(void)
@@ -51,8 +57,11 @@ void statsOnDisarm(void)
             statsConfigMutable()->stats_total_time += dt;   //[s]
             statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100;   //[m]
 #ifdef USE_ADC
-            if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))
+            if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
-                statsConfigMutable()->stats_total_energy += getMWhDrawn() - arm_mWhDrawn;
+                const uint32_t energy = getMWhDrawn() - arm_mWhDrawn;
+                statsConfigMutable()->stats_total_energy += energy;
+                flyingEnergy += energy;
+            }
 #endif
             writeEEPROM();
         }

src/main/fc/stats.h

@@ -11,6 +11,7 @@ typedef struct statsConfig_s {
     uint8_t  stats_enabled;
 } statsConfig_t;
 
+uint32_t getFlyingEnergy();
 void statsOnArm(void);
 void statsOnDisarm(void);
 

src/main/flight/rth_estimator.c

@@ -0,0 +1,201 @@
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "fc/config.h"
+#include "fc/fc_core.h"
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/wind_estimator.h"
+
+#include "navigation/navigation.h"
+
+#include "sensors/battery.h"
+
+#include <stdint.h>
+
+#if defined(USE_ADC) && defined(USE_GPS)
+
+/* INPUTS:
+ *   - forwardSpeed (same unit as horizontalWindSpeed)
+ *   - heading degrees
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns degrees
+ */
+static float windDriftCompensationAngle(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    return RADIANS_TO_DEGREES(asin_approx(-horizontalWindSpeed * sin_approx(DEGREES_TO_RADIANS(windHeading - heading)) / forwardSpeed));
+}
+
+/* INPUTS:
+ *   - forwardSpeed (same unit as horizontalWindSpeed)
+ *   - heading degrees
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns (same unit as forwardSpeed and horizontalWindSpeed)
+ */
+static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    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
+ *   - horizontalWindSpeed (same unit as forwardSpeed)
+ *   - windHeading degrees
+ * OUTPUT:
+ *   returns (same unit as forwardSpeed and horizontalWindSpeed)
+ */
+static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
+    return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
+}
+
+// returns degrees
+static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) {
+    return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
+}
+
+// altitudeChange is in meters
+// idle_power and cruise_power are in deciWatt
+// output is in Watt
+static float estimateRTHAltitudeChangePower(float altitudeChange) {
+    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;
+}
+
+// altitudeChange is in m
+// verticalWindSpeed is in m/s
+// cruise_speed is in cm/s
+// 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;
+    return altitudeChange / estimatedVerticalSpeed;
+}
+
+// altitudeChange is in m
+// horizontalWindSpeed is in m/s
+// windHeading is in degrees
+// verticalWindSpeed is in m/s
+// cruise_speed is in cm/s
+// 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);
+    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) {
+    return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
+}
+
+// returns distance in m
+// *heading is in degrees 
+static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
+    float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed);
+    if (altitudeChange > 0) {
+        float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome);
+        float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff);
+        float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff);
+        *heading = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart));
+        return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart));
+    } else {
+        *heading = GPS_directionToHome;
+        return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance;
+    }
+}
+
+// returns mWh
+static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
+    // Fixed wing only for now
+    if (!STATE(FIXED_WING))
+        return -1;
+
+    if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isEstimatedWindSpeedValid() && isImuHeadingValid()))
+        return -1;
+
+    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_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 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));
+    DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
+
+    if (RTH_speed <= 0)
+        return -2; // wind is too strong
+
+    const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds
+    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 uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh
+    const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh
+
+    if (remaining_energy_before_rth < 0) // No energy left = No time left
+        return 0;
+
+    return remaining_energy_before_rth;
+}
+
+// returns seconds
+int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
+
+    const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
+
+    // error: return error code directly
+    if (remainingEnergyBeforeRTH < 0)
+        return remainingEnergyBeforeRTH;
+
+    const int32_t averagePower = calculateAveragePower();
+
+    if (averagePower == 0)
+        return -1;
+
+    const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower;
+
+    if (time_before_rth > 0x7FFFFFFF) // int32 overflow
+        return -1;
+
+    return time_before_rth;
+}
+
+// returns meters
+int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
+
+    const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
+
+    // error: return error code directly
+    if (remainingFlightTimeBeforeRTH < 0)
+        return remainingFlightTimeBeforeRTH;
+
+    return remainingFlightTimeBeforeRTH * calculateAverageSpeed();
+}
+
+#endif

src/main/flight/rth_estimator.h

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

src/main/flight/wind_estimator.c

@@ -173,4 +173,4 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     }
 }
 
-#endif
+#endif

src/main/flight/wind_estimator.h

@@ -23,6 +23,7 @@
 #endif
 #endif
 
+#include "common/axis.h"
 #include "common/time.h"
 
 bool isEstimatedWindSpeedValid(void);

src/main/io/osd.c

@@ -67,9 +67,11 @@
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
+#include "fc/fc_tasks.h"
 
 #include "flight/imu.h"
 #include "flight/pid.h"
+#include "flight/rth_estimator.h"
 #include "flight/wind_estimator.h"
 
 #include "navigation/navigation.h"
@@ -116,7 +118,6 @@
     x; \
 })
 
-static timeUs_t flyTime = 0;
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
 
@@ -160,7 +161,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, 2);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);
 
 static int digitCount(int32_t value)
 {
@@ -460,7 +461,7 @@ static inline void osdFormatOnTime(char *buff)
 
 static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
 {
-    uint32_t seconds = flyTime / 1000000;
+    uint32_t seconds = getFlightTime();
     osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H);
     if (attr && osdConfig()->time_alarm > 0) {
        if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) {
@@ -1233,6 +1234,60 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH:
+        {
+            static timeUs_t updatedTimestamp = 0;
+            /*static int32_t updatedTimeSeconds = 0;*/
+            timeUs_t currentTimeUs = micros();
+            static int32_t timeSeconds = -1;
+            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+                timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
+                updatedTimestamp = currentTimeUs;
+            }
+            if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
+                buff[0] = SYM_FLY_M;
+                strcpy(buff + 1, "--:--");
+                updatedTimestamp = 0;
+            } else if (timeSeconds == -2) {
+                // Wind is too strong to come back with cruise throttle
+                buff[0] = SYM_FLY_M;
+                buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
+                buff[3] = ':';
+                buff[6] = '\0';
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            } else {
+                osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
+                if (timeSeconds == 0)
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+        }
+        break;
+
+    case OSD_REMAINING_DISTANCE_BEFORE_RTH:;
+        static timeUs_t updatedTimestamp = 0;
+        timeUs_t currentTimeUs = micros();
+        static int32_t distanceMeters = -1;
+        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+            distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
+            updatedTimestamp = currentTimeUs;
+        }
+        buff[0] = SYM_TRIP_DIST;
+        if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
+            buff[1] = SYM_DIST_M;
+            strcpy(buff + 2, "---");
+        } else if (distanceMeters == -2) {
+            // Wind is too strong to come back with cruise throttle
+            buff[0] = SYM_DIST_M;
+            buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL;
+            buff[5] = '\0';
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        } else {
+            osdFormatDistanceSymbol(buff + 1, distanceMeters * 100);
+            if (distanceMeters == 0)
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        }
+        break;
+
     case OSD_FLYMODE:
         {
             char *p = "ACRO";
@@ -1875,6 +1930,9 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
         if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
             elementIndex = OSD_TRIP_DIST;
         }
+        if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
+            elementIndex = OSD_ITEM_COUNT;
+        }
     }
     if (!feature(FEATURE_GPS)) {
         if (elementIndex == OSD_GPS_SPEED) {
@@ -1952,6 +2010,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
     osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
+    osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
+    osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
 
     osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
@@ -1995,6 +2055,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->units = OSD_UNIT_METRIC;
     osdConfig->main_voltage_decimals = 1;
     osdConfig->attitude_angle_decimals = 0;
+
+    osdConfig->estimations_wind_compensation = true;
 }
 
 static void osdSetNextRefreshIn(uint32_t timeMs) {
@@ -2196,7 +2258,7 @@ static void osdShowStats(void)
     }
 
     displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
-    uint32_t flySeconds = flyTime / 1000000;
+    uint16_t flySeconds = getFlightTime();
     uint16_t flyMinutes = flySeconds / 60;
     flySeconds %= 60;
     uint16_t flyHours = flyMinutes / 60;
@@ -2241,8 +2303,6 @@ static void osdShowArmed(void)
 
 static void osdRefresh(timeUs_t currentTimeUs)
 {
-    static timeUs_t lastTimeUs = 0;
-
     if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) {
       displayClearScreen(osdDisplayPort);
       armState = ARMING_FLAG(ARMED);
@@ -2263,13 +2323,6 @@ static void osdRefresh(timeUs_t currentTimeUs)
         armState = ARMING_FLAG(ARMED);
     }
 
-    if (ARMING_FLAG(ARMED)) {
-        timeUs_t deltaT = currentTimeUs - lastTimeUs;
-        flyTime += deltaT;
-    }
-
-    lastTimeUs = currentTimeUs;
-
     if (resumeRefreshAt) {
         // If we already reached he time for the next refresh,
         // or THR is high or PITCH is high, resume refreshing.

src/main/io/osd.h

@@ -85,6 +85,8 @@ typedef enum {
     OSD_WIND_SPEED_VERTICAL,
     OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
     OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
+    OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
+    OSD_REMAINING_DISTANCE_BEFORE_RTH,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -136,6 +138,8 @@ typedef struct osdConfig_s {
 
     uint8_t units; // from osd_unit_e
     uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
+
+    bool    estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/navigation/navigation.c

@@ -124,6 +124,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_climb_angle = 20,
         .max_dive_angle = 15,
         .cruise_throttle = 1400,
+        .cruise_speed = 0,         // cm/s
         .max_throttle = 1700,
         .min_throttle = 1200,
         .pitch_to_throttle = 10,   // pwm units per degree of pitch (10pwm units ~ 1% throttle)
@@ -1893,6 +1894,22 @@ static void updateHomePositionCompatibility(void)
     GPS_directionToHome = posControl.homeDirection / 100;
 }
 
+float RTHAltitude() {
+    switch (navConfig()->general.flags.rth_alt_control_mode) {
+        case NAV_RTH_NO_ALT:
+            return(posControl.actualState.abs.pos.z);
+        case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
+            return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude);
+        case NAV_RTH_MAX_ALT:
+            return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
+        case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
+            return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
+        case NAV_RTH_CONST_ALT:     // Climb/descend to predefined altitude above home
+        default:
+            return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude);
+    }
+}
+
 /*-----------------------------------------------------------
  * Reset home position to current position
  *-----------------------------------------------------------*/
@@ -1900,24 +1917,7 @@ static void updateDesiredRTHAltitude(void)
 {
     if (ARMING_FLAG(ARMED)) {
         if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
-            switch (navConfig()->general.flags.rth_alt_control_mode) {
+            posControl.homeWaypointAbove.pos.z = RTHAltitude();
-            case NAV_RTH_NO_ALT:
-                posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
-                break;
-            case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
-                posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
-                break;
-            case NAV_RTH_MAX_ALT:
-                posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
-                break;
-            case NAV_RTH_AT_LEAST_ALT:  // Climb to at least some predefined altitude above home
-                posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
-                break;
-            case NAV_RTH_CONST_ALT:     // Climb/descend to predefined altitude above home
-            default:
-                posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
-                break;
-            }
         }
     }
     else {
@@ -3053,6 +3053,11 @@ int32_t navigationGetHomeHeading(void)
     return posControl.homePosition.yaw;
 }
 
+// returns m/s
+float calculateAverageSpeed() {
+    return (float)getTotalTravelDistance() / (getFlightTime() * 100);
+}
+
 #else // NAV
 
 #ifdef USE_GPS
@@ -3115,6 +3120,7 @@ int32_t getTotalTravelDistance(void)
 {
     return lrintf(GPS_totalTravelDistance);
 }
+
 #endif
 
 #endif  // NAV

src/main/navigation/navigation.h

@@ -152,6 +152,7 @@ typedef struct navConfig_s {
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_throttle;            // Cruise throttle
+        uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
         uint16_t min_throttle;               // Minimum allowed throttle in auto mode
         uint16_t max_throttle;               // Maximum allowed throttle in auto mode
         uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
@@ -309,6 +310,8 @@ void resetWaypointList(void);
 bool loadNonVolatileWaypointList(void);
 bool saveNonVolatileWaypointList(void);
 
+float RTHAltitude();
+
 /* Geodetic functions */
 typedef enum {
     GEO_ALT_ABSOLUTE,
@@ -339,6 +342,9 @@ bool navigationIsFlyingAutonomousMode(void);
 bool navigationRTHAllowsLanding(void);
 
 bool isNavLaunchEnabled(void);
+bool isFixedWingLaunchDetected(void);
+
+float calculateAverageSpeed();
 
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles

src/main/sensors/battery.c

@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+#include "build/debug.h"
+
 #include "common/maths.h"
 #include "common/filter.h"
 
@@ -32,7 +34,14 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/fc_core.h"
 #include "fc/runtime_config.h"
+#include "fc/stats.h"
+
+#include "flight/imu.h"
+#include "flight/mixer.h"
+
+#include "navigation/navigation.h"
 
 #include "config/feature.h"
 
@@ -44,7 +53,6 @@
 
 #include "io/beeper.h"
 
-#include "build/debug.h"
 
 #define ADCVREF 3300                 // in mV (3300 = 3.3V)
 
@@ -119,7 +127,11 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
         .offset = CURRENT_METER_OFFSET
     },
 
-    .voltageSource = BAT_VOLTAGE_RAW
+    .voltageSource = BAT_VOLTAGE_RAW,
+
+    .cruise_power = 0,
+    .idle_power = 0,
+    .rth_energy_margin = 5
 
 );
 
@@ -459,6 +471,14 @@ void powerMeterUpdate(timeUs_t timeDelta)
     mWhDrawn = mWhDrawnRaw / (3600 * 100);
 }
 
+// calculate true power including heat losses in power supply (battery + wires)
+// power is in cW (0.01W)
+// batteryWarningVoltage is in cV (0.01V)
+int32_t heatLossesCompensatedPower(int32_t power)
+{
+    return power + sq(power * 100 / batteryWarningVoltage) * powerSupplyImpedance / 100000;
+}
+
 void sagCompensatedVBatUpdate(timeUs_t currentTime)
 {
     static timeUs_t recordTimestamp = 0;
@@ -510,3 +530,13 @@ uint8_t calculateBatteryPercentage(void)
 void batteryDisableProfileAutoswitch(void) {
     profileAutoswitchDisable = true;
 }
+
+// returns cW (0.01W)
+int32_t calculateAveragePower() {
+    return (int64_t)mWhDrawn * 360 / getFlightTime();
+}
+
+// returns mWh / meter
+int32_t calculateAverageEfficiency() {
+    return getFlyingEnergy() * 100 / getTotalTravelDistance();
+}

src/main/sensors/battery.h

@@ -74,6 +74,10 @@ typedef struct batteryMetersConfig_s {
 
     batVoltageSource_e voltageSource;
 
+    uint32_t cruise_power;      // power drawn by the motor(s) at cruise throttle/speed (cW)
+    uint16_t idle_power;        // power drawn by the system when the motor(s) are stopped (cW)
+    uint8_t rth_energy_margin;  // Energy that should be left after RTH (%), used for remaining time/distance before RTH
+
 } batteryMetersConfig_t;
 
 typedef struct batteryProfile_s {
@@ -144,3 +148,6 @@ void powerMeterUpdate(timeUs_t timeDelta);
 
 uint8_t calculateBatteryPercentage(void);
 float calculateThrottleCompensationFactor(void);
+int32_t calculateAveragePower();
+int32_t calculateAverageEfficiency();
+int32_t heatLossesCompensatedPower(int32_t power);