Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

fix landing

Browse source
This commit is contained in:
breadoven 2022-08-25 22:12:38 +01:00
parent a38bce9b1a
commit 8ceec5b304
4 changed files with 14 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/io/osd.c
View file

@ -4406,7 +4406,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
} }
if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED); messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
} else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
// Countdown display for remaining Waypoints // Countdown display for remaining Waypoints
char buf[6]; char buf[6];

1
src/main/io/osd.h
View file

@ -94,6 +94,7 @@
#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" #define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING"
#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME"
#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" #define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION"
#define OSD_MSG_WP_LANDED "WP END>LANDED"
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" #define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE"
#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" #define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)"

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

@ -655,7 +655,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING, .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING, .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
.timeoutMs = 0, .timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING, .mwError = MW_NAV_ERROR_LANDING,
@ -669,7 +669,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHED, .persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
.timeoutMs = 10, .timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .stateFlags = NAV_CTL_ALT | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LANDED, .mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_NONE, .mwError = MW_NAV_ERROR_NONE,
@ -1460,6 +1460,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} }
float descentVelLimited = 0; float descentVelLimited = 0;
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
@ -1469,7 +1470,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} else { } else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, navConfig()->general.land_slowdown_minalt + landingElevation,
navConfig()->general.land_slowdown_maxalt + landingElevation,
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
@ -1807,8 +1809,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
{ {
UNUSED(previousState); UNUSED(previousState);
rcCommand[THROTTLE] = getThrottleIdleValue(); disarm(DISARM_NAVIGATION);
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }

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

@ -635,9 +635,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
* Then altitude is below landing slowdown min. altitude, enable final approach procedure * Then altitude is below landing slowdown min. altitude, enable final approach procedure
* TODO refactor conditions in this metod if logic is proven to be correct * TODO refactor conditions in this metod if logic is proven to be correct
*/ */
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) || int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
(posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= finalAltitude)) {
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
rcCommand[THROTTLE] = getThrottleIdleValue(); rcCommand[THROTTLE] = getThrottleIdleValue();