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

Merge pull request #8920 from breadoven/abo_climb_rate_controller_improvement

Browse source 
Nav altitude control improvements
This commit is contained in:
breadoven 2023-08-07 12:21:57 +01:00 • committed by GitHub
commit 5f1a10ad74
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 51 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void)
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
}
@ -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 (stall prevention)
if (needToCalculateCircularLoiter) {
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,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, 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));