1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 17:55:30 +03:00

separate alt and pos hold deadbands

if poshold deadband is zero, sticks are ignored
if user tries to enable posHold and it can't work, they get stick with a deadband
This commit is contained in:
ctzsnooze 2024-10-14 13:28:50 +11:00
parent f09a71a76d
commit 4e1d199943
11 changed files with 34 additions and 25 deletions

View file

@ -1645,7 +1645,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_DEADBAND, "%d", rcControlsConfig()->yaw_deadband); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_DEADBAND, "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_DEADBAND, "%d", rcControlsConfig()->autopilot_deadband);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_HARDWARE_LPF, "%d", gyroConfig()->gyro_hardware_lpf); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_HARDWARE_LPF, "%d", gyroConfig()->gyro_hardware_lpf);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_TYPE, "%d", gyroConfig()->gyro_lpf1_type); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_TYPE, "%d", gyroConfig()->gyro_lpf1_type);
@ -1816,10 +1815,12 @@ static bool blackboxWriteSysinfo(void)
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_ADJUST_RATE, "%d", altHoldConfig()->alt_hold_adjust_rate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_ADJUST_RATE, "%d", altHoldConfig()->alt_hold_adjust_rate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", rcControlsConfig()->alt_hold_deadband);
#endif #endif
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", rcControlsConfig()->pos_hold_deadband);
#endif #endif
#ifdef USE_WING #ifdef USE_WING

View file

@ -1106,15 +1106,16 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) }, { PARAM_NAME_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
{ PARAM_NAME_YAW_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { PARAM_NAME_YAW_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
{ PARAM_NAME_AP_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, autopilot_deadband) },
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
{ PARAM_NAME_ALT_HOLD_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) }, { PARAM_NAME_ALT_HOLD_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
{ PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) },
#endif #endif
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
{ PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) }, { PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) },
{ PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, pos_hold_deadband) },
#endif #endif
// PG_PID_CONFIG // PG_PID_CONFIG

View file

@ -173,7 +173,6 @@
#define PARAM_NAME_POSITION_D "autopilot_position_D" #define PARAM_NAME_POSITION_D "autopilot_position_D"
#define PARAM_NAME_POSITION_J "autopilot_position_J" #define PARAM_NAME_POSITION_J "autopilot_position_J"
#define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff" #define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff"
#define PARAM_NAME_AP_DEADBAND "autopilot_deadband" // from rcControlsConfig
#define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward" #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
#define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
@ -248,12 +247,14 @@
#endif // USE_GPS #endif // USE_GPS
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
#define PARAM_NAME_ALT_HOLD_DEADBAND "alt_hold_deadband" // from rcControlsConfig
#define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate" #define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate"
#endif // USE_ALT_HOLD_MODE #endif
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "position_hold_without_mag" #define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag"
#endif // USE_POS_HOLD_MODE #define PARAM_NAME_POS_HOLD_DEADBAND "pos_hold_deadband" // from rcControlsConfig
#endif
#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp" #define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki" #define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"

View file

