Merge pull request #10370 from borisbstyle/new_mixer
This commit is contained in:
commit
34e6a83f70
|
@ -501,6 +501,10 @@ static const char * const lookupTableOsdLogoOnArming[] = {
|
|||
};
|
||||
#endif
|
||||
|
||||
static const char* const lookupTableMixerType[] = {
|
||||
"LEGACY", "LINEAR", "DYNAMIC",
|
||||
};
|
||||
|
||||
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
|
||||
|
||||
const lookupTableEntry_t lookupTables[] = {
|
||||
|
@ -622,6 +626,7 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#ifdef USE_OSD
|
||||
LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming),
|
||||
#endif
|
||||
LOOKUP_TABLE_ENTRY(lookupTableMixerType),
|
||||
};
|
||||
|
||||
#undef LOOKUP_TABLE_ENTRY
|
||||
|
@ -907,6 +912,7 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
// PG_MIXER_CONFIG
|
||||
{ "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },
|
||||
{ "mixer_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) },
|
||||
{ "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) },
|
||||
{ "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) },
|
||||
|
||||
|
|
|
@ -141,6 +141,7 @@ typedef enum {
|
|||
#ifdef USE_OSD
|
||||
TABLE_OSD_LOGO_ON_ARMING,
|
||||
#endif
|
||||
TABLE_MIXER_TYPE,
|
||||
|
||||
LOOKUP_TABLE_COUNT
|
||||
} lookupTableIndex_e;
|
||||
|
|
|
@ -425,6 +425,51 @@ static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled) {
|
||||
const float motorMixNormalizationFactor = motorMixRange > 1.0f ? motorMixRange : 1.0f;
|
||||
const float motorMixDelta = 0.5f * motorMixRange;
|
||||
|
||||
for (int i = 0; i < mixerRuntime.motorCount; ++i) {
|
||||
if (airmodeEnabled || throttle > 0.5f) {
|
||||
if (mixerConfig()->mixer_type == MIXER_LINEAR) {
|
||||
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
|
||||
} else {
|
||||
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + ABS(motorMix[i]), motorMix[i] - ABS(motorMix[i]));
|
||||
}
|
||||
}
|
||||
motorMix[i] /= motorMixNormalizationFactor;
|
||||
}
|
||||
}
|
||||
|
||||
static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled) {
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
const float unadjustedThrottle = throttle;
|
||||
throttle += pidGetAirmodeThrottleOffset();
|
||||
float airmodeThrottleChange = 0;
|
||||
#endif
|
||||
|
||||
if (motorMixRange > 1.0f) {
|
||||
for (int i = 0; i < mixerRuntime.motorCount; i++) {
|
||||
motorMix[i] /= motorMixRange;
|
||||
}
|
||||
// Get the maximum correction by setting offset to center when airmode enabled
|
||||
if (airmodeEnabled) {
|
||||
throttle = 0.5f;
|
||||
}
|
||||
} else {
|
||||
if (airmodeEnabled || throttle > 0.5f) {
|
||||
throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
pidUpdateAirmodeLpf(airmodeThrottleChange);
|
||||
#endif
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
||||
{
|
||||
// Find min and max throttle based on conditions. Throttle has to be known before mixing
|
||||
|
@ -549,35 +594,13 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
const float unadjustedThrottle = throttle;
|
||||
throttle += pidGetAirmodeThrottleOffset();
|
||||
float airmodeThrottleChange = 0;
|
||||
#endif
|
||||
|
||||
// apply airmode
|
||||
motorMixRange = motorMixMax - motorMixMin;
|
||||
if (motorMixRange > 1.0f) {
|
||||
for (int i = 0; i < mixerRuntime.motorCount; i++) {
|
||||
motorMix[i] /= motorMixRange;
|
||||
}
|
||||
// Get the maximum correction by setting offset to center when airmode enabled
|
||||
if (airmodeEnabled) {
|
||||
throttle = 0.5f;
|
||||
}
|
||||
if (mixerConfig()->mixer_type > MIXER_LEGACY) {
|
||||
applyMixerAdjustmentLinear(motorMix, airmodeEnabled);
|
||||
} else {
|
||||
if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
|
||||
throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle;
|
||||
#endif
|
||||
}
|
||||
applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
|
||||
}
|
||||
|
||||
#ifdef USE_AIRMODE_LPF
|
||||
pidUpdateAirmodeLpf(airmodeThrottleChange);
|
||||
#endif
|
||||
|
||||
if (featureIsEnabled(FEATURE_MOTOR_STOP)
|
||||
&& ARMING_FLAG(ARMED)
|
||||
&& !mixerRuntime.feature3dEnabled
|
||||
|
|
|
@ -60,6 +60,13 @@ typedef enum mixerMode
|
|||
MIXER_QUADX_1234 = 26
|
||||
} mixerMode_e;
|
||||
|
||||
typedef enum mixerType
|
||||
{
|
||||
MIXER_LEGACY = 0,
|
||||
MIXER_LINEAR = 1,
|
||||
MIXER_DYNAMIC = 2,
|
||||
} mixerType_e;
|
||||
|
||||
// Custom mixer data per motor
|
||||
typedef struct motorMixer_s {
|
||||
float throttle;
|
||||
|
@ -82,6 +89,7 @@ typedef struct mixerConfig_s {
|
|||
bool yaw_motors_reversed;
|
||||
uint8_t crashflip_motor_percent;
|
||||
uint8_t crashflip_expo;
|
||||
uint8_t mixer_type;
|
||||
} mixerConfig_t;
|
||||
|
||||
PG_DECLARE(mixerConfig_t, mixerConfig);
|
||||
|
|
|
@ -50,7 +50,8 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
|||
.mixerMode = DEFAULT_MIXER,
|
||||
.yaw_motors_reversed = false,
|
||||
.crashflip_motor_percent = 0,
|
||||
.crashflip_expo = 35
|
||||
.crashflip_expo = 35,
|
||||
.mixer_type = MIXER_LEGACY,
|
||||
);
|
||||
|
||||
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
|
||||
|
|
Loading…
Reference in New Issue