Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Tidy of mixer code. Remove motorCount extern

Browse source
This commit is contained in:
Martin Budden 2016-12-14 18:36:55 +00:00
parent 1475138e75
commit ae1944b78c
7 changed files with 71 additions and 61 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/blackbox/blackbox.c
View file

@ -289,9 +289,6 @@ typedef struct blackboxSlowState_s {
bool rxFlightChannelsValid;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From mixer.c:
extern uint8_t motorCount;
//From rc_controls.c
extern uint32_t rcModeActivationMask;
@ -367,7 +364,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
return getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI;
@ -472,7 +469,6 @@ static void blackboxSetState(BlackboxState newState)
static void writeIntraframe(void)
{
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int x;
blackboxWrite('I');
@ -483,7 +479,7 @@ static void writeIntraframe(void)
blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
// Don't bother writing the current D term if the corresponding PID setting is zero
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
}
@ -543,7 +539,8 @@ static void writeIntraframe(void)
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
for (x = 1; x < motorCount; x++) {
const int motorCount = getMotorCount();
for (int x = 1; x < motorCount; x++) {
blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
}
@ -667,7 +664,7 @@ static void writeInterframe(void)
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), 4);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), motorCount);
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount());
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
@ -1002,6 +999,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->debug[i] = debug[i];
}
const int motorCount = getMotorCount();
for (i = 0; i < motorCount; i++) {
blackboxCurrent->motor[i] = motor[i];
}

7
src/main/drivers/pwm_output.c
View file

