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

fix loiter detection FW + tweaks

Browse source
This commit is contained in:
breadoven 2023-03-27 21:42:01 +01:00
parent 5a454a3400
commit a0456565ac
3 changed files with 8 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2855,7 +2855,7 @@ bool isFlightDetected(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{ {
#define MIN_TARGET_CLIMB_RATE 100.0f #define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
static timeUs_t lastUpdateTimeUs; static timeUs_t lastUpdateTimeUs;
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
@ -2864,8 +2864,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate always /* ROC_TO_ALT_CONSTANT - constant climb rate
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min value when altitude reached * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */ * 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) { if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
@ -2873,7 +2873,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
float absClimbRate = fabsf(desiredClimbRate); float absClimbRate = fabsf(desiredClimbRate);
uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
direction * -500.0f, direction * -maxRateCutoffAlt, MIN_TARGET_CLIMB_RATE, absClimbRate); 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
} }

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

@ -160,8 +160,8 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
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); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Reduce max allowed climb pitch if performing loiter climb // Reduce max allowed climb pitch if performing loiter (stall prevention)
if (isNavHoldPositionActive()) { if (needToCalculateCircularLoiter) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
} }
@ -765,6 +765,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) { 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, ROC_TO_ALT_TARGET);
applyFixedWingAltitudeAndThrottleController(currentTimeUs); applyFixedWingAltitudeAndThrottleController(currentTimeUs);

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

@ -842,6 +842,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago // Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { 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, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);