Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 13:55:18 +03:00
Code Issues Wiki Activity

Improved mixTable calculation efficiency

Browse source
This commit is contained in:
Martin Budden 2017-08-17 07:54:08 +01:00
parent e058fe697c
commit e4975d735d
1 changed files with 18 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -543,7 +543,8 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
motorOutputRange = motorOutputMax - motorOutputMin; motorOutputRange = motorOutputMax - motorOutputMin;
} }
void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
{
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // 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. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (uint32_t i = 0; i < motorCount; i++) { for (uint32_t i = 0; i < motorCount; i++) {
@ -586,33 +587,35 @@ void mixTable(uint8_t vbatPidCompensation)
// Find min and max throttle based on conditions. Throttle has to be known before mixing // Find min and max throttle based on conditions. Throttle has to be known before mixing
calculateThrottleAndCurrentMotorEndpoints(); calculateThrottleAndCurrentMotorEndpoints();
float motorMix[MAX_SUPPORTED_MOTORS];
// Calculate and Limit the PIDsum // Calculate and Limit the PIDsum
const float scaledAxisPidRoll = float scaledAxisPidRoll =
constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
const float scaledAxisPidPitch = float scaledAxisPidPitch =
constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
const float scaledAxisPidYaw = float scaledAxisPidYaw =
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw); constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw);
if (isMotorsReversed()) {
scaledAxisPidRoll = -scaledAxisPidRoll;
scaledAxisPidPitch = -scaledAxisPidPitch;
scaledAxisPidYaw = -scaledAxisPidYaw;
}
if (mixerConfig()->yaw_motors_reversed) {
scaledAxisPidYaw = -scaledAxisPidYaw;
}
// Calculate voltage compensation // Calculate voltage compensation
const float vbatCompensationFactor = (vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; const float vbatCompensationFactor = (vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output // Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0; float motorMixMax = 0, motorMixMin = 0;
const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed);
int motorDirection = GET_DIRECTION(isMotorsReversed());
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
float mix = float mix =
scaledAxisPidRoll * currentMixer[i].roll * (motorDirection) + scaledAxisPidRoll * currentMixer[i].roll +
scaledAxisPidPitch * currentMixer[i].pitch * (motorDirection) + scaledAxisPidPitch * currentMixer[i].pitch +
scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection) * (motorDirection); scaledAxisPidYaw * currentMixer[i].yaw;
if (vbatCompensationFactor > 1.0f) { mix *= vbatCompensationFactor; // Add voltage compensation
mix *= vbatCompensationFactor; // Add voltage compensation
}
if (mix > motorMixMax) { if (mix > motorMixMax) {
motorMixMax = mix; motorMixMax = mix;