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:
parent
a1f9bbdc8c
commit
e0840a4ae6
1 changed files with 21 additions and 16 deletions
|
@ -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 &¤tBatteryProfile->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
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue