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

Merge pull request #8686 from breadoven/abo_manual_emerg_landing

Browse source 
Manual emergency landing FW and MR
This commit is contained in:
Paweł Spychalski 2023-01-27 16:11:05 +01:00 • committed by GitHub
commit bb79ad8f02
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 86 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -752,8 +752,6 @@ bool isFixedWingLandingDetected(void)
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
@ -765,6 +763,18 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible
applyFixedWingPositionController(currentTimeUs);
int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
-DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = 0;
} else {
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
}
}
/*-----------------------------------------------------------