rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tidy of mixer and servo code

This commit is contained in:
Martin Budden 2017-12-01 06:04:35 +00:00
parent ce345a8446
commit ccb4f77ae2
2 changed files with 28 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/main/flight/mixer.c
View File

@ -416,8 +416,9 @@ void mixerConfigureOutput(void)
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done // check if done
if (customMotorMixer(i)->throttle == 0.0f) if (customMotorMixer(i)->throttle == 0.0f) {
break; break;
}
currentMixer[i] = *customMotorMixer(i); currentMixer[i] = *customMotorMixer(i);
motorCount++; motorCount++;
} }
@ -455,7 +456,6 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMixers[i].throttle = 0.0f; customMixers[i].throttle = 0.0f;
} }
// do we have anything here to begin with? // do we have anything here to begin with?
if (mixers[index].motor != NULL) { if (mixers[index].motor != NULL) {
for (int i = 0; i < mixers[index].motorCount; i++) { for (int i = 0; i < mixers[index].motorCount; i++) {
@ -467,14 +467,12 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
void mixerConfigureOutput(void) void mixerConfigureOutput(void)
{ {
motorCount = QUAD_MOTOR_COUNT; motorCount = QUAD_MOTOR_COUNT;
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
currentMixer[i] = mixerQuadX[i]; currentMixer[i] = mixerQuadX[i];
} }
mixerResetDisarmedMotors(); mixerResetDisarmedMotors();
} }
#endif #endif // USE_QUAD_MIXER_ONLY
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
{ {
@ -500,14 +498,12 @@ static void writeAllMotors(int16_t mc)
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
motor[i] = mc; motor[i] = mc;
} }
writeMotors(); writeMotors();
} }
void stopMotors(void) void stopMotors(void)
{ {
writeAllMotors(disarmMotorOutput); writeAllMotors(disarmMotorOutput);
delay(50); // give the timers and ESCs a chance to react. delay(50); // give the timers and ESCs a chance to react.
} }
@ -524,21 +520,21 @@ static float motorRangeMax;
static float motorOutputRange; static float motorOutputRange;
static int8_t motorOutputMixSign; static int8_t motorOutputMixSign;
void calculateThrottleAndCurrentMotorEndpoints(void) static void calculateThrottleAndCurrentMotorEndpoints(void)
{ {
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
float currentThrottleInputRange = 0; float currentThrottleInputRange = 0;
if(feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
} }
if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) {
// INVERTED // INVERTED
motorRangeMin = motorOutputLow; motorRangeMin = motorOutputLow;
motorRangeMax = deadbandMotor3dLow; motorRangeMax = deadbandMotor3dLow;
if(isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
motorOutputMin = motorOutputLow; motorOutputMin = motorOutputLow;
motorOutputRange = deadbandMotor3dLow - motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow;
} else { } else {
@ -549,7 +545,7 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
rcThrottlePrevious = rcCommand[THROTTLE]; rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
currentThrottleInputRange = rcCommandThrottleRange3dLow; currentThrottleInputRange = rcCommandThrottleRange3dLow;
} else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
// NORMAL // NORMAL
motorRangeMin = deadbandMotor3dHigh; motorRangeMin = deadbandMotor3dHigh;
motorRangeMax = motorOutputHigh; motorRangeMax = motorOutputHigh;
@ -559,13 +555,13 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
rcThrottlePrevious = rcCommand[THROTTLE]; rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
currentThrottleInputRange = rcCommandThrottleRange3dHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if((rcThrottlePrevious <= rcCommand3dDeadBandLow && } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow &&
!isModeActivationConditionPresent(BOX3DONASWITCH)) || !isModeActivationConditionPresent(BOX3DONASWITCH)) ||
isMotorsReversed()) { isMotorsReversed()) {
// INVERTED_TO_DEADBAND // INVERTED_TO_DEADBAND
motorRangeMin = motorOutputLow; motorRangeMin = motorOutputLow;
motorRangeMax = deadbandMotor3dLow; motorRangeMax = deadbandMotor3dLow;
if(isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
motorOutputMin = motorOutputLow; motorOutputMin = motorOutputLow;
motorOutputRange = deadbandMotor3dLow - motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow;
} else { } else {
@ -602,21 +598,18 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
static void applyFlipOverAfterCrashModeToMotors(void) static void applyFlipOverAfterCrashModeToMotors(void)
{ {
float motorMix[MAX_SUPPORTED_MOTORS];
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
float motorMix[MAX_SUPPORTED_MOTORS];
for (int i = 0; i < motorCount; i++) { for (int i = 0; i < motorCount; i++) {
if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) {
motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1; motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1;
} else { } else {
motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1; motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1;
} }
// Apply the mix to motor endpoints // Apply the mix to motor endpoints
float motorOutput = motorOutputMin + motorOutputRange * motorMix[i]; float motorOutput = motorOutputMin + motorOutputRange * motorMix[i];
//Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND; motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND;
motor[i] = motorOutput; motor[i] = motorOutput;
} }
} else { } else {
@ -633,17 +626,14 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
// 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++) {
float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
if (failsafeIsActive()) { if (failsafeIsActive()) {
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
} }
motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
} else { } else {
motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
} }
// Motor stop handling // Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
@ -664,7 +654,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
void mixTable(uint8_t vbatPidCompensation) void mixTable(uint8_t vbatPidCompensation)
{ {
if (isFlipOverAfterCrashMode()) { if (isFlipOverAfterCrashMode()) {
applyFlipOverAfterCrashModeToMotors(); applyFlipOverAfterCrashModeToMotors();
return; return;
} }
@ -672,9 +662,9 @@ void mixTable(uint8_t vbatPidCompensation)
calculateThrottleAndCurrentMotorEndpoints(); calculateThrottleAndCurrentMotorEndpoints();
// Calculate and Limit the PIDsum // Calculate and Limit the PIDsum
float scaledAxisPidRoll = const float scaledAxisPidRoll =
constrainf(axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; constrainf(axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
float scaledAxisPidPitch = const float scaledAxisPidPitch =
constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
float scaledAxisPidYaw = float scaledAxisPidYaw =
constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
@ -683,7 +673,7 @@ void mixTable(uint8_t vbatPidCompensation)
} }
// 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 motorMix[MAX_SUPPORTED_MOTORS];
@ -750,7 +740,6 @@ float convertExternalToMotor(uint16_t externalValue)
#endif #endif
default: default:
motorValue = externalValue; motorValue = externalValue;
break; break;
} }
@ -774,13 +763,11 @@ uint16_t convertMotorToExternal(float motorValue)
} else { } else {
externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX); externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
} }
break; break;
case false: case false:
#endif #endif
default: default:
externalValue = motorValue; externalValue = motorValue;
break; break;
} }

