diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2c13a13db0..3e998ea75c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1645,7 +1645,6 @@ static bool blackboxWriteSysinfo(void) 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_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_LPF1_TYPE, "%d", gyroConfig()->gyro_lpf1_type); @@ -1816,10 +1815,12 @@ static bool blackboxWriteSysinfo(void) #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_DEADBAND, "%d", rcControlsConfig()->alt_hold_deadband); #endif #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_DEADBAND, "%d", rcControlsConfig()->pos_hold_deadband); #endif #ifdef USE_WING diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4c3fe09096..0a87c77afb 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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_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 - { 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 #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 // PG_PID_CONFIG diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 04ad4edc3b..682f580acb 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -173,7 +173,6 @@ #define PARAM_NAME_POSITION_D "autopilot_position_D" #define PARAM_NAME_POSITION_J "autopilot_position_J" #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_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms" @@ -248,12 +247,14 @@ #endif // USE_GPS #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" -#endif // USE_ALT_HOLD_MODE +#endif #ifdef USE_POS_HOLD_MODE -#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "position_hold_without_mag" -#endif // USE_POS_HOLD_MODE +#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag" +#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_KI "imu_dcm_ki" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 2bf571f1fc..e67e883c0f 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -716,7 +716,7 @@ FAST_CODE void processRcCommand(void) rcCommandf = rcCommand[axis] / rcCommandYawDivider; } else { 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 { rcCommandf = rcCommand[axis] / rcCommandDivider; } @@ -758,7 +758,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void) float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); // 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 (tmp > tmpDeadband) { tmp -= tmpDeadband; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 69bf166918..132c63e0ad 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -78,7 +78,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONT PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = 0, .yaw_deadband = 0, - .autopilot_deadband = 40, + .alt_hold_deadband = 40, + .pos_hold_deadband = 10, .yaw_control_reversed = false, ); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 8ba21596d3..c2e1afcc55 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -110,7 +110,8 @@ typedef struct rcSmoothingFilter_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 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 } rcControlsConfig_t; diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold.c index 2b3d1b3ebb..7f6789650e 100644 --- a/src/main/flight/alt_hold.c +++ b/src/main/flight/alt_hold.c @@ -66,6 +66,7 @@ void altHoldReset(void) void altHoldInit(void) { altHoldState.isAltHoldActive = false; + altHoldState.deadband = rcControlsConfig()->alt_hold_deadband / 100.0f; altHoldReset(); } @@ -108,8 +109,8 @@ void altHoldUpdateTargetAltitude(void) 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 highThreshold = 0.5f * (autopilotConfig()->hover_throttle + PWM_RANGE_MAX); // halfway between hover and MAX, e.g. 1650 if hover is 1300 + 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); float throttleAdjustmentFactor = 0.0f; if (rcThrottle < lowThreshold) { diff --git a/src/main/flight/alt_hold.h b/src/main/flight/alt_hold.h index 1655114eff..20f3ca369a 100644 --- a/src/main/flight/alt_hold.h +++ b/src/main/flight/alt_hold.h @@ -28,6 +28,7 @@ typedef struct { bool isAltHoldActive; float targetAltitudeCm; float targetAltitudeAdjustRate; + float deadband; } altHoldState_t; void altHoldInit(void); diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index f48ded974e..f9055137f1 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -45,7 +45,8 @@ void posHoldReset(void) void posHoldInit(void) { 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(); } @@ -67,11 +68,13 @@ void posHoldUpdateTargetLocation(void) // if failsafe is active, eg landing mode, don't update the original start point if (!failsafeIsActive()) { // otherwise if sticks are not centered, allow start point to be updated - 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 (?) - // while sticks are outside the deadband, - // keep updating the home location to the current GPS location each iteration - posHoldReset(); + if (posHold.useStickAdjustment) { + if ((getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband)) { + // allow user to fly the quad, in angle mode, when sticks are outside the deadband + // while sticks are outside the deadband, + // keep updating the home location to the current GPS location each iteration + posHoldReset(); + } } } } @@ -108,6 +111,4 @@ bool allowPosHoldWithoutMag(void) { return (posHoldConfig()->pos_hold_without_mag); } - - #endif // USE_POS_HOLD diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index dd62b60141..13e262c27b 100644 --- a/src/main/flight/pos_hold.h +++ b/src/main/flight/pos_hold.h @@ -30,6 +30,7 @@ typedef struct { bool posHoldIsOK; gpsLocation_t targetLocation; float deadband; + bool useStickAdjustment; } posHoldState_t; void posHoldInit(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 3ae3bd9b0d..60690be91b 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1806,7 +1806,7 @@ case MSP_NAME: case MSP_RC_DEADBAND: sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); - sbufWriteU8(dst, rcControlsConfig()->autopilot_deadband); + sbufWriteU8(dst, rcControlsConfig()->pos_hold_deadband); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); break; @@ -2962,7 +2962,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP_SET_RC_DEADBAND: rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); - rcControlsConfigMutable()->autopilot_deadband = sbufReadU8(src); + rcControlsConfigMutable()->pos_hold_deadband = sbufReadU8(src); flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); break;