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

fixes

Browse source
This commit is contained in:
breadoven 2023-03-28 11:29:50 +01:00
parent a0456565ac
commit 0b7632d4c4
3 changed files with 7 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/navigation/navigation.c
View file

@ -2869,10 +2869,10 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
int8_t direction = desiredClimbRate > 0 ? 1 : -1;
float absClimbRate = fabsf(desiredClimbRate);
uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
const float absClimbRate = fabsf(desiredClimbRate);
const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);

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

@ -766,7 +766,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
if (posControl.flags.estAltStatus >= EST_USABLE) {
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET);
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));

2
src/main/navigation/navigation_multicopter.c
View file

@ -843,7 +843,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// target min descent rate 5m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}