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

add deadbands

Browse source
This commit is contained in:
ctzsnooze 2024-10-09 21:53:05 +11:00
parent 897b9a9a61
commit 36f7379d6f
13 changed files with 61 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/main/blackbox/blackbox.c
View file

@ -1645,6 +1645,7 @@ 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);
@ -1687,24 +1688,24 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_HARDWARE, "%d", accelerometerConfig()->acc_hardware);
#endif
#ifdef USE_BARO
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware);
#endif
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", autopilotConfig()->altitude_I);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", autopilotConfig()->altitude_D);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", autopilotConfig()->altitude_F);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d", autopilotConfig()->position_P);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d", autopilotConfig()->position_I);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d", autopilotConfig()->position_D);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", autopilotConfig()->landing_altitude_m);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", autopilotConfig()->hover_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", autopilotConfig()->throttle_min);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d", autopilotConfig()->throttle_max);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d", autopilotConfig()->altitude_P);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d", autopilotConfig()->altitude_I);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d", autopilotConfig()->altitude_D);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d", autopilotConfig()->altitude_F);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d", autopilotConfig()->position_P);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d", autopilotConfig()->position_I);;
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d", autopilotConfig()->position_D);;
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
@ -1817,7 +1818,7 @@ static bool blackboxWriteSysinfo(void)
#endif
#ifdef USE_POS_HOLD_MODE
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_ADJUST_RATE, "%d", posHoldConfig()->pos_hold_adjust_rate);
// nothing at present
#endif
#ifdef USE_WING

3
src/main/cli/settings.c
View file

@ -1108,6 +1108,7 @@ 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) },
#ifdef USE_ALT_HOLD_MODE
@ -1115,7 +1116,7 @@ const clivalue_t valueTable[] = {
#endif
#ifdef USE_POS_HOLD_MODE
{ PARAM_NAME_POS_HOLD_ADJUST_RATE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_adjust_rate) },
// nothing at present
#endif
// PG_PID_CONFIG

5
src/main/fc/parameter_names.h
View file

@ -171,6 +171,7 @@
#define PARAM_NAME_POSITION_P "autopilot_position_P"
#define PARAM_NAME_POSITION_I "autopilot_position_I"
#define PARAM_NAME_POSITION_D "autopilot_position_D"
#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,10 +249,6 @@
#define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate"
#endif // USE_ALT_HOLD_MODE
#ifdef USE_POS_HOLD_MODE
#define PARAM_NAME_POS_HOLD_ADJUST_RATE "pos_hold_adjust_rate"
#endif // USE_POS_HOLD_MODE
#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
#define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"

13
src/main/fc/rc.c
View file

@ -715,9 +715,12 @@ FAST_CODE void processRcCommand(void)
if (axis == FD_YAW) {
rcCommandf = rcCommand[axis] / rcCommandYawDivider;
} else {
rcCommandf = rcCommand[axis] / rcCommandDivider;
if (FLIGHT_MODE(POS_HOLD_MODE)) {
rcCommandf = rcCommand[axis] / (500.0f - rcControlsConfig()->autopilot_deadband);
} else {
rcCommandf = rcCommand[axis] / rcCommandDivider;
}
}
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = fabsf(rcCommandf);
rcDeflectionAbs[axis] = rcCommandfAbs;
@ -754,9 +757,11 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
for (int axis = 0; axis < 3; axis++) {
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;
if (axis == ROLL || axis == PITCH) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
if (tmp > tmpDeadband) {
tmp -= tmpDeadband;
} else {
tmp = 0;
}

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

@ -73,12 +73,12 @@ static bool isUsingSticksToArm = true;
float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1);
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 0,
.yaw_deadband = 0,
.pos_hold_deadband = 40,
.autopilot_deadband = 40,
.yaw_control_reversed = false,
);

2
src/main/fc/rc_controls.h
View file

@ -110,7 +110,7 @@ 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 pos_hold_deadband; // pitch and roll deadband when position hold is active
uint8_t autopilot_deadband; // deadband while in altitude or position control modes
bool yaw_control_reversed; // invert control direction of yaw
} rcControlsConfig_t;

9
src/main/flight/autopilot.c
View file

