Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +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;
}
resetLandingDetector();
getTakeoffAltitude();
return;
}
@ -5278,6 +5279,16 @@ uint8_t getActiveWpNumber(void)
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
static void resetFwAutoland(void)

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

@ -712,10 +712,11 @@ bool isFixedWingFlying(void)
}
#endif
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;
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)
{
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)

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

@ -423,7 +423,7 @@ typedef enum {
#ifdef USE_GEOZONE
typedef struct navSendTo_s {
fpVector3_t targetPos;
uint16_t altitudeTargetRange; // 0 for only "2D"
uint16_t altitudeTargetRange; // 0 for only "2D"
uint32_t targetRange;
bool lockSticks;
uint32_t lockStickTime;
@ -554,6 +554,7 @@ bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void);
float getActiveSpeed(void);
bool isWaypointNavTrackingActive(void);
float getTakeoffAltitude(void);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);