1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00

move deadbands for pos and alt hold to their config files.

This commit is contained in:
ctzsnooze 2024-10-19 11:58:51 +11:00
parent af7e0f36f4
commit cbb169ae70
13 changed files with 24 additions and 24 deletions

View file

@ -1814,12 +1814,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); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->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); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband);
#endif #endif
#ifdef USE_WING #ifdef USE_WING

View file

@ -56,15 +56,12 @@
#include "fc/rc_adjustments.h" #include "fc/rc_adjustments.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/pos_hold.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
@ -81,6 +78,8 @@
#include "osd/osd.h" #include "osd/osd.h"
#include "pg/adc.h" #include "pg/adc.h"
#include "pg/alt_hold.h"
#include "pg/autopilot.h"
#include "pg/beeper.h" #include "pg/beeper.h"
#include "pg/beeper_dev.h" #include "pg/beeper_dev.h"
#include "pg/bus_i2c.h" #include "pg/bus_i2c.h"
@ -98,6 +97,7 @@
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "pg/pinio.h" #include "pg/pinio.h"
#include "pg/piniobox.h" #include "pg/piniobox.h"
#include "pg/pos_hold.h"
#include "pg/rx.h" #include "pg/rx.h"
#include "pg/rx_pwm.h" #include "pg/rx_pwm.h"
#include "pg/rx_spi.h" #include "pg/rx_spi.h"
@ -1110,12 +1110,12 @@ const clivalue_t valueTable[] = {
#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) }, { PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_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) }, { PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) },
#endif #endif
// PG_PID_CONFIG // PG_PID_CONFIG

View file

@ -247,13 +247,13 @@
#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_DEADBAND "alt_hold_deadband"
#define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate" #define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate"
#endif #endif
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
#define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag" #define PARAM_NAME_POS_HOLD_WITHOUT_MAG "pos_hold_without_mag"
#define PARAM_NAME_POS_HOLD_DEADBAND "pos_hold_deadband" // from rcControlsConfig #define PARAM_NAME_POS_HOLD_DEADBAND "pos_hold_deadband"
#endif #endif
#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp" #define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"

View file

@ -48,6 +48,7 @@
#include "flight/pid_init.h" #include "flight/pid_init.h"
#include "pg/rx.h" #include "pg/rx.h"
#include "pg/pos_hold.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -757,9 +758,9 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
float tmpDeadband = rcControlsConfig()->deadband; float tmpDeadband = rcControlsConfig()->deadband;
if (FLIGHT_MODE(POS_HOLD_MODE)) { if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (rcControlsConfig()->pos_hold_deadband) { if (posHoldConfig()->pos_hold_deadband) {
// if pos_hold_deadband is defined, ignore pitch & roll within deadband zone // if pos_hold_deadband is defined, ignore pitch & roll within deadband zone
tmpDeadband = rcControlsConfig()->pos_hold_deadband * 5.0f; tmpDeadband = posHoldConfig()->pos_hold_deadband * 5.0f;
// NB could attenuate RP responsiveness outside deadband here, with tmp * 0.8f or whatever // NB could attenuate RP responsiveness outside deadband here, with tmp * 0.8f or whatever
} else { } else {
// if pos_hold_deadband is zero, prevent user adjustment of pitch or roll // if pos_hold_deadband is zero, prevent user adjustment of pitch or roll
@ -855,8 +856,8 @@ void initRcProcessing(void)
{ {
#ifdef USE_POS_HOLD_MODE #ifdef USE_POS_HOLD_MODE
if (FLIGHT_MODE(POS_HOLD_MODE)) { if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (rcControlsConfig()->pos_hold_deadband) { if (posHoldConfig()->pos_hold_deadband) {
rcCommandDivider = 500.0f - rcControlsConfig()->pos_hold_deadband * 5.0f; // pos hold deadband in percent rcCommandDivider = 500.0f - posHoldConfig()->pos_hold_deadband * 5.0f; // pos hold deadband in percent
} }
} }
#else #else

View file

@ -78,8 +78,6 @@ 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,
.alt_hold_deadband = 40,
.pos_hold_deadband = 10,
.yaw_control_reversed = false, .yaw_control_reversed = false,
); );

View file

@ -110,8 +110,6 @@ 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 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,7 +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; altHoldState.deadband = altHoldConfig()->alt_hold_deadband / 100.0f;
altHoldReset(); altHoldReset();
} }

View file

@ -43,8 +43,8 @@ void posHoldResetTargetLocation(void)
void posHoldInit(void) void posHoldInit(void)
{ {
posHold.deadband = rcControlsConfig()->pos_hold_deadband / 100.0f; posHold.deadband = posHoldConfig()->pos_hold_deadband / 100.0f;
posHold.useStickAdjustment = rcControlsConfig()->pos_hold_deadband; posHold.useStickAdjustment = posHoldConfig()->pos_hold_deadband;
posHold.posHoldIsOK = false; posHold.posHoldIsOK = false;
} }

View file

@ -124,6 +124,7 @@
#include "pg/dyn_notch.h" #include "pg/dyn_notch.h"
#include "pg/gyrodev.h" #include "pg/gyrodev.h"
#include "pg/motor.h" #include "pg/motor.h"
#include "pg/pos_hold.h"
#include "pg/rx.h" #include "pg/rx.h"
#include "pg/rx_spi.h" #include "pg/rx_spi.h"
#ifdef USE_RX_EXPRESSLRS #ifdef USE_RX_EXPRESSLRS
@ -1806,7 +1807,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()->pos_hold_deadband); sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband);
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break; break;
@ -2962,7 +2963,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()->pos_hold_deadband = sbufReadU8(src); posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break; break;

View file

@ -34,5 +34,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFI
PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
.alt_hold_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s .alt_hold_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
.alt_hold_deadband = 40, // throttle deadband in percent of stick travel
); );
#endif #endif

View file

@ -27,7 +27,7 @@
typedef struct altHoldConfig_s { typedef struct altHoldConfig_s {
uint8_t alt_hold_adjust_rate; uint8_t alt_hold_adjust_rate;
uint8_t alt_hold_deadband;
} altHoldConfig_t; } altHoldConfig_t;
PG_DECLARE(altHoldConfig_t, altHoldConfig); PG_DECLARE(altHoldConfig_t, altHoldConfig);

View file

@ -34,5 +34,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFI
PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
.pos_hold_without_mag = false, // position hold within this percentage stick deflection .pos_hold_without_mag = false, // position hold within this percentage stick deflection
.pos_hold_deadband = 15, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks
); );
#endif #endif

View file

@ -27,7 +27,7 @@
typedef struct posHoldConfig_s { typedef struct posHoldConfig_s {
bool pos_hold_without_mag; bool pos_hold_without_mag;
uint8_t pos_hold_deadband;
} posHoldConfig_t; } posHoldConfig_t;
PG_DECLARE(posHoldConfig_t, posHoldConfig); PG_DECLARE(posHoldConfig_t, posHoldConfig);