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

Update navigation_fixedwing.c

Browse source
This commit is contained in:
breadoven 2022-12-19 11:48:50 +00:00
parent 99b03c70be
commit 1d292b1925
1 changed files with 6 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -755,14 +755,13 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
// Hold position of possible
if (posControl.flags.estPosStatus >= EST_USABLE) {
if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible
applyFixedWingPositionController(currentTimeUs);
if (isRollAdjustmentValid) {
// ROLL >0 right, <0 left
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]);
}
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]);