Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +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);
}
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) {
// Countdown display for remaining Waypoints
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_HEADING_HOME "EN ROUTE TO HOME"
#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_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE"
#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)"

13
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,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
.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,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_LANDING,
@ -669,7 +669,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
.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,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_NONE,
@ -1460,6 +1460,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
}
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
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
@ -1469,8 +1470,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
navConfig()->general.land_slowdown_minalt + landingElevation,
navConfig()->general.land_slowdown_maxalt + landingElevation,
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);
rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
disarm(DISARM_NAVIGATION);
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
* TODO refactor conditions in this metod if logic is proven to be correct
*/
if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) {
int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
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
rcCommand[THROTTLE] = getThrottleIdleValue();