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

Limit the climb/descent rate from RTH alt. to desired home alt. to the max auto climb rate setting

Browse source
This commit is contained in:
Michel Pastor 2018-07-24 17:55:36 +02:00
parent 14738b5c90
commit 606155da27
1 changed files with 15 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -501,7 +501,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = { [NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME, .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME, .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
.timeoutMs = 500, .timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
@ -1185,20 +1185,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// If position ok OR within valid timeout - continue // If position ok OR within valid timeout - continue
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// set altitude to go to when landing is not allowed
if (navConfig()->general.rth_home_altitude && !navigationRTHAllowsLanding()) {
posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude;
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
}
// Wait until target heading is reached (with 15 deg margin for error) // Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
resetLandingDetector(); resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else { else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector(); resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else if (!validateRTHSanityChecker()) { else if (!validateRTHSanityChecker()) {
@ -1223,6 +1219,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
if ((navConfig()->general.rth_home_altitude) && (posControl.desiredState.pos.z != navConfig()->general.rth_home_altitude)) {
int8_t altitudeChangeDirection = navConfig()->general.rth_home_altitude > posControl.homeWaypointAbove.pos.z ? 1 : -1;
float timeToReachHomeAltitude = altitudeChangeDirection * (navConfig()->general.rth_home_altitude - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
if (timeToReachHomeAltitude < 1) {
// we almost reached the target home altitude so set the desired altitude to the desired home altitude
posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude;
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
} else {
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
}
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }