add deadbands

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_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_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_MIN, "%d",         autopilotConfig()->throttle_min);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d",           autopilotConfig()->throttle_max);
+        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_P, "%d",           autopilotConfig()->altitude_P);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d",   autopilotConfig()->altitude_I);;
+        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_D, "%d",           autopilotConfig()->altitude_D);;
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d",   autopilotConfig()->altitude_F);
+        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_P, "%d",           autopilotConfig()->position_P);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d",   autopilotConfig()->position_I);;
+        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_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

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

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"

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) {
+            if (tmp > tmpDeadband) {
-                tmp -= rcControlsConfig()->deadband;
+                tmp -= tmpDeadband;
             } else {
                 tmp = 0;
             }

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,
 );
 

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;
 

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 :-)
+    // if sticks are centered, allow pilot control, otherwise use stick control
-//    posHoldAngle[AI_PITCH] = 0.0f;
+    posHoldAngle[AI_ROLL] = (getRcDeflectionAbs(FD_ROLL) < deadband) ? rollSetpoint : 0.0f;
-//    posHoldAngle[AI_ROLL] = 0.0f;
+    posHoldAngle[AI_PITCH] = (getRcDeflectionAbs(FD_PITCH) < deadband) ? pitchSetpoint : 0.0f;
 }
 
 bool isBelowLandingAltitude(void)

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);

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;
+    posHold.targetLocation = gpsSol.llh;
-    posHoldState.targetAdjustRate = 0.0f;
 }
 
 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
+    // if failsafe is active, eg landing mode, don't update the original start point 
-    // We need to provide a big deadband in rc.c
-
     if (!failsafeIsActive()) {
-        const float deadband = 0.2f;
+        // otherwise if sticks are not centered, allow start point to be updated
-        if ((getRcDeflectionAbs(FD_ROLL) > deadband) || (getRcDeflectionAbs(FD_PITCH) > 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 (?)
             // while sticks are outside the deadband,
-            // set the target location to the current GPS location each iteration
+            // keep updating the home location to the current GPS location each iteration
-            posHoldState.targetLocation = gpsSol.llh;
+            posHold.targetLocation = gpsSol.llh;
         }
     }
 }
 
 void posHoldUpdate(void)
 {
-    // check if the user wants to change the target position using sticks
+    posHoldUpdateTargetLocation();
-    if (posHoldConfig()->pos_hold_adjust_rate) {
-        posHoldUpdateTargetLocation();
-    }
 
     if (getIsNewDataForPosHold()) {
-        positionControl(posHoldState.targetLocation);
+        positionControl(posHold.targetLocation, posHold.deadband);
-    } else {
     }
 }
 
@@ -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;
     }
 }
 

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);

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;
 

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

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);