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 = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle;
  81     altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
  82     const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.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 calc_length_pythagorean_2D(triangleAltitude, 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
 149     const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
 150
 151     float RTH_heading; // degrees
 152 #ifdef USE_WIND_ESTIMATOR
 153     uint16_t windHeading; // centidegrees
 154     const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
 155     const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
 156     const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU
 157
 158     const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
 159     const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees);
 160 #else
 161     UNUSED(takeWindIntoAccount);
 162     const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading);
 163     const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100;
 164 #endif
 165
 166     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100));
 167     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
 168     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
 169 #ifdef USE_WIND_ESTIMATOR
 170     DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
 171 #endif
 172
 173     if (RTH_speed <= 0)
 174         return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost)
 175
 176 #ifdef USE_WIND_ESTIMATOR
 177     const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
 178 #else
 179     const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh
 180 #endif
 181     const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh
 182     const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh
 183
 184     if (remaining_energy_before_rth < 0) // No energy left = No time left
 185         return 0;
 186
 187     return remaining_energy_before_rth;
 188 }
 189
 190 // returns seconds
 191 float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
 192
 193     const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
 194
 195     // error: return error code directly
 196     if (remainingEnergyBeforeRTH < 0)
 197         return remainingEnergyBeforeRTH;
 198
 199     const float averagePower = (float)calculateAveragePower() / 100;
 200
 201     if (averagePower == 0)
 202         return -1;
 203
 204     const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower;
 205
 206     return time_before_rth;
 207 }
 208
 209 // returns meters
 210 float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 211
 212     // Fixed wing only for now
 213     if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) {
 214         return -1;
 215     }
 216
 217 #ifdef USE_WIND_ESTIMATOR
 218     if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) {
 219         return -1;
 220     }
 221 #endif
 222
 223     // check requirements
 224     const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
 225     const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
 226     const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
 227
 228     if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {
 229         return -1;
 230     }
 231
 232     const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
 233
 234     // error: return error code directly
 235     if (remainingFlightTimeBeforeRTH < 0)
 236         return remainingFlightTimeBeforeRTH;
 237
 238     return remainingFlightTimeBeforeRTH * calculateAverageSpeed();
 239 }
 240
 241 #endif