diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c47a9be093..4b9b521fdf 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -106,7 +106,6 @@ enum { int16_t magHold; #endif -static bool reverseMotors = false; static bool flipOverAfterCrashMode = false; static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero @@ -728,10 +727,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } } -bool isMotorsReversed(void) -{ - return reverseMotors; -} bool isFlipOverAfterCrashMode(void) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 95a8739014..55bc350914 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -47,5 +47,4 @@ void updateArmingStatus(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -bool isMotorsReversed(void); bool isFlipOverAfterCrashMode(void); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 142fb3ad01..0c9dca5256 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -79,6 +79,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH", 35 }, { BOXPREARM, "PREARM", 36 }, { BOXBEEPGPSCOUNT, "BEEP GPS SATELLITE COUNT", 37 }, + { BOX3DONASWITCH, "3D ON A SWITCH", 38 }, + }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index @@ -216,6 +218,7 @@ void initActiveBoxIds(void) if (feature(FEATURE_3D)) { BME(BOX3DDISABLE); + BME(BOX3DONASWITCH); } if (isMotorProtocolDshot()) { diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index bdd23bb3da..6e76055afe 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -49,6 +49,7 @@ static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; +static bool reverseMotors = false; float getSetpointRate(int axis) { @@ -327,6 +328,18 @@ void updateRcCommands(void) rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); } + if (feature(FEATURE_3D) && isModeActivationConditionPresent(BOX3DONASWITCH) && !failsafeIsActive()) { + if (IS_RC_MODE_ACTIVE(BOX3DONASWITCH)) { + reverseMotors = true; + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc - qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } + else { + reverseMotors = false; + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } + } if (FLIGHT_MODE(HEADFREE_MODE)) { static t_fp_vector_def rcCommandBuff; @@ -351,3 +364,8 @@ void resetYawAxis(void) rcCommand[YAW] = 0; setpointRate[YAW] = 0; } + +bool isMotorsReversed(void) +{ + return reverseMotors; +} diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 7c2f378645..c4b47a83c1 100755 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -24,3 +24,5 @@ float getThrottlePIDAttenuation(void); void updateRcCommands(void); void resetYawAxis(void); void generateThrottleCurve(void); +bool isMotorsReversed(void); + diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 5e39160be9..1f7123a7d2 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -108,9 +108,13 @@ bool areSticksInApModePosition(uint16_t ap_mode) throttleStatus_e calculateThrottleStatus(void) { - if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)) { - if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) + if (feature(FEATURE_3D)) { + if (IS_RC_MODE_ACTIVE(BOX3DDISABLE) || isModeActivationConditionPresent(BOX3DONASWITCH)) { + if (rcData[THROTTLE] < rxConfig()->mincheck) + return THROTTLE_LOW; + } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) { return THROTTLE_LOW; + } } else { if (rcData[THROTTLE] < rxConfig()->mincheck) return THROTTLE_LOW; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 191bb5e849..d4455e08fa 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -60,6 +60,7 @@ typedef enum { BOXFLIPOVERAFTERCRASH, BOXPREARM, BOXBEEPGPSCOUNT, + BOX3DONASWITCH, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1699016809..3a3246eefd 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -559,7 +559,9 @@ void calculateThrottleAndCurrentMotorEndpoints(void) rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) { + } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow && + !isModeActivationConditionPresent(BOX3DONASWITCH)) || + isMotorsReversed()) { // INVERTED_TO_DEADBAND motorRangeMin = motorOutputLow; motorRangeMax = deadbandMotor3dLow; @@ -676,11 +678,6 @@ void mixTable(uint8_t vbatPidCompensation) constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; float scaledAxisPidYaw = constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; - if (isMotorsReversed()) { - scaledAxisPidRoll = -scaledAxisPidRoll; - scaledAxisPidPitch = -scaledAxisPidPitch; - scaledAxisPidYaw = -scaledAxisPidYaw; - } if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; }