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

40
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,26 +452,28 @@ 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 (posControl.flags.hasValidAltitudeSensor && posControl.actualState.pos.V.Z < navConfig()->general.land_slowdown_minalt) { if (navStateFlags & NAV_CTL_LAND) {
/* 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
rcCommand[THROTTLE] = motorConfig()->minthrottle; */
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); rcCommand[THROTTLE] = motorConfig()->minthrottle;
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
/* /*
* Stabilize ROLL axis on 0 degress banking regardless of loiter radius and position * Stabilize ROLL axis on 0 degress banking regardless of loiter radius and position
*/ */
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
/* /*
* Stabilize PITCH angle into shallow dive as specified by the * Stabilize PITCH angle into shallow dive as specified by the
* nav_fw_land_dive_angle setting (default value is 2 - defined * nav_fw_land_dive_angle setting (default value is 2 - defined
* in navigation.c). * in navigation.c).
* PITCH angle is measured: >0 - dive, <0 - climb) * PITCH angle is measured: >0 - dive, <0 - climb)
*/ */
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
} }