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

Merge pull request #1828 from martinbudden/bf_mixer_tidy 

Tidy of mixer code. Remove motorCount extern
This commit is contained in:
J Blackman 2016-12-21 21:35:08 +11:00 committed by GitHub
parent c2af22c4f1 99760469d8
commit 684660b584
7 changed files with 79 additions and 74 deletions
Show Stats 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];
}

17
src/main/drivers/pwm_output.c
View File

@ -28,6 +28,7 @@
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
static pwmWriteFuncPtr pwmWritePtr;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
@ -35,7 +36,7 @@ static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif
bool pwmMotorsEnabled = true;
bool pwmMotorsEnabled = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -101,9 +102,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) {
motors[index].pwmWritePtr(index, value);
}
pwmWritePtr(index, value);
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@ -127,6 +126,11 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true;
}
bool pwmAreMotorsEnabled(void)
{
return pwmMotorsEnabled;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
@ -157,7 +161,6 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
uint32_t timerMhzCounter = 0;
pwmWriteFuncPtr pwmWritePtr;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
@ -191,6 +194,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDigital;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
@ -220,7 +224,6 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
motors[motorIndex].enabled = true;
continue;
}
@ -229,7 +232,6 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
motors[motorIndex].pwmWritePtr = pwmWritePtr;
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
@ -238,6 +240,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
}
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
}
pwmOutputPort_t *pwmGetMotors(void)

2
src/main/drivers/pwm_output.h
View File

@ -88,7 +88,6 @@ typedef struct {
volatile timCCR_t *ccr;
TIM_TypeDef *tim;
uint16_t period;
pwmWriteFuncPtr pwmWritePtr;
bool enabled;
IO_t io;
} pwmOutputPort_t;
@ -114,3 +113,4 @@ pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
void pwmDisableMotors(void);
void pwmEnableMotors(void);
bool pwmAreMotorsEnabled(void);

17
src/main/drivers/pwm_output_hal.c
View File

@ -28,6 +28,7 @@
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
static pwmWriteFuncPtr pwmWritePtr;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
@ -35,7 +36,7 @@ static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif
bool pwmMotorsEnabled = true;
bool pwmMotorsEnabled = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -113,9 +114,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) {
motors[index].pwmWritePtr(index, value);
}
pwmWritePtr(index, value);
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@ -136,6 +135,11 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true;
}
bool pwmAreMotorsEnabled(void)
{
return pwmMotorsEnabled;
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
@ -166,7 +170,6 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
uint32_t timerMhzCounter;
pwmWriteFuncPtr pwmWritePtr;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
@ -200,6 +203,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDigital;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
@ -227,7 +231,6 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
motors[motorIndex].enabled = true;
continue;
}
@ -238,7 +241,6 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
//IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
motors[motorIndex].pwmWritePtr = pwmWritePtr;
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse);
@ -247,6 +249,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
}
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
}
pwmOutputPort_t *pwmGetMotors(void)

91
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,18 +346,18 @@ 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
@ -363,7 +365,7 @@ 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,16 +375,18 @@ 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++) {
pwmWriteMotor(i, motor[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();
// Find roll/pitch/yaw desired output
for (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;
if (vbatCompensationFactor > 1.0f) motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (motorMix[i] > motorMixMax) motorMixMax = motorMix[i];
if (motorMix[i] < motorMixMin) motorMixMin = motorMix[i];
}
motorMixRange = motorMixMax - motorMixMin;
// Calculate voltage compensation
const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output
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 +
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];
} 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);