Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 15:25:29 +03:00
Code Issues Wiki Activity

Update navigation_fixedwing.c

Browse source 
Add fixed wing Emergency Landing vertical descent rate control if altitude sensors working
This commit is contained in:
breadoven 2021-08-21 11:00:30 +01:00
parent b55bc46f12
commit 35cf5e89a3
1 changed files with 16 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -612,13 +612,23 @@ bool isFixedWingLandingDetected(void)
/*-----------------------------------------------------------
* FixedWing emergency landing
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(void)
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
// FIXME: Use altitude controller if available (similar to MC code)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (isPitchAdjustmentValid) {
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
}
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
}
/*-----------------------------------------------------------