@ -716,7 +716,7 @@ FAST_CODE void processRcCommand(void)
rcCommandf = rcCommand[axis] / rcCommandYawDivider; rcCommandf = rcCommand[axis] / rcCommandYawDivider;
} else { } else {
if (FLIGHT_MODE(POS_HOLD_MODE)) { if (FLIGHT_MODE(POS_HOLD_MODE)) {
rcCommandf = rcCommand[axis] / (500.0f - rcControlsConfig()->autopilot_deadband); rcCommandf = rcCommand[axis] / (500.0f - rcControlsConfig()->pos_hold_deadband * 5.0f);
} else { } else {
rcCommandf = rcCommand[axis] / rcCommandDivider; rcCommandf = rcCommand[axis] / rcCommandDivider;
} }
@ -758,7 +758,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
// larger deadband on pitch and roll when in position hold // larger deadband on pitch and roll when in position hold
const float tmpDeadband = (FLIGHT_MODE(POS_HOLD_MODE)) ? rcControlsConfig()->autopilot_deadband * 5.0f : rcControlsConfig()->deadband; const float tmpDeadband = (FLIGHT_MODE(POS_HOLD_MODE) && rcControlsConfig()->pos_hold_deadband) ? rcControlsConfig()->pos_hold_deadband * 5.0f : rcControlsConfig()->deadband;
if (axis == ROLL || axis == PITCH) { if (axis == ROLL || axis == PITCH) {
if (tmp > tmpDeadband) { if (tmp > tmpDeadband) {
tmp -= tmpDeadband; tmp -= tmpDeadband;

View file

@ -78,7 +78,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONT
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 0, .deadband = 0,
.yaw_deadband = 0, .yaw_deadband = 0,
.autopilot_deadband = 40, .alt_hold_deadband = 40,
.pos_hold_deadband = 10,
.yaw_control_reversed = false, .yaw_control_reversed = false,
); );

View file

@ -110,7 +110,8 @@ typedef struct rcSmoothingFilter_s {
typedef struct rcControlsConfig_s { typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t autopilot_deadband; // deadband while in altitude or position control modes uint8_t alt_hold_deadband; // throttle deadband while in altitude hold
uint8_t pos_hold_deadband; // RPY deadband while in position hold
bool yaw_control_reversed; // invert control direction of yaw bool yaw_control_reversed; // invert control direction of yaw
} rcControlsConfig_t; } rcControlsConfig_t;

View file

@ -66,6 +66,7 @@ void altHoldReset(void)
void altHoldInit(void) void altHoldInit(void)
{ {
altHoldState.isAltHoldActive = false; altHoldState.isAltHoldActive = false;
altHoldState.deadband = rcControlsConfig()->alt_hold_deadband / 100.0f;
altHoldReset(); altHoldReset();
} }
@ -108,8 +109,8 @@ void altHoldUpdateTargetAltitude(void)
const float rcThrottle = rcCommand[THROTTLE]; const float rcThrottle = rcCommand[THROTTLE];
const float lowThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MIN); // halfway between hover and MIN, e.g. 1150 if hover is 1300 const float lowThreshold = autopilotConfig()->hover_throttle - altHoldState.deadband * (autopilotConfig()->hover_throttle - PWM_RANGE_MIN);
const float highThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300 const float highThreshold = autopilotConfig()->hover_throttle + altHoldState.deadband * (PWM_RANGE_MAX - autopilotConfig()->hover_throttle);
float throttleAdjustmentFactor = 0.0f; float throttleAdjustmentFactor = 0.0f;
if (rcThrottle < lowThreshold) { if (rcThrottle < lowThreshold) {

View file

@ -28,6 +28,7 @@ typedef struct {
bool isAltHoldActive; bool isAltHoldActive;
float targetAltitudeCm; float targetAltitudeCm;
float targetAltitudeAdjustRate; float targetAltitudeAdjustRate;
float deadband;
} altHoldState_t; } altHoldState_t;
void altHoldInit(void); void altHoldInit(void);

View file

@ -45,7 +45,8 @@ void posHoldReset(void)
void posHoldInit(void) void posHoldInit(void)
{ {
posHold.isPosHoldRequested = false; posHold.isPosHoldRequested = false;
posHold.deadband = rcControlsConfig()->autopilot_deadband / 100.0f; posHold.deadband = rcControlsConfig()->pos_hold_deadband / 100.0f;
posHold.useStickAdjustment = rcControlsConfig()->pos_hold_deadband;
posHoldReset(); posHoldReset();
} }
@ -67,13 +68,15 @@ void posHoldUpdateTargetLocation(void)
// if failsafe is active, eg landing mode, don't update the original start point // if failsafe is active, eg landing mode, don't update the original start point
if (!failsafeIsActive()) { if (!failsafeIsActive()) {
// otherwise if sticks are not centered, allow start point to be updated // otherwise if sticks are not centered, allow start point to be updated
if (posHold.useStickAdjustment) {
if ((getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband)) { if ((getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband)) {
// allow user to fly the quad, in angle mode, enabling a 20% deadband via rc.c (?) // allow user to fly the quad, in angle mode, when sticks are outside the deadband
// while sticks are outside the deadband, // while sticks are outside the deadband,
// keep updating the home location to the current GPS location each iteration // keep updating the home location to the current GPS location each iteration
posHoldReset(); posHoldReset();
} }
} }
}
} }
void posHoldUpdate(void) void posHoldUpdate(void)
@ -108,6 +111,4 @@ bool allowPosHoldWithoutMag(void) {
return (posHoldConfig()->pos_hold_without_mag); return (posHoldConfig()->pos_hold_without_mag);
} }
#endif // USE_POS_HOLD #endif // USE_POS_HOLD

View file

@ -30,6 +30,7 @@ typedef struct {
bool posHoldIsOK; bool posHoldIsOK;
gpsLocation_t targetLocation; gpsLocation_t targetLocation;
float deadband; float deadband;
bool useStickAdjustment;
} posHoldState_t; } posHoldState_t;
void posHoldInit(void); void posHoldInit(void);

View file

@ -1806,7 +1806,7 @@ case MSP_NAME:
case MSP_RC_DEADBAND: case MSP_RC_DEADBAND:
sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, rcControlsConfig()->autopilot_deadband); sbufWriteU8(dst, rcControlsConfig()->pos_hold_deadband);
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break; break;
@ -2962,7 +2962,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
case MSP_SET_RC_DEADBAND: case MSP_SET_RC_DEADBAND:
rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->autopilot_deadband = sbufReadU8(src); rcControlsConfigMutable()->pos_hold_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break; break;