mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-21 15:25:36 +03:00
initial commit
This commit is contained in:
tdogb
parent
4605309d82
commit
1fcb206c46
5 changed files with 330 additions and 164 deletions
|
|
@ -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) },
|
||||
|
|
|
|||
|
|
@ -20,9 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#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
|
||||
}
|
||||
|
|
@ -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 +
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
Loading…
Add table
Add a link
Reference in a new issue