src/main/flight/rth_estimator.c

   1
   2 #include "build/debug.h"
   3
   4 #include "common/maths.h"
   5 #include "common/utils.h"
   6
   7 #include "fc/config.h"
   8 #include "fc/fc_core.h"
   9 #include "fc/runtime_config.h"
  10
  11 #include "flight/imu.h"
  12 #include "flight/mixer.h"
  13 #include "flight/wind_estimator.h"
  14
  15 #include "navigation/navigation.h"
  16
  17 #include "sensors/battery.h"
  18
  19 #include <stdint.h>
  20
  21 #if defined(USE_ADC) && defined(USE_GPS)
  22
  23 /* INPUTS:
  24  *   - heading degrees
  25  *   - horizontalWindSpeed
  26  *   - windHeading degrees
  27  * OUTPUT:
  28  *   returns same unit as horizontalWindSpeed
  29  */
  30 static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
  31     return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
  32 }
  33
  34 #ifdef USE_WIND_ESTIMATOR
  35 /* INPUTS:
  36  *   - forwardSpeed (same unit as horizontalWindSpeed)
  37  *   - heading degrees
  38  *   - horizontalWindSpeed (same unit as forwardSpeed)
  39  *   - windHeading degrees
  40  * OUTPUT:
  41  *   returns degrees
  42  */
  43 static float windDriftCompensationAngle(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
  44     return RADIANS_TO_DEGREES(asin_approx(-horizontalWindSpeed * sin_approx(DEGREES_TO_RADIANS(windHeading - heading)) / forwardSpeed));
  45 }
  46
  47 /* INPUTS:
  48  *   - forwardSpeed (same unit as horizontalWindSpeed)
  49  *   - heading degrees
  50  *   - horizontalWindSpeed (same unit as forwardSpeed)
  51  *   - windHeading degrees
  52  * OUTPUT:
  53  *   returns (same unit as forwardSpeed and horizontalWindSpeed)
  54  */
  55 static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
  56     return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading)));
  57 }
  58
  59 /* INPUTS:
  60  *   - forwardSpeed (same unit as horizontalWindSpeed)
  61  *   - heading degrees
  62  *   - horizontalWindSpeed (same unit as forwardSpeed)
  63  *   - windHeading degrees
  64  * OUTPUT:
  65  *   returns (same unit as forwardSpeed and horizontalWindSpeed)
  66  */
  67 static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
  68     return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
  69 }
  70 #endif
  71
  72 // returns degrees
  73 static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
  74     return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
  75 }
  76
  77 // pitch in degrees
  78 // output in Watt
  79 static float estimatePitchPower(float pitch) {
  80     int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
  81     altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
  82     const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
  83     return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
  84 }
  85
  86 // altitudeChange is in m
  87 // verticalWindSpeed is in m/s
  88 // cruise_speed is in cm/s
  89 // output is in seconds
  90 static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) {
  91     // Assuming increase in throttle keeps air speed at cruise speed
  92     const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
  93     return altitudeChange / estimatedVerticalSpeed;
  94 }
  95
  96 // altitudeChange is in m
  97 // horizontalWindSpeed is in m/s
  98 // windHeading is in degrees
  99 // verticalWindSpeed is in m/s
 100 // cruise_speed is in cm/s
 101 // output is in meters
 102 static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) {
 103     // Assuming increase in throttle keeps air speed at cruise speed
 104     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);
 105     return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed;
 106 }
 107
 108 // altitudeChange is in m
 109 // verticalWindSpeed is in m/s
 110 // output is in Wh
 111 static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
 112     const float RTHInitialAltitudeChangePower = estimatePitchPower(altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle);
 113     return RTHInitialAltitudeChangePower * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
 114 }
 115
 116 // horizontalWindSpeed is in m/s
 117 // windHeading is in degrees
 118 // verticalWindSpeed is in m/s
 119 // altitudeChange is in m
 120 // returns distance in m
 121 // *heading is in degrees
 122 static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
 123     float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed);
 124     if (navConfig()->general.flags.rth_climb_first && (altitudeChange > 0)) {
 125         float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome);
 126         float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff);
 127         float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff);
 128         const float reverseHeadingDiff = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart));
 129         *heading = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(180 + reverseHeadingDiff + DECIDEGREES_TO_DEGREES((float)attitude.values.yaw))));
 130         return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart));
 131     } else {
 132         *heading = GPS_directionToHome;
 133         return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance;
 134     }
 135 }
 136
 137 // distanceToHome is in meters
 138 // output in Watt
 139 static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
 140     const float timeToHome = distanceToHome / speedToHome; // seconds
 141     const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - getFinalRTHAltitude()) : 0);
 142     const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
 143     return estimatePitchPower(pitchToHome) * timeToHome / 3600;
 144 }
 145
 146 // returns Wh
 147 static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
 148     // Fixed wing only for now
 149     if (!STATE(FIXED_WING_LEGACY))
 150         return -1;
 151
 152     if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING_LEGACY)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isImuHeadingValid()
 153 #ifdef USE_WIND_ESTIMATOR
 154             && isEstimatedWindSpeedValid()
 155 #endif
 156        ))
 157         return -1;
 158
 159     const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
 160
 161     float RTH_heading; // degrees
 162 #ifdef USE_WIND_ESTIMATOR
 163     uint16_t windHeading; // centidegrees
 164     const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
 165     const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
 166     const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
 167
 168     const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
 169     const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees);
 170 #else
 171     UNUSED(takeWindIntoAccount);
 172     const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading);
 173     const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100;
 174 #endif
 175
 176     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100));
 177     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
 178     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
 179 #ifdef USE_WIND_ESTIMATOR
 180     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
 181 #endif
 182
 183     if (RTH_speed <= 0)
 184         return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost)
 185
 186 #ifdef USE_WIND_ESTIMATOR
 187     const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
 188 #else
 189     const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
 190 #endif
 191     const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh
 192     const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh
 193
 194     if (remaining_energy_before_rth < 0) // No energy left = No time left
 195         return 0;
 196
 197     return remaining_energy_before_rth;
 198 }
 199
 200 // returns seconds
 201 float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
 202
 203     const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
 204
 205     // error: return error code directly
 206     if (remainingEnergyBeforeRTH < 0)
 207         return remainingEnergyBeforeRTH;
 208
 209     const float averagePower = (float)calculateAveragePower() / 100;
 210
 211     if (averagePower == 0)
 212         return -1;
 213
 214     const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower;
 215
 216     return time_before_rth;
 217 }
 218
 219 // returns meters
 220 float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 221
 222     const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
 223
 224     // error: return error code directly
 225     if (remainingFlightTimeBeforeRTH < 0)
 226         return remainingFlightTimeBeforeRTH;
 227
 228     return remainingFlightTimeBeforeRTH * calculateAverageSpeed();
 229 }
 230
 231 #endif