Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Fix a bug when landing controller took over the altitude control on a FW if below certain altitude

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-08-30 21:38:15 +10:00
parent 49c35fbaf8
commit 562f1403f5
1 changed files with 21 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -424,7 +424,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
} }
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if (navStateFlags & NAV_CTL_POS && !(navStateFlags & NAV_CTL_LAND)) { if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs); throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
} }
@ -452,6 +452,7 @@ 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 (posControl.flags.hasValidAltitudeSensor && posControl.actualState.pos.V.Z < navConfig()->general.land_slowdown_minalt) { if (posControl.flags.hasValidAltitudeSensor && posControl.actualState.pos.V.Z < navConfig()->general.land_slowdown_minalt) {
/* /*
* 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
@ -472,6 +473,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
*/ */
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
}
#endif #endif
} }