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

re-name variables in alt_hold and update comments

Browse source
This commit is contained in:
ctzsnooze 2024-11-18 09:07:38 +11:00
parent 2f70f989ae
commit f31c84edc5
4 changed files with 46 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/fc/tasks.c
View file

@ -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

79
src/main/flight/alt_hold.c
View file

@ -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

2
src/main/flight/alt_hold.h
View file

@ -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

10
src/test/unit/althold_unittest.cc
View file

@ -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);
}