Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 19:40:27 +03:00
Code Issues Wiki Activity

fw flight detection fix

Browse source
This commit is contained in:
breadoven 2025-01-19 12:48:01 +00:00
parent a54ba7d052
commit 0d9dd1b721
4 changed files with 17 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/navigation/navigation.c
View file

@ -3543,6 +3543,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
landingDetectorIsActive = false; landingDetectorIsActive = false;
} }
resetLandingDetector(); resetLandingDetector();
getTakeoffAltitude();
return; return;
} }
@ -5278,6 +5279,16 @@ uint8_t getActiveWpNumber(void)
return NAV_Status.activeWpNumber; return NAV_Status.activeWpNumber;
} }
float getTakeoffAltitude(void)
{
static float refTakeoffAltitude = 0.0f;
if (!ARMING_FLAG(ARMED) && !landingDetectorIsActive) {
refTakeoffAltitude = posControl.actualState.abs.pos.z;
}
return refTakeoffAltitude;
}
#ifdef USE_FW_AUTOLAND #ifdef USE_FW_AUTOLAND
static void resetFwAutoland(void) static void resetFwAutoland(void)

5
src/main/navigation/navigation_fixedwing.c
View file

@ -712,10 +712,11 @@ bool isFixedWingFlying(void)
} }
#endif #endif
bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool velCondition = posControl.actualState.velXY > 350.0f || airspeed > 350.0f;
bool altCondition = fabsf(posControl.actualState.abs.pos.z - getTakeoffAltitude()) > 500.0f;
bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition; return (isGPSHeadingValid() && throttleCondition && velCondition && altCondition) || launchCondition;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

2
src/main/navigation/navigation_fw_launch.c
View file

@ -299,7 +299,7 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
static inline bool isLaunchMaxAltitudeReached(void) static inline bool isLaunchMaxAltitudeReached(void)
{ {
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); return (navConfig()->fw.launch_max_altitude > 0) && ((getEstimatedActualPosition(Z) - getTakeoffAltitude()) >= navConfig()->fw.launch_max_altitude);
} }
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)

1
src/main/navigation/navigation_private.h
View file

@ -554,6 +554,7 @@ bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void); bool isLastMissionWaypoint(void);
float getActiveSpeed(void); float getActiveSpeed(void);
bool isWaypointNavTrackingActive(void); bool isWaypointNavTrackingActive(void);
float getTakeoffAltitude(void);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);