Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed wing flight detection fix #10621

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+17 −4
Open

Fixed wing flight detection fix #10621

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3543,6 +3543,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
landingDetectorIsActive = false;
}
resetLandingDetector();
getTakeoffAltitude();

return;
}
Expand Down Expand Up @@ -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)
Expand Down
5 changes: 3 additions & 2 deletions src/main/navigation/navigation_fixedwing.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

/*-----------------------------------------------------------
Expand Down
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_fw_launch.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
3 changes: 2 additions & 1 deletion src/main/navigation/navigation_private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
Loading