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

update

Browse source
This commit is contained in:
breadoven 2023-03-26 23:24:26 +01:00
parent f8da237e28
commit 5a454a3400
3 changed files with 18 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -157,9 +157,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
const float pitchGainInv = 1.0f / 1.0f;
// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Reduce max allowed climb pitch if performing loiter climb
if (isNavHoldPositionActive()) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
}
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
@ -760,7 +765,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));