From f31c84edc5d1d126b7138dae30dca47bb43f3c7b Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 18 Nov 2024 09:07:38 +1100 Subject: [PATCH] re-name variables in alt_hold and update comments --- src/main/fc/tasks.c | 4 +- src/main/flight/alt_hold.c | 79 +++++++++++++++---------------- src/main/flight/alt_hold.h | 2 +- src/test/unit/althold_unittest.cc | 10 ++-- 4 files changed, 46 insertions(+), 49 deletions(-) diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index fe3e871ff0..3b582be80c 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -203,7 +203,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) break; case RX_STATE_UPDATE: - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + // updateRcCommands sets rcCommand, which is needed by updateAltHold and updateSonarAltHoldState updateRcCommands(); updateArmingStatus(); @@ -379,7 +379,7 @@ task_attribute_t task_attributes[TASK_COUNT] = { #endif #ifdef USE_ALT_HOLD_MODE - [TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHoldState, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW), + [TASK_ALTHOLD] = DEFINE_TASK("ALTHOLD", NULL, NULL, updateAltHold, TASK_PERIOD_HZ(ALTHOLD_TASK_RATE_HZ), TASK_PRIORITY_LOW), #endif #ifdef USE_POS_HOLD_MODE diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold.c index ea5d489791..098e017c9b 100644 --- a/src/main/flight/alt_hold.c +++ b/src/main/flight/alt_hold.c @@ -38,46 +38,46 @@ static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s typedef struct { - bool isAltHoldActive; + bool isActive; float targetAltitudeCm; - float maxRequestedVerticalVelocity; - float targetAltitudeAdjustRate; + float maxVelocity; + float targetVelocity; float deadband; bool allowStickAdjustment; } altHoldState_t; -altHoldState_t altHoldState; +altHoldState_t altHold; void altHoldReset(void) { resetAltitudeControl(); - altHoldState.targetAltitudeCm = getAltitudeCm(); - altHoldState.targetAltitudeAdjustRate = 0.0f; + altHold.targetAltitudeCm = getAltitudeCm(); + altHold.targetVelocity = 0.0f; } void altHoldInit(void) { - altHoldState.isAltHoldActive = false; - altHoldState.deadband = altHoldConfig()->alt_hold_deadband / 100.0f; - altHoldState.allowStickAdjustment = altHoldConfig()->alt_hold_deadband; - altHoldState.maxRequestedVerticalVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s + altHold.isActive = false; + altHold.deadband = altHoldConfig()->alt_hold_deadband / 100.0f; + altHold.allowStickAdjustment = altHoldConfig()->alt_hold_deadband; + altHold.maxVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s altHoldReset(); } void altHoldProcessTransitions(void) { if (FLIGHT_MODE(ALT_HOLD_MODE)) { - if (!altHoldState.isAltHoldActive) { + if (!altHold.isActive) { altHoldReset(); - altHoldState.isAltHoldActive = true; + altHold.isActive = true; } } else { - altHoldState.isAltHoldActive = false; + altHold.isActive = false; } // ** the transition out of alt hold (exiting altHold) may be rough. Some notes... ** // The original PR had a gradual transition from hold throttle to pilot control throttle - // using !(altHoldRequested && altHoldState.isAltHoldActive) to run an exit function + // using !(altHoldRequested && altHold.isAltHoldActive) to run an exit function // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated // it was removed primarily to simplify this PR @@ -88,46 +88,43 @@ void altHoldProcessTransitions(void) { void altHoldUpdateTargetAltitude(void) { - // The user can raise or lower the target altitude with throttle outside deadband. - // Adjustment is blocked by throttle low (zero), or inside deadband, or deadband configured to zero + // User can adjust the target altitude with throttle, but only when + // - throttle is outside deadband, and + // - throttle is not low (zero), and + // - deadband is not configured to zero - // Some people may not like throttle being able to change the target altitude, because: - // - with throttle adjustment, hitting the switch won't always hold altitude if throttle is bumped - // - eg if the throttle is bumped low accidentally, quad will start descending. - // On the plus side: - // - the pilot has control nice control over altitude, and the deadband is wide - // - Slow controlled descents are possible, eg for landing - // - fine-tuning height is possible, eg if there is slow sensor drift + float stickFactor = 0.0f; - float throttleAdjustmentFactor = 0.0f; - if (altHoldState.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) { + if (altHold.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) { const float rcThrottle = rcCommand[THROTTLE]; - const float lowThreshold = autopilotConfig()->hover_throttle - altHoldState.deadband * (autopilotConfig()->hover_throttle - PWM_RANGE_MIN); - const float highThreshold = autopilotConfig()->hover_throttle + altHoldState.deadband * (PWM_RANGE_MAX - autopilotConfig()->hover_throttle); + const float lowThreshold = autopilotConfig()->hover_throttle - altHold.deadband * (autopilotConfig()->hover_throttle - PWM_RANGE_MIN); + const float highThreshold = autopilotConfig()->hover_throttle + altHold.deadband * (PWM_RANGE_MAX - autopilotConfig()->hover_throttle); if (rcThrottle < lowThreshold) { - throttleAdjustmentFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); + stickFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); } else if (rcThrottle > highThreshold) { - throttleAdjustmentFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); + stickFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); } } // if failsafe is active, and we get here, we are in failsafe landing mode, it controls throttle if (failsafeIsActive()) { // descend at up to 10 times faster when high - // default landing time is now 60s; need to get the quad down in this time from reasonable height - // need a rapid descent if high to avoid that timeout, and must slow down closer to ground + // default landing timeout is now 60s; must to get the quad down within this limit + // need a rapid descent when initiated high, and must slow down closer to ground // this code doubles descent rate at 20m, to max 10x (10m/s on defaults) at 200m - // user should be able to descend within 60s from around 150m high without disarming, on defaults // the deceleration may be a bit rocky if it starts very high up // constant (set) deceleration target in the last 2m - throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f)); + stickFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f)); } - altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altHoldState.maxRequestedVerticalVelocity; + altHold.targetVelocity = stickFactor * altHold.maxVelocity; - // if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position - altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate * taskIntervalSeconds; - altHoldState.targetAltitudeCm = fmaxf(-200.0f, altHoldState.targetAltitudeCm); // not less than 2m below takeoff point + // prevent stick input from moving target altitude too far away from current altitude + // otherwise it can be difficult to bring target to the other side of current in reasonable time + // this constrains the P and I response to user target changes, but not D of F responses + if (fabsf(getAltitudeCm() - altHold.targetAltitudeCm) < 500.0f) { + altHold.targetAltitudeCm += altHold.targetVelocity * taskIntervalSeconds; + } } void altHoldUpdate(void) @@ -136,21 +133,21 @@ void altHoldUpdate(void) if (altHoldConfig()->alt_hold_adjust_rate) { altHoldUpdateTargetAltitude(); } - altitudeControl(altHoldState.targetAltitudeCm, taskIntervalSeconds, altHoldState.targetAltitudeAdjustRate); + altitudeControl(altHold.targetAltitudeCm, taskIntervalSeconds, altHold.targetVelocity); } -void updateAltHoldState(timeUs_t currentTimeUs) { +void updateAltHold(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); // check for enabling Alt Hold, otherwise do as little as possible while inactive altHoldProcessTransitions(); - if (altHoldState.isAltHoldActive) { + if (altHold.isActive) { altHoldUpdate(); } } bool isAltHoldActive(void) { - return altHoldState.isAltHoldActive; + return altHold.isActive; } #endif diff --git a/src/main/flight/alt_hold.h b/src/main/flight/alt_hold.h index 01dde94cc8..76428afa4e 100644 --- a/src/main/flight/alt_hold.h +++ b/src/main/flight/alt_hold.h @@ -25,7 +25,7 @@ #define ALTHOLD_TASK_RATE_HZ 100 // hz void altHoldInit(void); -void updateAltHoldState(timeUs_t currentTimeUs); +void updateAltHold(timeUs_t currentTimeUs); bool isAltHoldActive(void); #endif diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index 75931d6328..1e2dfc5a68 100644 --- a/src/test/unit/althold_unittest.cc +++ b/src/test/unit/althold_unittest.cc @@ -67,22 +67,22 @@ uint32_t millis() { TEST(AltholdUnittest, altHoldTransitionsTest) { - updateAltHoldState(currentTimeUs); + updateAltHold(currentTimeUs); EXPECT_EQ(isAltHoldActive(), false); flightModeFlags |= ALT_HOLD_MODE; millisRW = 42; - updateAltHoldState(currentTimeUs); + updateAltHold(currentTimeUs); EXPECT_EQ(isAltHoldActive(), true); flightModeFlags ^= ALT_HOLD_MODE; millisRW = 56; - updateAltHoldState(currentTimeUs); + updateAltHold(currentTimeUs); EXPECT_EQ(isAltHoldActive(), false); flightModeFlags |= ALT_HOLD_MODE; millisRW = 64; - updateAltHoldState(currentTimeUs); + updateAltHold(currentTimeUs); EXPECT_EQ(isAltHoldActive(), true); } @@ -93,7 +93,7 @@ TEST(AltholdUnittest, altHoldTransitionsTestUnfinishedExitEnter) flightModeFlags |= ALT_HOLD_MODE; millisRW = 42; - updateAltHoldState(currentTimeUs); + updateAltHold(currentTimeUs); EXPECT_EQ(isAltHoldActive(), true); }