diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b81e509f8..a82dcdea8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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]; } diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ed58b2529..bb5f8d323 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -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) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 9d94a1d07..4966f38ab 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -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); diff --git a/src/main/drivers/pwm_output_hal.c b/src/main/drivers/pwm_output_hal.c index 17e2c9c81..4d530648b 100644 --- a/src/main/drivers/pwm_output_hal.c +++ b/src/main/drivers/pwm_output_hal.c @@ -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) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 07b12681a..37a6b6c50 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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))); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index e8ecca09d..da4c04c15 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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]; diff --git a/src/main/main.c b/src/main/main.c index eb67fe857..74f874deb 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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);