@ -24,6 +24,7 @@
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "fc/rc.h"
#include "flight/imu.h"
#include "flight/pid.h"
@ -124,7 +125,7 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float vertical
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 7, lrintf(altitudeF));
}
void positionControl(gpsLocation_t targetLocation) {
void positionControl(gpsLocation_t targetLocation, float deadband) {
// gpsSol.llh = current gps location
// get distance and bearing from current location to target location
// void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing)
@ -184,9 +185,9 @@ void positionControl(gpsLocation_t targetLocation) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollSetpoint));
// but for now let's not really do that until we get the PIDs sorted out :-)
// posHoldAngle[AI_PITCH] = 0.0f;
// posHoldAngle[AI_ROLL] = 0.0f;
// if sticks are centered, allow pilot control, otherwise use stick control
posHoldAngle[AI_ROLL] = (getRcDeflectionAbs(FD_ROLL) < deadband) ? rollSetpoint : 0.0f;
posHoldAngle[AI_PITCH] = (getRcDeflectionAbs(FD_PITCH) < deadband) ? pitchSetpoint : 0.0f;
}
bool isBelowLandingAltitude(void)

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

@ -28,7 +28,7 @@ void resetAltitudeControl(void);
void resetPositionControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
void positionControl(gpsLocation_t targetLocation);
void positionControl(gpsLocation_t targetLocation, float deadband);
bool isBelowLandingAltitude(void);

44
src/main/flight/pos_hold.c
View file

@ -33,60 +33,55 @@
#include "pos_hold.h"
posHoldState_t posHoldState;
posHoldState_t posHold;
void posHoldReset(void)
{
resetPositionControl();
posHoldState.targetLocation = gpsSol.llh;
posHoldState.targetAdjustRate = 0.0f;
posHold.targetLocation = gpsSol.llh;
}
void posHoldInit(void)
{
posHoldState.isPosHoldActive = false;
posHold.isPosHoldActive = false;
posHold.deadband = rcControlsConfig()->autopilot_deadband / 100.0f;
posHoldReset();
}
void posHoldProcessTransitions(void) {
void posHoldProcessTransitions(void)
{
if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (!posHoldState.isPosHoldActive) {
if (!posHold.isPosHoldActive) {
// initialise relevant values each time position hold starts
posHoldReset();
posHoldState.isPosHoldActive = true;
posHold.isPosHoldActive = true;
}
} else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, -22);
posHoldState.isPosHoldActive = false;
posHold.isPosHoldActive = false;
}
}
void posHoldUpdateTargetLocation(void)
{
// The user can adjust the target position by flying around in level mode
// We need to provide a big deadband in rc.c
// if failsafe is active, eg landing mode, don't update the original start point
if (!failsafeIsActive()) {
const float deadband = 0.2f;
if ((getRcDeflectionAbs(FD_ROLL) > deadband) || (getRcDeflectionAbs(FD_PITCH) > deadband)) {
// 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,
// set the target location to the current GPS location each iteration
posHoldState.targetLocation = gpsSol.llh;
// keep updating the home location to the current GPS location each iteration
posHold.targetLocation = gpsSol.llh;
}
}
}
void posHoldUpdate(void)
{
// check if the user wants to change the target position using sticks
if (posHoldConfig()->pos_hold_adjust_rate) {
posHoldUpdateTargetLocation();
}
posHoldUpdateTargetLocation();
if (getIsNewDataForPosHold()) {
positionControl(posHoldState.targetLocation);
} else {
positionControl(posHold.targetLocation, posHold.deadband);
}
}
@ -96,8 +91,11 @@ void updatePosHoldState(timeUs_t currentTimeUs) {
// check for enabling Alt Hold, otherwise do as little as possible while inactive
posHoldProcessTransitions();
if (posHoldState.isPosHoldActive) {
if (posHold.isPosHoldActive) {
posHoldUpdate();
} else {
posHoldAngle[AI_PITCH] = 0.0f;
posHoldAngle[AI_ROLL] = 0.0f;
}
}

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

@ -28,7 +28,7 @@
typedef struct {
bool isPosHoldActive;
gpsLocation_t targetLocation;
float targetAdjustRate;
float deadband;
} posHoldState_t;
void posHoldInit(void);

4
src/main/msp/msp.c
View file

@ -1806,7 +1806,7 @@ case MSP_NAME:
case MSP_RC_DEADBAND:
sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, rcControlsConfig()->pos_hold_deadband);
sbufWriteU8(dst, rcControlsConfig()->autopilot_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()->pos_hold_deadband = sbufReadU8(src);
rcControlsConfigMutable()->autopilot_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break;

2
src/main/pg/pos_hold.c
View file

@ -32,6 +32,6 @@
PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 1);
PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
.pos_hold_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
.unused = 33, // position hold within this percentage stick deflection
);
#endif

2
src/main/pg/pos_hold.h
View file

@ -25,7 +25,7 @@
#include "pg/pg.h"
typedef struct posHoldConfig_s {
uint8_t pos_hold_adjust_rate;
uint8_t unused;
} posHoldConfig_t;
PG_DECLARE(posHoldConfig_t, posHoldConfig);