24
src/main/flight/servos.c
View File

@ -213,10 +213,11 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
int servoDirection(int servoIndex, int inputSource) int servoDirection(int servoIndex, int inputSource)
{ {
// determine the direction (reversed or not) from the direction bitfield of the servo // determine the direction (reversed or not) from the direction bitfield of the servo
if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) {
return -1; return -1;
else } else {
return 1; return 1;
}
} }
void servosInit(void) void servosInit(void)
@ -224,8 +225,9 @@ void servosInit(void)
// enable servos for mixes that require them. note, this shifts motor counts. // enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[currentMixerMode].useServo; useServo = mixers[currentMixerMode].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't // if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) {
useServo = 1; useServo = 1;
}
// give all servos a default command // give all servos a default command
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
@ -242,9 +244,9 @@ void loadCustomServoMixer(void)
// load custom mixer into currentServoMixer // load custom mixer into currentServoMixer
for (int i = 0; i < MAX_SERVO_RULES; i++) { for (int i = 0; i < MAX_SERVO_RULES; i++) {
// check if done // check if done
if (customServoMixers(i)->rate == 0) if (customServoMixers(i)->rate == 0) {
break; break;
}
currentServoMixer[i] = *customServoMixers(i); currentServoMixer[i] = *customServoMixers(i);
servoRuleCount++; servoRuleCount++;
} }
@ -266,13 +268,11 @@ void servoConfigureOutput(void)
currentMixerMode == MIXER_CUSTOM_AIRPLANE currentMixerMode == MIXER_CUSTOM_AIRPLANE
) { ) {
ENABLE_STATE(FIXED_WING); ENABLE_STATE(FIXED_WING);
if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
loadCustomServoMixer(); loadCustomServoMixer();
} }
} else { } else {
DISABLE_STATE(FIXED_WING); DISABLE_STATE(FIXED_WING);
if (currentMixerMode == MIXER_CUSTOM_TRI) { if (currentMixerMode == MIXER_CUSTOM_TRI) {
loadCustomServoMixer(); loadCustomServoMixer();
} }
@ -288,7 +288,6 @@ void servoMixerLoadMix(int index)
for (int i = 0; i < MAX_SERVO_RULES; i++) { for (int i = 0; i < MAX_SERVO_RULES; i++) {
customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = customServoMixersMutable(i)->box = 0; customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = customServoMixersMutable(i)->box = 0;
} }
for (int i = 0; i < servoMixers[index].servoRuleCount; i++) { for (int i = 0; i < servoMixers[index].servoRuleCount; i++) {
*customServoMixersMutable(i) = servoMixers[index].rule[i]; *customServoMixersMutable(i) = servoMixers[index].rule[i];
} }
@ -297,8 +296,8 @@ void servoMixerLoadMix(int index)
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
{ {
// start forwarding from this channel // start forwarding from this channel
uint8_t channelOffset = servoConfig()->channelForwardingStartChannel; int channelOffset = servoConfig()->channelForwardingStartChannel;
for (uint8_t servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { for (int servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
} }
} }
@ -329,7 +328,7 @@ void writeServos(void)
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RIGHT]); pwmWriteServo(servoIndex++, servo[SERVO_HELI_RIGHT]);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_TOP]); pwmWriteServo(servoIndex++, servo[SERVO_HELI_TOP]);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RUD]); pwmWriteServo(servoIndex++, servo[SERVO_HELI_RUD]);
break; break;
case MIXER_TRI: case MIXER_TRI:
case MIXER_CUSTOM_TRI: case MIXER_CUSTOM_TRI:
@ -535,7 +534,6 @@ static void filterServos(void)
#if defined(MIXER_DEBUG) #if defined(MIXER_DEBUG)
uint32_t startTime = micros(); uint32_t startTime = micros();
#endif #endif
if (servoConfig()->servo_lowpass_freq) { if (servoConfig()->servo_lowpass_freq) {
for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
@ -547,4 +545,4 @@ static void filterServos(void)
debug[0] = (int16_t)(micros() - startTime); debug[0] = (int16_t)(micros() - startTime);
#endif #endif
} }
#endif #endif // USE_SERVOS