diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aa90d16145..6943cb0eaa 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -69,6 +69,8 @@ static rxConfig_t *rxConfig; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +float rpy_limiting = 1.0f; + #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; @@ -749,41 +751,88 @@ void mixTable(void) axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } - // motors for non-servo mixes - for (i = 0; i < motorCount; i++) { - motor[i] = - rcCommand[THROTTLE] * currentMixer[i].throttle + - axisPID[PITCH] * currentMixer[i].pitch + - axisPID[ROLL] * currentMixer[i].roll + - -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; + if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { + // motors for non-servo mixes + for (i = 0; i < motorCount; i++) { + motor[i] = + rcCommand[THROTTLE] * currentMixer[i].throttle + + axisPID[PITCH] * currentMixer[i].pitch + + axisPID[ROLL] * currentMixer[i].roll + + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; + } + } else { + int16_t rpy[MAX_SUPPORTED_MOTORS]; + int16_t rpy_max = 0; // assumption: symetrical about zero. + int16_t rpy_min = 0; + + // Find roll/pitch/yaw desired output + for (i = 0; i < motorCount; i++) { + rpy[i] = + axisPID[PITCH] * currentMixer[i].pitch + + axisPID[ROLL] * currentMixer[i].roll + + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; + + if (rpy[i] > rpy_max) rpy_max = rpy[i]; + if (rpy[i] < rpy_min) rpy_min = rpy[i]; + } + + // Scale roll/pitch/yaw uniformly to fit within throttle range + int16_t rpy_range = rpy_max - rpy_min; + int16_t th_range = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle; + int16_t th_min, th_max; + + if (rpy_range > th_range) { + for (i = 0; i < motorCount; i++) { + rpy[i] = (rpy[i] * th_range) / rpy_range; + } + th_min = th_max = escAndServoConfig->minthrottle + (th_range / 2); + } else { + th_min = escAndServoConfig->minthrottle + (rpy_range / 2); + th_max = escAndServoConfig->maxthrottle - (rpy_range / 2); + } + + // Now add in the desired throttle, but keep in a range that doesn't clip adjusted + // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. + // + // TODO: handle the case when motors don't all get the same throttle factor... + // too lazy to sort out the math right now. + for (i = 0; i < motorCount; i++) { + motor[i] = rpy[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, th_min, th_max); + } + + // adjust feedback to scale PID error inputs to our limitations. + rpy_limiting = constrainf(((rpy_limiting * th_range) / rpy_range), 0.1f, 1.0f); } if (ARMING_FLAG(ARMED)) { bool isFailsafeActive = failsafeIsActive(); + int16_t maxThrottleDifference = 0; - // Find the maximum motor output. - int16_t maxMotor = motor[0]; - for (i = 1; i < motorCount; i++) { - // If one motor is above the maxthrottle threshold, we reduce the value - // of all motors by the amount of overshoot. That way, only one motor - // is at max and the relative power of each motor is preserved. - if (motor[i] > maxMotor) { - maxMotor = motor[i]; + if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { + // Find the maximum motor output. + int16_t maxMotor = motor[0]; + for (i = 1; i < motorCount; i++) { + // If one motor is above the maxthrottle threshold, we reduce the value + // of all motors by the amount of overshoot. That way, only one motor + // is at max and the relative power of each motor is preserved. + if (motor[i] > maxMotor) { + maxMotor = motor[i]; + } + } + + if (maxMotor > escAndServoConfig->maxthrottle) { + maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle; } } - - int16_t maxThrottleDifference = 0; - if (maxMotor > escAndServoConfig->maxthrottle) { - maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle; - } - for (i = 0; i < motorCount; i++) { - // this is a way to still have good gyro corrections if at least one motor reaches its max. - motor[i] -= maxThrottleDifference; + if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { + // this is a way to still have good gyro corrections if at least one motor reaches its max. + motor[i] -= maxThrottleDifference; + } if (feature(FEATURE_3D)) { - if ((IS_RC_MODE_ACTIVE(BOXIDLE_UP)) + if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) || rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle || rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) { if (rcData[THROTTLE] > rxConfig->midrc) { @@ -805,7 +854,7 @@ void mixTable(void) // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); - if (((rcData[THROTTLE]) < rxConfig->mincheck) && !(IS_RC_MODE_ACTIVE(BOXIDLE_UP))) { + if (((rcData[THROTTLE]) < rxConfig->mincheck) && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; } else { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a85a95bcbe..bbb4dcb5f4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -50,6 +50,7 @@ extern uint16_t cycleTime; extern uint8_t motorCount; extern float dT; +extern float rpy_limiting; int16_t axisPID[3]; @@ -170,6 +171,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + RateError = RateError * rpy_limiting; + } + // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; @@ -286,6 +291,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRateTmp - (gyroADC[axis] / 4); + if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + RateError = RateError * rpy_limiting; + } + // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index fe78f7fe5e..46ae9207ee 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -48,7 +48,7 @@ typedef enum { BOXSERVO3, BOXBLACKBOX, BOXFAILSAFE, - BOXIDLE_UP, + BOXAIRMODE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 05a09cb35e..d82a58fc89 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -356,7 +356,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXSERVO3, "SERVO3;", 25 }, { BOXBLACKBOX, "BLACKBOX;", 26 }, { BOXFAILSAFE, "FAILSAFE;", 27 }, - { BOXIDLE_UP, "IDLE UP;", 28 }, + { BOXAIRMODE, "AIR MODE;", 28 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -642,7 +642,7 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; - activeBoxIds[activeBoxIdCount++] = BOXIDLE_UP; + activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; @@ -837,7 +837,7 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXIDLE_UP)) << BOXIDLE_UP; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); if (flag) diff --git a/src/main/mw.c b/src/main/mw.c index acfbe93bcf..056352fd27 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -565,7 +565,7 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - if (throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXIDLE_UP))) { + if ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED))) { pidResetErrorAngle(); pidResetErrorGyro(); }