diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1e859d348a..c43d531108 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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 diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9e2debc8fc..95b3d4c104 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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 diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 2c9e953520..2fc5a7a0d7 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -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" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 0710eeb22a..2bf571f1fc 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -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; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 9680123fd3..69bf166918 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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, ); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 85a61930ee..8ba21596d3 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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; diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 1a1ed7e4ce..7615a3f431 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot.c @@ -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) diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index 7d352b18dc..726003b686 100644 --- a/src/main/flight/autopilot.h +++ b/src/main/flight/autopilot.h @@ -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); diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 73e1045bd7..e469b4c13c 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -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; } } diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index 13092c09fe..5c1bc8e543 100644 --- a/src/main/flight/pos_hold.h +++ b/src/main/flight/pos_hold.h @@ -28,7 +28,7 @@ typedef struct { bool isPosHoldActive; gpsLocation_t targetLocation; - float targetAdjustRate; + float deadband; } posHoldState_t; void posHoldInit(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 60690be91b..3ae3bd9b0d 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()->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; diff --git a/src/main/pg/pos_hold.c b/src/main/pg/pos_hold.c index a38bee39b3..efc156656a 100644 --- a/src/main/pg/pos_hold.c +++ b/src/main/pg/pos_hold.c @@ -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 diff --git a/src/main/pg/pos_hold.h b/src/main/pg/pos_hold.h index ba5f6629cb..dac6a3c6de 100644 --- a/src/main/pg/pos_hold.h +++ b/src/main/pg/pos_hold.h @@ -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);