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

Fixed mixer for 3D / Dshot. (#4161)

Browse source
This commit is contained in:
Michael Keller 2017-09-18 02:45:39 +12:00 • committed by Martin Budden
parent 4f2bea6a84
commit ce824a60d6
4 changed files with 85 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -318,9 +318,6 @@ typedef struct blackboxSlowState_s {
bool rxFlightChannelsValid; bool rxFlightChannelsValid;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From mixer.c:
extern float motorOutputHigh, motorOutputLow;
//From rc_controls.c //From rc_controls.c
extern boxBitmask_t rcModeActivationMask; extern boxBitmask_t rcModeActivationMask;

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

@ -23,6 +23,7 @@
#include "platform.h" #include "platform.h"
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
@ -313,8 +314,11 @@ const mixer_t mixers[] = {
}; };
#endif // !USE_QUAD_MIXER_ONLY #endif // !USE_QUAD_MIXER_ONLY
static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
float motorOutputHigh, motorOutputLow; float motorOutputHigh, motorOutputLow;
static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
static uint16_t rcCommand3dDeadBandLow;
static uint16_t rcCommand3dDeadBandHigh;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount(void) uint8_t getMotorCount(void)
@ -357,7 +361,9 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range // DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) { void initEscEndpoints(void)
{
// Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
switch (motorConfig()->dev.motorPwmProtocol) { switch (motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT #ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000: case PWM_TYPE_PROSHOT1000:
@ -366,10 +372,11 @@ void initEscEndpoints(void) {
case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150: case PWM_TYPE_DSHOT150:
disarmMotorOutput = DSHOT_DISARM_COMMAND; disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D)) if (feature(FEATURE_3D)) {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
else } else {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
}
motorOutputHigh = DSHOT_MAX_THROTTLE; motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
@ -377,8 +384,13 @@ void initEscEndpoints(void) {
break; break;
#endif #endif
default: default:
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; if (feature(FEATURE_3D)) {
motorOutputLow = motorConfig()->minthrottle; disarmMotorOutput = flight3DConfig()->neutral3d;
motorOutputLow = PWM_RANGE_MIN;
} else {
disarmMotorOutput = motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
}
motorOutputHigh = motorConfig()->maxthrottle; motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low; deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
@ -386,9 +398,13 @@ void initEscEndpoints(void) {
break; break;
} }
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck;
rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
} }
void mixerInit(mixerMode_e mixerMode) void mixerInit(mixerMode_e mixerMode)
@ -515,53 +531,82 @@ void stopPwmAllMotors(void)
delayMicroseconds(1500); delayMicroseconds(1500);
} }
float throttle = 0; static float throttle = 0;
float motorOutputMin, motorOutputMax; static float motorOutputMin;
bool mixerInversion = false; static float motorRangeMin;
float motorOutputRange; static float motorRangeMax;
static float motorOutputRange;
static int8_t motorOutputMixSign;
void calculateThrottleAndCurrentMotorEndpoints(void) void calculateThrottleAndCurrentMotorEndpoints(void)
{ {
static uint16_t throttlePrevious = 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)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if (!ARMING_FLAG(ARMED)) {
rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
}
if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) {
motorOutputMax = deadbandMotor3dLow; // INVERTED
motorOutputMin = motorOutputLow; motorRangeMin = motorOutputLow;
throttlePrevious = rcCommand[THROTTLE]; //3D Mode Throttle Fix #3696 motorRangeMax = deadbandMotor3dLow;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696 if(isMotorProtocolDshot()) {
motorOutputMin = motorOutputLow;
motorOutputRange = deadbandMotor3dLow - motorOutputLow;
} else {
motorOutputMin = deadbandMotor3dLow;
motorOutputRange = motorOutputLow - deadbandMotor3dLow;
}
motorOutputMixSign = -1;
rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
currentThrottleInputRange = rcCommandThrottleRange3dLow; currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true; } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
} else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // NORMAL
motorOutputMax = motorOutputHigh; motorRangeMin = deadbandMotor3dHigh;
motorRangeMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh; motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE]; motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; motorOutputMixSign = 1;
rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
currentThrottleInputRange = rcCommandThrottleRange3dHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) {
motorOutputMax = deadbandMotor3dLow; // INVERTED_TO_DEADBAND
motorOutputMin = motorOutputLow; motorRangeMin = motorOutputLow;
throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; motorRangeMax = deadbandMotor3dLow;
if(isMotorProtocolDshot()) {
motorOutputMin = motorOutputLow;
motorOutputRange = deadbandMotor3dLow - motorOutputLow;
} else {
motorOutputMin = deadbandMotor3dLow;
motorOutputRange = motorOutputLow - deadbandMotor3dLow;
}
motorOutputMixSign = -1;
throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dLow; currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else { } else {
motorOutputMax = motorOutputHigh; // NORMAL_TO_DEADBAND
motorRangeMin = deadbandMotor3dHigh;
motorRangeMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh; motorOutputMin = deadbandMotor3dHigh;
motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
motorOutputMixSign = 1;
throttle = 0; throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} }
} else { } else {
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange; currentThrottleInputRange = rcCommandThrottleRange;
motorRangeMin = motorOutputLow;
motorRangeMax = motorOutputHigh;
motorOutputMin = motorOutputLow; motorOutputMin = motorOutputLow;
motorOutputMax = motorOutputHigh; motorOutputRange = motorOutputHigh - motorOutputLow;
} }
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
motorOutputRange = motorOutputMax - motorOutputMin;
} }
static void applyFlipOverAfterCrashModeToMotors(void) static void applyFlipOverAfterCrashModeToMotors(void)
@ -597,21 +642,16 @@ 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++) {
float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)); float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
// Dshot works exactly opposite in lower 3D section.
if (mixerInversion) {
motorOutput = motorOutputMin + (motorOutputMax - motorOutput);
}
if (failsafeIsActive()) { if (failsafeIsActive()) {
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
} }
motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax); motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
} else { } else {
motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax); motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
} }
// Motor stop handling // Motor stop handling
@ -637,6 +677,7 @@ void mixTable(uint8_t vbatPidCompensation)
applyFlipOverAfterCrashModeToMotors(); applyFlipOverAfterCrashModeToMotors();
return; return;
} }
// 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();

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

@ -105,6 +105,7 @@ PG_DECLARE(motorConfig_t, motorConfig);
extern const mixer_t mixers[]; extern const mixer_t mixers[];
extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor[MAX_SUPPORTED_MOTORS];
extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
extern float motorOutputHigh, motorOutputLow;
struct rxConfig_s; struct rxConfig_s;
uint8_t getMotorCount(void); uint8_t getMotorCount(void);

2
src/test/unit/blackbox_unittest.cc
View file

@ -365,7 +365,7 @@ int32_t GPS_home[2];
gyro_t gyro; gyro_t gyro;
uint16_t motorOutputHigh, motorOutputLow; float motorOutputHigh, motorOutputLow;
float motor_disarmed[MAX_SUPPORTED_MOTORS]; float motor_disarmed[MAX_SUPPORTED_MOTORS];
struct pidProfile_s; struct pidProfile_s;
struct pidProfile_s *currentPidProfile; struct pidProfile_s *currentPidProfile;