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

3d on a mode switch

Browse source
This commit is contained in:
Bryce Johnson 2017-09-24 19:56:46 -05:00
parent 4e325d9290
commit d1a197f99e
8 changed files with 33 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/fc/fc_core.c
View file

@ -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)
{

1
src/main/fc/fc_core.h
View file

@ -47,5 +47,4 @@ void updateArmingStatus(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
bool isMotorsReversed(void);
bool isFlipOverAfterCrashMode(void);

3
src/main/fc/fc_msp_box.c
View file

@ -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()) {

18
src/main/fc/fc_rc.c
View file

@ -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;
}

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

@ -24,3 +24,5 @@ float getThrottlePIDAttenuation(void);
void updateRcCommands(void);
void resetYawAxis(void);
void generateThrottleCurve(void);
bool isMotorsReversed(void);

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

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

1
src/main/fc/rc_modes.h
View file

@ -60,6 +60,7 @@ typedef enum {
BOXFLIPOVERAFTERCRASH,
BOXPREARM,
BOXBEEPGPSCOUNT,
BOX3DONASWITCH,
CHECKBOX_ITEM_COUNT
} boxId_e;

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

@ -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;
}