From 1fcb206c46f513d2b4dba106c60a8ca211f70f52 Mon Sep 17 00:00:00 2001 From: tdogb Date: Fri, 10 Feb 2023 00:08:12 -0500 Subject: [PATCH] initial commit --- src/main/cli/settings.c | 23 ++ src/main/flight/mixer.c | 404 +++++++++++++++++++++-------------- src/main/flight/mixer.h | 10 + src/main/flight/mixer_init.c | 40 +++- src/main/flight/mixer_init.h | 17 +- 5 files changed, 330 insertions(+), 164 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f406524b72..5ca109b4bf 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -931,6 +931,29 @@ const clivalue_t valueTable[] = { { "beeper_dshot_beacon_tone", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {1, DSHOT_CMD_BEACON5 }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotBeaconTone) }, #endif #endif // USE_BEEPER +// 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) }, + { "rpm_limiter", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor) }, + { "rpm_limiter_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_p) }, + { "rpm_limiter_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_i) }, + { "rpm_limiter_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_d) }, + { "rpm_limiter_rpm_limit", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 999 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_rpm_limit) }, + { "rpm_limiter_acceleration_limit", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_acceleration_limit) }, + { "rpm_limiter_deceleration_limit", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_deceleration_limit) }, + //Street League customization + { "rpm_limiter_idle_rpm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_idle_rpm) }, + //{ "rpm_limiter_idle_rpm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 999 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, govenor_idle_rpm) }, + { "rpm_limiter_full_linearization", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_linearization) }, + + { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIDDLE, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_high) }, + { "3d_neutral", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, neutral3d) }, + { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 100 }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_throttle) }, + { "3d_limit_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, limit3d_low) }, + { "3d_limit_high", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIDDLE, PWM_PULSE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, limit3d_high) }, + { "3d_switched_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, switched_mode3d) }, // 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) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a02f78ee7b..f2fb18fab5 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -20,9 +20,7 @@ #include #include -#include #include -#include #include "platform.h" @@ -109,113 +107,114 @@ static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign; static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { - static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions - static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode + UNUSED(currentTimeUs); + // static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions + // static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode static float motorRangeMinIncrease = 0; float currentThrottleInputRange = 0; - if (mixerRuntime.feature3dEnabled) { - uint16_t rcCommand3dDeadBandLow; - uint16_t rcCommand3dDeadBandHigh; +// if (mixerRuntime.feature3dEnabled) { +// uint16_t rcCommand3dDeadBandLow; +// uint16_t rcCommand3dDeadBandHigh; - if (!ARMING_FLAG(ARMED)) { - rcThrottlePrevious = 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 (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) { - // The min_check range is halved because the output throttle is scaled to 500us. - // So by using half of min_check we maintain the same low-throttle deadband - // stick travel as normal non-3D mode. - const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2; - rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset; - rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset; - } else { - rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; - rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; - } +// if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) { +// // The min_check range is halved because the output throttle is scaled to 500us. +// // So by using half of min_check we maintain the same low-throttle deadband +// // stick travel as normal non-3D mode. +// const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2; +// rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset; +// rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset; +// } else { +// rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; +// rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; +// } - const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; - const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; +// const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; +// const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; - if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) { - // INVERTED - motorRangeMin = mixerRuntime.motorOutputLow; - motorRangeMax = mixerRuntime.deadbandMotor3dLow; -#ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - motorOutputMin = mixerRuntime.motorOutputLow; - motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; - } else -#endif - { - motorOutputMin = mixerRuntime.deadbandMotor3dLow; - motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; - } +// if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) { +// // INVERTED +// motorRangeMin = mixerRuntime.motorOutputLow; +// motorRangeMax = mixerRuntime.deadbandMotor3dLow; +// #ifdef USE_DSHOT +// if (isMotorProtocolDshot()) { +// motorOutputMin = mixerRuntime.motorOutputLow; +// motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; +// } else +// #endif +// { +// motorOutputMin = mixerRuntime.deadbandMotor3dLow; +// motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; +// } - if (motorOutputMixSign != -1) { - reversalTimeUs = currentTimeUs; - } - motorOutputMixSign = -1; +// if (motorOutputMixSign != -1) { +// reversalTimeUs = currentTimeUs; +// } +// motorOutputMixSign = -1; - rcThrottlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; - currentThrottleInputRange = rcCommandThrottleRange3dLow; - } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { - // NORMAL - motorRangeMin = mixerRuntime.deadbandMotor3dHigh; - motorRangeMax = mixerRuntime.motorOutputHigh; - motorOutputMin = mixerRuntime.deadbandMotor3dHigh; - motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; - if (motorOutputMixSign != 1) { - reversalTimeUs = currentTimeUs; - } - motorOutputMixSign = 1; - rcThrottlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; - currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow && - !flight3DConfigMutable()->switched_mode3d) || - isMotorsReversed()) { - // INVERTED_TO_DEADBAND - motorRangeMin = mixerRuntime.motorOutputLow; - motorRangeMax = mixerRuntime.deadbandMotor3dLow; +// rcThrottlePrevious = rcCommand[THROTTLE]; +// throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; +// currentThrottleInputRange = rcCommandThrottleRange3dLow; +// } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { +// // NORMAL +// motorRangeMin = mixerRuntime.deadbandMotor3dHigh; +// motorRangeMax = mixerRuntime.motorOutputHigh; +// motorOutputMin = mixerRuntime.deadbandMotor3dHigh; +// motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; +// if (motorOutputMixSign != 1) { +// reversalTimeUs = currentTimeUs; +// } +// motorOutputMixSign = 1; +// rcThrottlePrevious = rcCommand[THROTTLE]; +// throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; +// currentThrottleInputRange = rcCommandThrottleRange3dHigh; +// } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow && +// !flight3DConfigMutable()->switched_mode3d) || +// isMotorsReversed()) { +// // INVERTED_TO_DEADBAND +// motorRangeMin = mixerRuntime.motorOutputLow; +// motorRangeMax = mixerRuntime.deadbandMotor3dLow; -#ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - motorOutputMin = mixerRuntime.motorOutputLow; - motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; - } else -#endif - { - motorOutputMin = mixerRuntime.deadbandMotor3dLow; - motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; - } +// #ifdef USE_DSHOT +// if (isMotorProtocolDshot()) { +// motorOutputMin = mixerRuntime.motorOutputLow; +// motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; +// } else +// #endif +// { +// motorOutputMin = mixerRuntime.deadbandMotor3dLow; +// motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; +// } - if (motorOutputMixSign != -1) { - reversalTimeUs = currentTimeUs; - } - motorOutputMixSign = -1; +// if (motorOutputMixSign != -1) { +// reversalTimeUs = currentTimeUs; +// } +// motorOutputMixSign = -1; - throttle = 0; - currentThrottleInputRange = rcCommandThrottleRange3dLow; - } else { - // NORMAL_TO_DEADBAND - motorRangeMin = mixerRuntime.deadbandMotor3dHigh; - motorRangeMax = mixerRuntime.motorOutputHigh; - motorOutputMin = mixerRuntime.deadbandMotor3dHigh; - motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; - if (motorOutputMixSign != 1) { - reversalTimeUs = currentTimeUs; - } - motorOutputMixSign = 1; - throttle = 0; - currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } - if (currentTimeUs - reversalTimeUs < 250000) { - // keep iterm zero for 250ms after motor reversal - pidResetIterm(); - } - } else { +// throttle = 0; +// currentThrottleInputRange = rcCommandThrottleRange3dLow; +// } else { +// // NORMAL_TO_DEADBAND +// motorRangeMin = mixerRuntime.deadbandMotor3dHigh; +// motorRangeMax = mixerRuntime.motorOutputHigh; +// motorOutputMin = mixerRuntime.deadbandMotor3dHigh; +// motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; +// if (motorOutputMixSign != 1) { +// reversalTimeUs = currentTimeUs; +// } +// motorOutputMixSign = 1; +// throttle = 0; +// currentThrottleInputRange = rcCommandThrottleRange3dHigh; +// } +// if (currentTimeUs - reversalTimeUs < 250000) { +// // keep iterm zero for 250ms after motor reversal +// pidResetIterm(); +// } + // } else { throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; currentThrottleInputRange = PWM_RANGE; @@ -245,13 +244,13 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) float motorRangeAttenuationFactor = 0; // reduce motorRangeMax when battery is full - if (mixerRuntime.vbatSagCompensationFactor > 0.0f) { + if (!mixerConfig()->govenor && mixerRuntime.vbatSagCompensationFactor > 0.0f) { const uint16_t currentCellVoltage = getBatterySagCellVoltage(); // batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f); motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor; - DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100)); - DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000)); + DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100); + DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000); } motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); #else @@ -262,8 +261,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) motorOutputMin = motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin; motorOutputMixSign = 1; - } - + // } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); } @@ -346,17 +344,123 @@ static void applyFlipOverAfterCrashModeToMotors(void) } } +static void applyRPMLimiter(void) +{ + //Street League customization + float forcedRPMLimit = 130.0f; + //if (mixerConfig()->govenor && motorConfig()->dev.useDshotTelemetry) { + if (motorConfig()->dev.useDshotTelemetry) { + float RPM_GOVENOR_LIMIT = 0; + float averageRPM = 0; + float averageRPM_smoothed = 0; + float PIDOutput = 0; + float rcCommandThrottle = (rcCommand[THROTTLE]-1000)/1000.0f; + + //Street League customization + //if (mixerConfig()->rpm_linearization) { + if (true) { + //scales rpm setpoint between idle rpm and rpm limit based on throttle percent + //Street League customization + //RPM_GOVENOR_LIMIT = ((mixerConfig()->govenor_rpm_limit - mixerConfig()->govenor_idle_rpm))*100.0f*(rcCommandThrottle) + mixerConfig()->govenor_idle_rpm * 100.0f; + RPM_GOVENOR_LIMIT = ((forcedRPMLimit - mixerConfig()->govenor_idle_rpm))*100.0f*(rcCommandThrottle) + mixerConfig()->govenor_idle_rpm * 100.0f; + + //limit the speed with which the rpm setpoint can increase based on the rpm_limiter_acceleration_limit cli command + float acceleration = RPM_GOVENOR_LIMIT - mixerRuntime.govenorPreviousRPMLimit; + if(acceleration > 0) { + acceleration = MIN(acceleration, mixerRuntime.govenorAccelerationLimit); + RPM_GOVENOR_LIMIT = mixerRuntime.govenorPreviousRPMLimit + acceleration; + } + else if(acceleration < 0) { + acceleration = MAX(acceleration, -mixerRuntime.govenorDecelerationLimit); + RPM_GOVENOR_LIMIT = mixerRuntime.govenorPreviousRPMLimit + acceleration; + } + } + else { + throttle = throttle * mixerRuntime.govenorExpectedThrottleLimit; + //Street League customization + //RPM_GOVENOR_LIMIT = ((mixerConfig()->govenor_rpm_limit))*100.0f; + RPM_GOVENOR_LIMIT = ((forcedRPMLimit))*100.0f; + } + + //get the rpm averaged across the motors + bool motorsSaturated = false; + for (int i = 0; i < getMotorCount(); i++) { + averageRPM += getDshotTelemetry(i); + if (motor[i] >= motorConfig()->maxthrottle) { + motorsSaturated = true; + } + } + averageRPM = 100 * averageRPM / (getMotorCount()*motorConfig()->motorPoleCount/2.0f); + + //get the smoothed rpm to avoid d term noise + averageRPM_smoothed = mixerRuntime.govenorPreviousSmoothedRPM + mixerRuntime.govenorDelayK * (averageRPM - mixerRuntime.govenorPreviousSmoothedRPM); //kinda braindead to convert to rps then back + + float smoothedRPMError = averageRPM_smoothed - RPM_GOVENOR_LIMIT; + float govenorP = smoothedRPMError * mixerRuntime.govenorPGain; //+ when overspped + float govenorD = (smoothedRPMError-mixerRuntime.govenorPreviousSmoothedRPMError) * mixerRuntime.govenorDGain; // + when quickly going overspeed + + if (mixerConfig()->rpm_linearization) { + //don't let I term wind up if throttle is below the motor idle + if (rcCommandThrottle < motorConfig()->digitalIdleOffsetValue / 10000.0f) { + mixerRuntime.govenorI *= 1.0f/(1.0f+(pidGetDT()*10.0f)); //slowly ramp down i term instead of resetting to avoid throttle pulsing cheats + } else { + //don't let I term wind up if motors are saturated. Otherwise, motors may stay at high throttle even after low throttle is commanded + if(!motorsSaturated) + { + mixerRuntime.govenorI += smoothedRPMError * mixerRuntime.govenorIGain; // + when overspeed + } + } + //sum our pid terms + PIDOutput = govenorP + mixerRuntime.govenorI + govenorD; //more + when overspeed, should be subtracted from throttle + + } else { + throttle = throttle * mixerRuntime.govenorExpectedThrottleLimit; + mixerRuntime.govenorI += smoothedRPMError * mixerRuntime.govenorIGain; // + when overspeed + mixerRuntime.govenorI = MAX(mixerRuntime.govenorI, 0.0f); + PIDOutput = govenorP + mixerRuntime.govenorI + govenorD; //more + when overspeed, should be subtracted from throttle + if (PIDOutput > 0.05) { + mixerRuntime.govenorExpectedThrottleLimit = 0.9994 * mixerRuntime.govenorExpectedThrottleLimit; + } + if (PIDOutput < -0.05 && rcCommand[THROTTLE] > 1950 && !motorsSaturated) { + mixerRuntime.govenorExpectedThrottleLimit = (1+1-0.9994) * mixerRuntime.govenorExpectedThrottleLimit; + mixerRuntime.govenorExpectedThrottleLimit = MAX(mixerRuntime.govenorExpectedThrottleLimit, 1.0f); + } + + PIDOutput = MAX(PIDOutput,0.0f); + + } + if (mixerRuntime.govenor_init) { + if (mixerConfig()->rpm_linearization) { + throttle = constrainf(-PIDOutput, 0.0f, 1.0f); + } else { + throttle = constrainf(throttle-PIDOutput, 0.0f, 1.0f); + } + } + mixerRuntime.govenor_init = true; + + //update previous values for next loop + mixerRuntime.prevAverageRPM = averageRPM; + mixerRuntime.govenorPreviousSmoothedRPM = averageRPM_smoothed; + mixerRuntime.govenorPreviousSmoothedRPMError = smoothedRPMError; + mixerRuntime.govenorPreviousRPMLimit = RPM_GOVENOR_LIMIT; + + // DEBUG_SET(DEBUG_RPM_LIMITER, 0, averageRPM); + // DEBUG_SET(DEBUG_RPM_LIMITER, 1, smoothedRPMError); + // DEBUG_SET(DEBUG_RPM_LIMITER, 2, mixerRuntime.govenorI*100.0f); + // DEBUG_SET(DEBUG_RPM_LIMITER, 3, govenorD*10000.0f); + } +} + static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer) { - // 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. for (int i = 0; i < mixerRuntime.motorCount; i++) { float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle; -#ifdef USE_THRUST_LINEARIZATION - motorOutput = pidApplyThrustLinearization(motorOutput); -#endif + if (!mixerConfig()->govenor) { + #ifdef USE_THRUST_LINEARIZATION + motorOutput = pidApplyThrustLinearization(motorOutput); + #endif + } motorOutput = motorOutputMin + motorOutputRange * motorOutput; - #ifdef USE_SERVOS if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); @@ -377,6 +481,8 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t // Disarmed mode if (!ARMING_FLAG(ARMED)) { + mixerRuntime.govenorI = 0; + mixerRuntime.govenor_init = false; for (int i = 0; i < mixerRuntime.motorCount; i++) { motor[i] = motor_disarmed[i]; } @@ -425,68 +531,47 @@ static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle) } #endif -static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled) -{ - float airmodeTransitionPercent = 1.0f; - float motorDeltaScale = 0.5f; - - if (!airmodeEnabled && throttle < 0.5f) { - // this scales the motor mix authority to be 0.5 at 0 throttle, and 1.0 at 0.5 throttle as airmode off intended for things to work. - // also lays the groundwork for how an airmode percent would work. - airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent - motorDeltaScale *= airmodeTransitionPercent; // this should be half of the motor authority allowed - } - - const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent; - - const float motorMixDelta = motorDeltaScale * motorMixRange; - - float minMotor = FLT_MAX; - float maxMotor = FLT_MIN; +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 (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] + fabsf(motorMix[i]), motorMix[i] - fabsf(motorMix[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; - - maxMotor = MAX(motorMix[i], maxMotor); - minMotor = MIN(motorMix[i], minMotor); + motorMix[i] /= motorMixNormalizationFactor; } - - // constrain throttle so it won't clip any outputs - throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor); } -static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled) -{ +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.0f; + float airmodeThrottleChange = 0; #endif - float airmodeTransitionPercent = 1.0f; - if (!airmodeEnabled && throttle < 0.5f) { - // this scales the motor mix authority to be 0.5 at 0 throttle, and 1.0 at 0.5 throttle as airmode off intended for things to work. - // also lays the groundwork for how an airmode percent would work. - airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent + 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 + } } - const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent; - - for (int i = 0; i < mixerRuntime.motorCount; i++) { - motorMix[i] *= motorMixNormalizationFactor; - } - - const float normalizedMotorMixMin = motorMixMin * motorMixNormalizationFactor; - const float normalizedMotorMixMax = motorMixMax * motorMixNormalizationFactor; - throttle = constrainf(throttle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax); - #ifdef USE_AIRMODE_LPF - airmodeThrottleChange = constrainf(unadjustedThrottle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax) - unadjustedThrottle; pidUpdateAirmodeLpf(airmodeThrottleChange); #endif } @@ -557,7 +642,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) } #endif - // send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode + // send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode mixerThrottle = throttle; #ifdef USE_DYN_IDLE @@ -571,13 +656,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) // reduce throttle to offset additional motor output throttle = pidCompensateThrustLinearization(throttle); #endif - + applyRPMLimiter(); // Find roll/pitch/yaw desired output // ??? Where is the optimal location for this code? float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; for (int i = 0; i < mixerRuntime.motorCount; i++) { - float mix = scaledAxisPidRoll * activeMixer[i].roll + scaledAxisPidPitch * activeMixer[i].pitch + @@ -647,4 +731,4 @@ void mixerSetThrottleAngleCorrection(int correctionValue) float mixerGetThrottle(void) { return mixerThrottle; -} +} \ No newline at end of file diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 9e7ea6712c..d8c5040deb 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -89,6 +89,16 @@ typedef struct mixerConfig_s { bool yaw_motors_reversed; uint8_t crashflip_motor_percent; uint8_t crashflip_expo; + bool govenor; + uint16_t govenor_p; + uint16_t govenor_i; + uint16_t govenor_d; + uint16_t govenor_rpm_limit; + uint16_t govenor_acceleration_limit; + uint16_t govenor_deceleration_limit; + bool rpm_linearization; + uint16_t govenorThrottleLimitLearningTimeMS; + uint16_t govenor_idle_rpm; uint8_t mixer_type; } mixerConfig_t; diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 41cc172d87..306caa0808 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -50,7 +50,16 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = DEFAULT_MIXER, .yaw_motors_reversed = false, .crashflip_motor_percent = 0, - .crashflip_expo = 35, + .crashflip_expo = 0, + .govenor = true, + .govenor_p = 20, + .govenor_i = 15, + .govenor_d = 10, + .rpm_linearization = true, + .govenor_idle_rpm = 17, + .govenor_acceleration_limit = 60, + .govenor_deceleration_limit = 60, + .govenor_rpm_limit = 130, .mixer_type = MIXER_LEGACY, ); @@ -310,6 +319,28 @@ void mixerInitProfile(void) mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE; // Override value set by initEscEndpoints to allow zero motor drive } #endif +mixerRuntime.govenorExpectedThrottleLimit = 1.0f; +//Street League customization +mixerRuntime.govenorPGain = 20.0f * 0.0000015f; +mixerRuntime.govenorIGain = 15.0f * 0.0001f * pidGetDT(); +mixerRuntime.govenorDGain = 10.0f * 0.00000003f * pidGetPidFrequency(); +mixerRuntime.govenorAccelerationLimit = 60.0f * 1000.0f * pidGetDT(); +mixerRuntime.govenorDecelerationLimit = 60.0f * 1000.0f * pidGetDT(); +/*mixerRuntime.govenorPGain = mixerConfig()->govenor_p * 0.0000015f; +mixerRuntime.govenorIGain = mixerConfig()->govenor_i * 0.0001f * pidGetDT(); +mixerRuntime.govenorDGain = mixerConfig()->govenor_d * 0.00000003f * pidGetPidFrequency(); +mixerRuntime.govenorAccelerationLimit = mixerConfig()->govenor_acceleration_limit * 1000.0f * pidGetDT(); +mixerRuntime.govenorDecelerationLimit = mixerConfig()->govenor_deceleration_limit * 1000.0f * pidGetDT();*/ +mixerRuntime.govenorI = 0; +// mixerRuntime.govenorPrevThrottle = 0; +// mixerRuntime.govenorFFGain = 0.05f * (float)(mixerConfig()->govenor_ff) * 0.001f; +// mixerRuntime.govenorAverageAverageRPM = 0; +// mixerRuntime.govenorAverageStickThrottle = 0; +mixerRuntime.govenorPreviousSmoothedRPMError = 0; +// mixerRuntime.govenorIterationStep = 1.0f/(pidGetPidFrequency() * mixerConfig()->govenor_learning_threshold_window); // 3 is the averaging +mixerRuntime.govenorDelayK = 800 * pidGetDT() / 20.0f; +mixerRuntime.govenorLearningThrottleK = 0.5 / (pidGetPidFrequency() * mixerConfig()->govenorThrottleLimitLearningTimeMS / 1000); // 0.5 = value ^ (4000 * time) 0.99^(4000*(20/1000)) +mixerRuntime.govenor_init = false; #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) mixerRuntime.vbatSagCompensationFactor = 0.0f; @@ -346,7 +377,7 @@ void loadLaunchControlMixer(void) static void mixerConfigureOutput(void) { mixerRuntime.motorCount = 0; - + if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { @@ -410,6 +441,7 @@ void mixerInit(mixerMode_e mixerMode) mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D); initEscEndpoints(); + #ifdef USE_SERVOS if (mixerIsTricopter()) { mixerTricopterInit(); @@ -445,9 +477,11 @@ bool mixerModeIsFixedWing(mixerMode_e mixerMode) case MIXER_AIRPLANE: case MIXER_CUSTOM_AIRPLANE: return true; + break; default: return false; + break; } } @@ -465,4 +499,4 @@ float getMotorOutputLow(void) float getMotorOutputHigh(void) { return mixerRuntime.motorOutputHigh; -} +} \ No newline at end of file diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h index dbbcf45fd6..8e781503fd 100644 --- a/src/main/flight/mixer_init.h +++ b/src/main/flight/mixer_init.h @@ -53,6 +53,21 @@ typedef struct mixerRuntime_s { float vbatFull; float vbatRangeToCompensate; #endif + float govenorExpectedThrottleLimit; + float govenorAccelerationLimit; + float govenorDecelerationLimit; + float prevAverageRPM; + float govenorPreviousSmoothedRPMError; + float minRPMDelayK; + float govenorI; + float govenorPGain; + float govenorIGain; + float govenorDGain; + float govenorPreviousSmoothedRPM; + float govenorPreviousRPMLimit; + float govenorDelayK; + float govenorLearningThrottleK; + bool govenor_init; } mixerRuntime_t; -extern mixerRuntime_t mixerRuntime; +extern mixerRuntime_t mixerRuntime; \ No newline at end of file