1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00

Update rth_estimator.c

This commit is contained in:
breadoven 2020-12-30 14:26:19 +00:00
parent a1f9bbdc8c
commit e0840a4ae6

View file

@ -145,21 +145,6 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee
// returns Wh
static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
// Fixed wing only for now
if (!STATE(FIXED_WING_LEGACY))
return -1;
if (!(feature(FEATURE_VBAT) && batteryWasFullWhenPluggedIn()
&& feature(FEATURE_CURRENT_METER) && (batteryMetersConfig()->cruise_power > 0)
&& (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0)
&& navigationPositionEstimateIsHealthy() && isImuHeadingValid() && (navConfig()->fw.cruise_speed > 0)
&& ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))
&& (ARMING_FLAG(ARMED))
#ifdef USE_WIND_ESTIMATOR
&& (!takeWindIntoAccount || isEstimatedWindSpeedValid())
#endif
))
return -1;
const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
@ -223,7 +208,27 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
// returns meters
float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
// Fixed wing only for now
if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) {
return -1;
}
#ifdef USE_WIND_ESTIMATOR
if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) {
return -1;
}
#endif
// check requirements
const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {
return -1;
}
const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
// error: return error code directly