Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

initial commit

Browse source
This commit is contained in:
tdogb 2023-02-10 00:08:12 -05:00
parent 4605309d82
commit 1fcb206c46
5 changed files with 330 additions and 164 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/cli/settings.c
View file

@ -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) }, { "beeper_dshot_beacon_tone", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = {1, DSHOT_CMD_BEACON5 }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotBeaconTone) },
#endif #endif
#endif // USE_BEEPER #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 // 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) }, { "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },

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

@ -20,9 +20,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h>
#include <math.h> #include <math.h>
#include <float.h>
#include "platform.h" #include "platform.h"
@ -109,113 +107,114 @@ static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign;
static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
{ {
static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions UNUSED(currentTimeUs);
static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode // 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; static float motorRangeMinIncrease = 0;
float currentThrottleInputRange = 0; float currentThrottleInputRange = 0;
if (mixerRuntime.feature3dEnabled) { // if (mixerRuntime.feature3dEnabled) {
uint16_t rcCommand3dDeadBandLow; // uint16_t rcCommand3dDeadBandLow;
uint16_t rcCommand3dDeadBandHigh; // uint16_t rcCommand3dDeadBandHigh;
if (!ARMING_FLAG(ARMED)) { // if (!ARMING_FLAG(ARMED)) {
rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. // 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) { // if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
// The min_check range is halved because the output throttle is scaled to 500us. // // 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 // // So by using half of min_check we maintain the same low-throttle deadband
// stick travel as normal non-3D mode. // // stick travel as normal non-3D mode.
const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2; // const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2;
rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset; // rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset;
rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset; // rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset;
} else { // } else {
rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; // rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; // rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
} // }
const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; // const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; // const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) { // if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) {
// INVERTED // // INVERTED
motorRangeMin = mixerRuntime.motorOutputLow; // motorRangeMin = mixerRuntime.motorOutputLow;
motorRangeMax = mixerRuntime.deadbandMotor3dLow; // motorRangeMax = mixerRuntime.deadbandMotor3dLow;
#ifdef USE_DSHOT // #ifdef USE_DSHOT
if (isMotorProtocolDshot()) { // if (isMotorProtocolDshot()) {
motorOutputMin = mixerRuntime.motorOutputLow; // motorOutputMin = mixerRuntime.motorOutputLow;
motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; // motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow;
} else // } else
#endif // #endif
{ // {
motorOutputMin = mixerRuntime.deadbandMotor3dLow; // motorOutputMin = mixerRuntime.deadbandMotor3dLow;
motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; // motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow;
} // }
if (motorOutputMixSign != -1) { // if (motorOutputMixSign != -1) {
reversalTimeUs = currentTimeUs; // reversalTimeUs = currentTimeUs;
} // }
motorOutputMixSign = -1; // motorOutputMixSign = -1;
rcThrottlePrevious = rcCommand[THROTTLE]; // rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; // throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
currentThrottleInputRange = rcCommandThrottleRange3dLow; // currentThrottleInputRange = rcCommandThrottleRange3dLow;
} else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { // } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
// NORMAL // // NORMAL
motorRangeMin = mixerRuntime.deadbandMotor3dHigh; // motorRangeMin = mixerRuntime.deadbandMotor3dHigh;
motorRangeMax = mixerRuntime.motorOutputHigh; // motorRangeMax = mixerRuntime.motorOutputHigh;
motorOutputMin = mixerRuntime.deadbandMotor3dHigh; // motorOutputMin = mixerRuntime.deadbandMotor3dHigh;
motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; // motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh;
if (motorOutputMixSign != 1) { // if (motorOutputMixSign != 1) {
reversalTimeUs = currentTimeUs; // reversalTimeUs = currentTimeUs;
} // }
motorOutputMixSign = 1; // motorOutputMixSign = 1;
rcThrottlePrevious = rcCommand[THROTTLE]; // rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; // throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
currentThrottleInputRange = rcCommandThrottleRange3dHigh; // currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow && // } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow &&
!flight3DConfigMutable()->switched_mode3d) || // !flight3DConfigMutable()->switched_mode3d) ||
isMotorsReversed()) { // isMotorsReversed()) {
// INVERTED_TO_DEADBAND // // INVERTED_TO_DEADBAND
motorRangeMin = mixerRuntime.motorOutputLow; // motorRangeMin = mixerRuntime.motorOutputLow;
motorRangeMax = mixerRuntime.deadbandMotor3dLow; // motorRangeMax = mixerRuntime.deadbandMotor3dLow;
#ifdef USE_DSHOT // #ifdef USE_DSHOT
if (isMotorProtocolDshot()) { // if (isMotorProtocolDshot()) {
motorOutputMin = mixerRuntime.motorOutputLow; // motorOutputMin = mixerRuntime.motorOutputLow;
motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; // motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow;
} else // } else
#endif // #endif
{ // {
motorOutputMin = mixerRuntime.deadbandMotor3dLow; // motorOutputMin = mixerRuntime.deadbandMotor3dLow;
motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; // motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow;
} // }
if (motorOutputMixSign != -1) { // if (motorOutputMixSign != -1) {
reversalTimeUs = currentTimeUs; // reversalTimeUs = currentTimeUs;
} // }
motorOutputMixSign = -1; // motorOutputMixSign = -1;
throttle = 0; // throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dLow; // currentThrottleInputRange = rcCommandThrottleRange3dLow;
} else { // } else {
// NORMAL_TO_DEADBAND // // NORMAL_TO_DEADBAND
motorRangeMin = mixerRuntime.deadbandMotor3dHigh; // motorRangeMin = mixerRuntime.deadbandMotor3dHigh;
motorRangeMax = mixerRuntime.motorOutputHigh; // motorRangeMax = mixerRuntime.motorOutputHigh;
motorOutputMin = mixerRuntime.deadbandMotor3dHigh; // motorOutputMin = mixerRuntime.deadbandMotor3dHigh;
motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; // motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh;
if (motorOutputMixSign != 1) { // if (motorOutputMixSign != 1) {
reversalTimeUs = currentTimeUs; // reversalTimeUs = currentTimeUs;
} // }
motorOutputMixSign = 1; // motorOutputMixSign = 1;
throttle = 0; // throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dHigh; // currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} // }
if (currentTimeUs - reversalTimeUs < 250000) { // if (currentTimeUs - reversalTimeUs < 250000) {
// keep iterm zero for 250ms after motor reversal // // keep iterm zero for 250ms after motor reversal
pidResetIterm(); // pidResetIterm();
} // }
} else { // } else {
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
currentThrottleInputRange = PWM_RANGE; currentThrottleInputRange = PWM_RANGE;
@ -245,13 +244,13 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
float motorRangeAttenuationFactor = 0; float motorRangeAttenuationFactor = 0;
// reduce motorRangeMax when battery is full // reduce motorRangeMax when battery is full
if (mixerRuntime.vbatSagCompensationFactor > 0.0f) { if (!mixerConfig()->govenor && mixerRuntime.vbatSagCompensationFactor > 0.0f) {
const uint16_t currentCellVoltage = getBatterySagCellVoltage(); const uint16_t currentCellVoltage = getBatterySagCellVoltage();
// batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow // 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); float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f);
motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor; motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor;
DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100)); DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100);
DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000)); DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000);
} }
motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
#else #else
@ -262,8 +261,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
motorOutputMin = motorRangeMin; motorOutputMin = motorRangeMin;
motorOutputRange = motorRangeMax - motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin;
motorOutputMixSign = 1; motorOutputMixSign = 1;
} // }
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); 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) 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++) { for (int i = 0; i < mixerRuntime.motorCount; i++) {
float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle; float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
#ifdef USE_THRUST_LINEARIZATION if (!mixerConfig()->govenor) {
motorOutput = pidApplyThrustLinearization(motorOutput); #ifdef USE_THRUST_LINEARIZATION
#endif motorOutput = pidApplyThrustLinearization(motorOutput);
#endif
}
motorOutput = motorOutputMin + motorOutputRange * motorOutput; motorOutput = motorOutputMin + motorOutputRange * motorOutput;
#ifdef USE_SERVOS #ifdef USE_SERVOS
if (mixerIsTricopter()) { if (mixerIsTricopter()) {
motorOutput += mixerTricopterMotorCorrection(i); motorOutput += mixerTricopterMotorCorrection(i);
@ -377,6 +481,8 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
// Disarmed mode // Disarmed mode
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
mixerRuntime.govenorI = 0;
mixerRuntime.govenor_init = false;
for (int i = 0; i < mixerRuntime.motorCount; i++) { for (int i = 0; i < mixerRuntime.motorCount; i++) {
motor[i] = motor_disarmed[i]; motor[i] = motor_disarmed[i];
} }
@ -425,68 +531,47 @@ static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
} }
#endif #endif
static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled) static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled) {
{ const float motorMixNormalizationFactor = motorMixRange > 1.0f ? motorMixRange : 1.0f;
float airmodeTransitionPercent = 1.0f; const float motorMixDelta = 0.5f * motorMixRange;
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;
for (int i = 0; i < mixerRuntime.motorCount; ++i) { for (int i = 0; i < mixerRuntime.motorCount; ++i) {
if (mixerConfig()->mixer_type == MIXER_LINEAR) { if (airmodeEnabled || throttle > 0.5f) {
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta); if (mixerConfig()->mixer_type == MIXER_LINEAR) {
} else { motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + fabsf(motorMix[i]), motorMix[i] - fabsf(motorMix[i])); } else {
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + ABS(motorMix[i]), motorMix[i] - ABS(motorMix[i]));
}
} }
motorMix[i] *= motorMixNormalizationFactor; motorMix[i] /= motorMixNormalizationFactor;
maxMotor = MAX(motorMix[i], maxMotor);
minMotor = MIN(motorMix[i], minMotor);
} }
// 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 #ifdef USE_AIRMODE_LPF
const float unadjustedThrottle = throttle; const float unadjustedThrottle = throttle;
throttle += pidGetAirmodeThrottleOffset(); throttle += pidGetAirmodeThrottleOffset();
float airmodeThrottleChange = 0.0f; float airmodeThrottleChange = 0;
#endif #endif
float airmodeTransitionPercent = 1.0f;
if (!airmodeEnabled && throttle < 0.5f) { if (motorMixRange > 1.0f) {
// 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. for (int i = 0; i < mixerRuntime.motorCount; i++) {
// also lays the groundwork for how an airmode percent would work. motorMix[i] /= motorMixRange;
airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent }
// 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 #ifdef USE_AIRMODE_LPF
airmodeThrottleChange = constrainf(unadjustedThrottle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax) - unadjustedThrottle;
pidUpdateAirmodeLpf(airmodeThrottleChange); pidUpdateAirmodeLpf(airmodeThrottleChange);
#endif #endif
} }
@ -557,7 +642,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
} }
#endif #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; mixerThrottle = throttle;
#ifdef USE_DYN_IDLE #ifdef USE_DYN_IDLE
@ -571,13 +656,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
// reduce throttle to offset additional motor output // reduce throttle to offset additional motor output
throttle = pidCompensateThrustLinearization(throttle); throttle = pidCompensateThrustLinearization(throttle);
#endif #endif
applyRPMLimiter();
// Find roll/pitch/yaw desired output // Find roll/pitch/yaw desired output
// ??? Where is the optimal location for this code? // ??? Where is the optimal location for this code?
float motorMix[MAX_SUPPORTED_MOTORS]; float motorMix[MAX_SUPPORTED_MOTORS];
float motorMixMax = 0, motorMixMin = 0; float motorMixMax = 0, motorMixMin = 0;
for (int i = 0; i < mixerRuntime.motorCount; i++) { for (int i = 0; i < mixerRuntime.motorCount; i++) {
float mix = float mix =
scaledAxisPidRoll * activeMixer[i].roll + scaledAxisPidRoll * activeMixer[i].roll +
scaledAxisPidPitch * activeMixer[i].pitch + scaledAxisPidPitch * activeMixer[i].pitch +
@ -647,4 +731,4 @@ void mixerSetThrottleAngleCorrection(int correctionValue)
float mixerGetThrottle(void) float mixerGetThrottle(void)
{ {
return mixerThrottle; return mixerThrottle;
} }

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

@ -89,6 +89,16 @@ typedef struct mixerConfig_s {
bool yaw_motors_reversed; bool yaw_motors_reversed;
uint8_t crashflip_motor_percent; uint8_t crashflip_motor_percent;
uint8_t crashflip_expo; 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; uint8_t mixer_type;
} mixerConfig_t; } mixerConfig_t;

40
src/main/flight/mixer_init.c
View file

@ -50,7 +50,16 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.mixerMode = DEFAULT_MIXER, .mixerMode = DEFAULT_MIXER,
.yaw_motors_reversed = false, .yaw_motors_reversed = false,
.crashflip_motor_percent = 0, .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, .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 mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE; // Override value set by initEscEndpoints to allow zero motor drive
} }
#endif #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) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
mixerRuntime.vbatSagCompensationFactor = 0.0f; mixerRuntime.vbatSagCompensationFactor = 0.0f;
@ -346,7 +377,7 @@ void loadLaunchControlMixer(void)
static void mixerConfigureOutput(void) static void mixerConfigureOutput(void)
{ {
mixerRuntime.motorCount = 0; mixerRuntime.motorCount = 0;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -410,6 +441,7 @@ void mixerInit(mixerMode_e mixerMode)
mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D); mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D);
initEscEndpoints(); initEscEndpoints();
#ifdef USE_SERVOS #ifdef USE_SERVOS
if (mixerIsTricopter()) { if (mixerIsTricopter()) {
mixerTricopterInit(); mixerTricopterInit();
@ -445,9 +477,11 @@ bool mixerModeIsFixedWing(mixerMode_e mixerMode)
case MIXER_AIRPLANE: case MIXER_AIRPLANE:
case MIXER_CUSTOM_AIRPLANE: case MIXER_CUSTOM_AIRPLANE:
return true; return true;
break; break;
default: default:
return false; return false;
break; break;
} }
} }
@ -465,4 +499,4 @@ float getMotorOutputLow(void)
float getMotorOutputHigh(void) float getMotorOutputHigh(void)
{ {
return mixerRuntime.motorOutputHigh; return mixerRuntime.motorOutputHigh;
} }

17
src/main/flight/mixer_init.h
View file

@ -53,6 +53,21 @@ typedef struct mixerRuntime_s {
float vbatFull; float vbatFull;
float vbatRangeToCompensate; float vbatRangeToCompensate;
#endif #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; } mixerRuntime_t;
extern mixerRuntime_t mixerRuntime; extern mixerRuntime_t mixerRuntime;