diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index b029d14f19..a904ccef34 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -715,16 +715,7 @@ FAST_CODE void processRcCommand(void) if (axis == FD_YAW) { rcCommandf = rcCommand[axis] / rcCommandYawDivider; } else { -#ifdef USE_POS_HOLD_MODE - if (FLIGHT_MODE(POS_HOLD_MODE)) { - // attenuate rcCommand by the deadband percentage - rcCommandf = rcCommand[axis] / (500.0f - rcControlsConfig()->pos_hold_deadband * 5.0f); - rcCommandf *= 0.7f; // attenuate overall stick responsiveness to 70% of normal - } else -#endif - { - rcCommandf = rcCommand[axis] / rcCommandDivider; - } + rcCommandf = rcCommand[axis] / rcCommandDivider; } rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = fabsf(rcCommandf); @@ -760,12 +751,24 @@ FAST_CODE_NOINLINE void updateRcCommands(void) isRxDataNew = true; for (int axis = 0; axis < 3; axis++) { + float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); // -500 to 500 - float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); - // larger deadband on pitch and roll when in position hold - // mulitply pos_hold_deadband percent by 5.0 to get a range where 100% would be 500 - 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) { +#ifdef USE_POS_HOLD_MODE + float tmpDeadband = rcControlsConfig()->deadband; + if (FLIGHT_MODE(POS_HOLD_MODE)) { + if (rcControlsConfig()->pos_hold_deadband) { + // if pos_hold_deadband is defined, ignore pitch & roll within deadband zone + tmpDeadband = rcControlsConfig()->pos_hold_deadband * 5.0f; + // NB could attenuate RP responsiveness outside deadband here, with tmp * 0.8f or whatever + } else { + // if pos_hold_deadband is zero, prevent user adjustment of pitch or roll + tmp = 0; + } + } +#else + const float tmpDeadband = rcControlsConfig()->deadband; +#endif if (tmp > tmpDeadband) { tmp -= tmpDeadband; } else { @@ -850,7 +853,16 @@ bool isMotorsReversed(void) void initRcProcessing(void) { +#ifdef USE_POS_HOLD_MODE + if (FLIGHT_MODE(POS_HOLD_MODE)) { + if (rcControlsConfig()->pos_hold_deadband) { + rcCommandDivider = 500.0f - rcControlsConfig()->pos_hold_deadband * 5.0f; // pos hold deadband in percent + } + } +#else rcCommandDivider = 500.0f - rcControlsConfig()->deadband; +#endif + rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband; for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 13b5d70b69..0eb1e2e263 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot.c @@ -173,7 +173,7 @@ void resetPositionControl(gpsLocation_t initialTargetLocation) { resetPositionControlParams(); } -bool positionControl(float deadband) { +bool positionControl(bool useStickAdjustment, float deadband) { // exit if we don't have suitable data if (!STATE(GPS_FIX)) { @@ -327,9 +327,9 @@ bool positionControl(float deadband) { // or in pid.c, when angle rate is calculated // if done in pid.c, the same upsampler could be used for GPS and PosHold. - // if sticks are centered, allow autopilot control, otherwise pilot can fly like a mavic - posHoldAngle[AI_ROLL] = (getRcDeflectionAbs(FD_ROLL) < deadband) ? pidSumRoll : 0.0f; - posHoldAngle[AI_PITCH] = (getRcDeflectionAbs(FD_PITCH) < deadband) ? pidSumPitch : 0.0f; + // if a deadband is set, and sticks are outside deadband, allow pilot control, otherwise hold position + posHoldAngle[AI_ROLL] = (useStickAdjustment && getRcDeflectionAbs(FD_ROLL) > deadband) ? 0.0f : pidSumRoll; + posHoldAngle[AI_PITCH] = (useStickAdjustment && getRcDeflectionAbs(FD_PITCH) > deadband) ? 0.0f : pidSumPitch; // note: // if FLIGHT_MODE(POS_HOLD_MODE): diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index 049bb5eac0..b9e942f098 100644 --- a/src/main/flight/autopilot.h +++ b/src/main/flight/autopilot.h @@ -28,7 +28,7 @@ void resetAltitudeControl(void); void resetPositionControl(gpsLocation_t initialTargetLocation); void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep); -bool positionControl(float deadband); +bool positionControl(bool useStickAdjustment, float deadband); bool isBelowLandingAltitude(void); float getAutopilotThrottle(void); diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 8b2d00fd19..fb0fa81083 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -83,7 +83,7 @@ void posHoldUpdate(void) posHoldUpdateTargetLocation(); if (getIsNewDataForPosHold() && posHold.posHoldIsOK) { - posHold.posHoldIsOK = positionControl(posHold.deadband); + posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband); } }