@ -101,7 +101,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value)
void pwmWriteMotor(uint8_t index, uint16_t value)
{
if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
if (motors[index].pwmWritePtr) {
motors[index].pwmWritePtr(index, value);
}
}
@ -127,6 +127,11 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true;
}
bool pwmAreMotorsEnabled(void)
{
return pwmMotorsEnabled;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {

1
src/main/drivers/pwm_output.h
View file

@ -114,3 +114,4 @@ pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
void pwmDisableMotors(void);
void pwmEnableMotors(void);
bool pwmAreMotorsEnabled(void);

7
src/main/drivers/pwm_output_hal.c
View file

@ -113,7 +113,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value)
void pwmWriteMotor(uint8_t index, uint16_t value)
{
if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
if (motors[index].pwmWritePtr) {
motors[index].pwmWritePtr(index, value);
}
}
@ -136,6 +136,11 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true;
}
bool pwmAreMotorsEnabled(void)
{
return pwmMotorsEnabled;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {

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

@ -55,7 +55,7 @@
#define EXTERNAL_CONVERSION_MIN_VALUE 1000
#define EXTERNAL_CONVERSION_MAX_VALUE 2000
uint8_t motorCount;
static uint8_t motorCount;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@ -239,7 +239,8 @@ static motorMixer_t *customMixers;
static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount() {
uint8_t getMotorCount()
{
return motorCount;
}
@ -306,13 +307,11 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
void mixerConfigureOutput(void)
{
int i;
motorCount = 0;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (customMixers[i].throttle == 0.0f)
break;
@ -321,9 +320,12 @@ void mixerConfigureOutput(void)
}
} else {
motorCount = mixers[currentMixerMode].motorCount;
if (motorCount > MAX_SUPPORTED_MOTORS) {
motorCount = MAX_SUPPORTED_MOTORS;
}
// copy motor-based mixers
if (mixers[currentMixerMode].motor) {
for (i = 0; i < motorCount; i++)
for (int i = 0; i < motorCount; i++)
currentMixer[i] = mixers[currentMixerMode].motor[i];
}
}
@ -331,7 +333,7 @@ void mixerConfigureOutput(void)
// in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) {
if (motorCount > 1) {
for (i = 0; i < motorCount; i++) {
for (int i = 0; i < motorCount; i++) {
currentMixer[i].pitch *= 0.5f;
currentMixer[i].roll *= 0.5f;
currentMixer[i].yaw *= 0.5f;
@ -344,26 +346,26 @@ void mixerConfigureOutput(void)
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
int i;
// we're 1-based
index++;
// clear existing
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMixers[i].throttle = 0.0f;
}
// do we have anything here to begin with?
if (mixers[index].motor != NULL) {
for (i = 0; i < mixers[index].motorCount; i++)
for (int i = 0; i < mixers[index].motorCount; i++) {
customMixers[i] = mixers[index].motor[i];
}
}
}
#else
void mixerConfigureOutput(void)
{
motorCount = QUAD_MOTOR_COUNT;
for (uint8_t i = 0; i < motorCount; i++) {
for (int i = 0; i < motorCount; i++) {
currentMixer[i] = mixerQuadX[i];
}
@ -373,17 +375,19 @@ void mixerConfigureOutput(void)
void mixerResetDisarmedMotors(void)
{
int i;
// set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motor_disarmed[i] = disarmMotorOutput;
}
}
void writeMotors(void)
{
for (uint8_t i = 0; i < motorCount; i++) {
if (pwmAreMotorsEnabled()) {
for (int i = 0; i < motorCount; i++) {
pwmWriteMotor(i, motor[i]);
}
}
pwmCompleteMotorUpdate(motorCount);
}
@ -391,7 +395,7 @@ void writeMotors(void)
static void writeAllMotors(int16_t mc)
{
// Sends commands to all motors
for (uint8_t i = 0; i < motorCount; i++) {
for (int i = 0; i < motorCount; i++) {
motor[i] = mc;
}
@ -413,13 +417,9 @@ void stopPwmAllMotors(void)
void mixTable(pidProfile_t *pidProfile)
{
uint32_t i = 0;
float vbatCompensationFactor = 1;
// Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
float motorMix[MAX_SUPPORTED_MOTORS];
float motorOutputRange = 0, throttle = 0, currentThrottleInputRange = 0, motorMixRange = 0, motorMixMax = 0, motorMixMin = 0;
float throttle = 0, currentThrottleInputRange = 0;
uint16_t motorOutputMin, motorOutputMax;
static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
bool mixerInversion = false;
@ -461,33 +461,41 @@ void mixTable(pidProfile_t *pidProfile)
}
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
motorOutputRange = motorOutputMax - motorOutputMin;
const float motorOutputRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3];
// Limit the PIDsum
for (int axis = 0; axis < 3; axis++)
for (int axis = 0; axis < 3; axis++) {
scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
}
// Calculate voltage compensation
if (batteryConfig && pidProfile->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation();
const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0;
for (int i = 0; i < motorCount; i++) {
motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * scaledAxisPIDf[YAW] * currentMixer[i].yaw;
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i];
if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i];
if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
}
motorMixRange = motorMixMax - motorMixMin;
if (motorMix[i] > motorMixMax) {
motorMixMax = motorMix[i];
} else if (motorMix[i] < motorMixMin) {
motorMixMin = motorMix[i];
}
}
const float motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) {
for (i = 0; i < motorCount; i++) {
for (int i = 0; i < motorCount; i++) {
motorMix[i] /= motorMixRange;
}
// Get the maximum correction by setting offset to center
@ -499,6 +507,7 @@ void mixTable(pidProfile_t *pidProfile)
// 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.
uint32_t i = 0;
for (i = 0; i < motorCount; i++) {
motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));

1
src/main/flight/mixer.h
View file

@ -110,7 +110,6 @@ typedef struct airplaneConfig_s {
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
extern uint8_t motorCount;
extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];

11
src/main/main.c
View file

@ -271,17 +271,10 @@ void init(void)
}
mixerConfigureOutput();
motorInit(&masterConfig.motorConfig, idlePulse, getMotorCount());
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
#ifdef USE_QUAD_MIXER_ONLY
motorInit(motorConfig(), idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(motorConfig(), idlePulse, motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);