RTH straight

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/fc/settings.yaml

@@ -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

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
-// 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;
+// pitch in degrees
+// output in Watt
+static float estimatePitchPower(float pitch) {
+    int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
     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) {
-    return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
+static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
+    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
-static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
+// distanceToHome is in meters
+// 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 windHeadingDegrees = 0;
-    const float verticalWindSpeed = 0;
+    const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading);
+    const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100;
 #endif
 
-    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, 0, lrintf(RTH_initial_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
-    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
+#ifdef USE_WIND_ESTIMATOR
+    const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
+#else
+    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;
-
-    if (time_before_rth > 0x7FFFFFFF) // int32 overflow
-        return -1;
+    const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower;
 
     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);
-int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
+float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
+float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
 #endif

src/main/io/osd.c

@@ -98,9 +98,6 @@
 #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

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_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);
+            if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
+                fpVector3_t pos;
+                uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0;
+                uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome;
+                uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart);
+                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_EXTRA_ALT       = 1,            // Maintain current altitude + predefined safety margin
-    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_AT_LEAST_ALT    = 4,            // Climb to predefined altitude if below it
+    NAV_RTH_NO_ALT                       = 0, // Maintain current altitude
+    NAV_RTH_EXTRA_ALT                    = 1, // Maintain current altitude + predefined safety margin
+    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_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_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