diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index deb5a9dbb27..875e04cb4e9 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -109,4 +109,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"GPS_DOP",
"FAILSAFE",
"DSHOT_TELEMETRY_COUNTS"
+ "RPM_LIMIT",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 1413badcb41..9987528d801 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -93,7 +93,6 @@ typedef enum {
DEBUG_D_LPF,
DEBUG_VTX_TRAMP,
DEBUG_GHST,
- DEBUG_GHST_MSP,
DEBUG_SCHEDULER_DETERMINISM,
DEBUG_TIMING_ACCURACY,
DEBUG_RX_EXPRESSLRS_SPI,
@@ -107,6 +106,7 @@ typedef enum {
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
DEBUG_DSHOT_TELEMETRY_COUNTS,
+ DEBUG_RPM_LIMIT,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 1aaba47b692..0132e64195b 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -835,6 +835,7 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, minthrottle) },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) },
{ "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) },
+ { "motor_kv", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 1, 40000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, kv) },
#ifdef USE_DSHOT
{ PARAM_NAME_DSHOT_IDLE_VALUE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 2000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, digitalIdleOffsetValue) },
#ifdef USE_DSHOT_DMAR
@@ -936,7 +937,14 @@ const clivalue_t valueTable[] = {
{ "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) },
+ { "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) },
+#ifdef USE_RPM_LIMIT
+ { "rpm_limit", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit) },
+ { "rpm_limit_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_p) },
+ { "rpm_limit_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_i) },
+ { "rpm_limit_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_d) },
+ { "rpm_limit_value", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_value) },
+#endif
// PG_MOTOR_3D_CONFIG
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) },
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index a02f78ee7b5..94f6e16579d 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -18,8 +18,6 @@
* If not, see .
*/
-#include
-#include
#include
#include
#include
@@ -67,6 +65,12 @@
#define DYN_LPF_THROTTLE_STEPS 100
#define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
+#ifdef USE_RPM_LIMIT
+#define RPM_LIMIT_ACTIVE mixerConfig()->rpm_limit
+#else
+#define RPM_LIMIT_ACTIVE false
+#endif
+
static FAST_DATA_ZERO_INIT float motorMixRange;
float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
@@ -218,7 +222,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
} else {
throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
currentThrottleInputRange = PWM_RANGE;
-
#ifdef USE_DYN_IDLE
if (mixerRuntime.dynIdleMinRps > 0.0f) {
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f;
@@ -346,6 +349,47 @@ static void applyFlipOverAfterCrashModeToMotors(void)
}
}
+#ifdef USE_RPM_LIMIT
+static void applyRpmLimiter(mixerRuntime_t *mixer)
+{
+ static float prevError = 0.0f;
+ static float i = 0.0f;
+ const float unsmoothedAverageRpm = getDshotAverageRpm();
+ const float averageRpm = pt1FilterApply(&mixer->averageRpmFilter, unsmoothedAverageRpm);
+ const float error = averageRpm - mixer->rpmLimiterRpmLimit;
+
+ // PID
+ const float p = error * mixer->rpmLimiterPGain;
+ const float d = (error - prevError) * mixer->rpmLimiterDGain; // rpmLimiterDGain already adjusted for looprate (see mixer_init.c)
+ i += error * mixer->rpmLimiterIGain; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c)
+ i = MAX(0.0f, i);
+ float pidOutput = p + i + d;
+
+ // Throttle limit learning
+ if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) {
+ mixer->rpmLimiterThrottleScale *= 1.0f - 4.8f * pidGetDT();
+ } else if (pidOutput < -400 * pidGetDT() && rcCommand[THROTTLE] >= rxConfig()->maxcheck && !areMotorsSaturated()) { // Throttle accel corresponds with motor accel
+ mixer->rpmLimiterThrottleScale *= 1.0f + 3.2f * pidGetDT();
+ }
+ mixer->rpmLimiterThrottleScale = constrainf(mixer->rpmLimiterThrottleScale, 0.01f, 1.0f);
+ throttle *= mixer->rpmLimiterThrottleScale;
+
+ // Output
+ pidOutput = MAX(0.0f, pidOutput);
+ throttle = constrainf(throttle - pidOutput, 0.0f, 1.0f);
+ prevError = error;
+
+ DEBUG_SET(DEBUG_RPM_LIMIT, 0, lrintf(averageRpm));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 1, lrintf(unsmoothedAverageRpm));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 2, lrintf(mixer->rpmLimiterThrottleScale * 100.0f));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 5, lrintf(p * 100.0f));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 6, lrintf(i * 100.0f));
+ DEBUG_SET(DEBUG_RPM_LIMIT, 7, lrintf(d * 100.0f));
+}
+#endif // USE_RPM_LIMIT
+
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
@@ -353,7 +397,9 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
for (int i = 0; i < mixerRuntime.motorCount; i++) {
float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
#ifdef USE_THRUST_LINEARIZATION
+ if (!RPM_LIMIT_ACTIVE) {
motorOutput = pidApplyThrustLinearization(motorOutput);
+ }
#endif
motorOutput = motorOutputMin + motorOutputRange * motorOutput;
@@ -385,7 +431,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
static float applyThrottleLimit(float throttle)
{
- if (currentControlRateProfile->throttle_limit_percent < 100) {
+ if (currentControlRateProfile->throttle_limit_percent < 100 && !RPM_LIMIT_ACTIVE) {
const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
switch (currentControlRateProfile->throttle_limit_type) {
case THROTTLE_LIMIT_TYPE_SCALE:
@@ -498,7 +544,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
if (isFlipOverAfterCrashActive()) {
applyFlipOverAfterCrashModeToMotors();
-
return;
}
@@ -572,12 +617,17 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
throttle = pidCompensateThrustLinearization(throttle);
#endif
+#ifdef USE_RPM_LIMIT
+ if (RPM_LIMIT_ACTIVE && motorConfig()->dev.useDshotTelemetry && ARMING_FLAG(ARMED)) {
+ applyRpmLimiter(&mixerRuntime);
+ }
+#endif
+
// 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 +
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 9e7ea6712c1..3b171725a73 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -20,10 +20,15 @@
#pragma once
+#include
+#include
+
#include "platform.h"
#include "common/time.h"
+
#include "pg/pg.h"
+
#include "drivers/io_types.h"
#include "drivers/pwm_output.h"
@@ -90,6 +95,13 @@ typedef struct mixerConfig_s {
uint8_t crashflip_motor_percent;
uint8_t crashflip_expo;
uint8_t mixer_type;
+#ifdef USE_RPM_LIMIT
+ bool rpm_limit;
+ uint16_t rpm_limit_p;
+ uint16_t rpm_limit_i;
+ uint16_t rpm_limit_d;
+ uint16_t rpm_limit_value;
+#endif
} mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig);
@@ -104,12 +116,13 @@ struct rxConfig_s;
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool areMotorsRunning(void);
+bool areMotorsSaturated(void);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void initEscEndpoints(void);
void mixerInit(mixerMode_e mixerMode);
void mixerInitProfile(void);
-
+void mixerResetRpmLimiter(void);
void mixerResetDisarmedMotors(void);
void mixTable(timeUs_t currentTimeUs);
void stopMotors(void);
diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c
index 41cc172d873..44d73b4ab84 100644
--- a/src/main/flight/mixer_init.c
+++ b/src/main/flight/mixer_init.c
@@ -27,6 +27,8 @@
#include "build/build_config.h"
#include "build/debug.h"
+#include "common/maths.h"
+
#include "config/config.h"
#include "config/feature.h"
@@ -44,15 +46,23 @@
#include "mixer_init.h"
-PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_FN(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2);
-PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
- .mixerMode = DEFAULT_MIXER,
- .yaw_motors_reversed = false,
- .crashflip_motor_percent = 0,
- .crashflip_expo = 35,
- .mixer_type = MIXER_LEGACY,
-);
+void pgResetFn_mixerConfig(mixerConfig_t *mixerConfig)
+{
+ mixerConfig->mixerMode = DEFAULT_MIXER;
+ mixerConfig->yaw_motors_reversed = false;
+ mixerConfig->crashflip_motor_percent = 0;
+ mixerConfig->crashflip_expo = 35;
+ mixerConfig->mixer_type = MIXER_LEGACY;
+#ifdef USE_RPM_LIMIT
+ mixerConfig->rpm_limit = false;
+ mixerConfig->rpm_limit_p = 25;
+ mixerConfig->rpm_limit_i = 10;
+ mixerConfig->rpm_limit_d = 8;
+ mixerConfig->rpm_limit_value = 18000;
+#endif
+}
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
@@ -264,15 +274,23 @@ bool areMotorsRunning(void)
for (int i = 0; i < mixerRuntime.motorCount; i++) {
if (motor_disarmed[i] != mixerRuntime.disarmMotorOutput) {
motorsRunning = true;
-
break;
}
}
}
-
return motorsRunning;
}
+bool areMotorsSaturated(void)
+{
+ for (int i = 0; i < getMotorCount(); i++) {
+ if (motor[i] >= motorConfig()->maxthrottle) {
+ return true;
+ }
+ }
+ return false;
+}
+
#ifdef USE_SERVOS
bool mixerIsTricopter(void)
{
@@ -322,7 +340,24 @@ void mixerInitProfile(void)
}
}
#endif
+
+#ifdef USE_RPM_LIMIT
+ mixerRuntime.rpmLimiterRpmLimit = mixerConfig()->rpm_limit_value;
+ mixerRuntime.rpmLimiterPGain = mixerConfig()->rpm_limit_p * 15e-6f;
+ mixerRuntime.rpmLimiterIGain = mixerConfig()->rpm_limit_i * 1e-3f * pidGetDT();
+ mixerRuntime.rpmLimiterDGain = mixerConfig()->rpm_limit_d * 3e-7f * pidGetPidFrequency();
+ pt1FilterInit(&mixerRuntime.averageRpmFilter, pt1FilterGain(6.0f, pidGetDT()));
+ mixerResetRpmLimiter();
+#endif
+}
+
+#ifdef USE_RPM_LIMIT
+void mixerResetRpmLimiter(void)
+{
+ const float maxExpectedRpm = MAX(1.0f, motorConfig()->kv * getBatteryVoltage() * 0.01f);
+ mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / maxExpectedRpm, 0.0f, 1.0f);
}
+#endif // USE_RPM_LIMIT
#ifdef USE_LAUNCH_CONTROL
// Create a custom mixer for launch control based on the current settings
diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h
index dbbcf45fd65..816e8c587af 100644
--- a/src/main/flight/mixer_init.h
+++ b/src/main/flight/mixer_init.h
@@ -24,7 +24,6 @@
#include "flight/mixer.h"
-
typedef struct mixerRuntime_s {
uint8_t motorCount;
motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@@ -53,6 +52,14 @@ typedef struct mixerRuntime_s {
float vbatFull;
float vbatRangeToCompensate;
#endif
+#if defined(USE_RPM_LIMIT)
+ float rpmLimiterRpmLimit;
+ float rpmLimiterThrottleScale;
+ float rpmLimiterPGain;
+ float rpmLimiterIGain;
+ float rpmLimiterDGain;
+ pt1Filter_t averageRpmFilter;
+#endif
} mixerRuntime_t;
extern mixerRuntime_t mixerRuntime;
diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c
index 3f70786fd0c..543bbbb500f 100644
--- a/src/main/pg/motor.c
+++ b/src/main/pg/motor.c
@@ -33,7 +33,7 @@
#include "pg/pg_ids.h"
#include "pg/motor.h"
-PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
+PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2);
void pgResetFn_motorConfig(motorConfig_t *motorConfig)
{
@@ -61,6 +61,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000;
motorConfig->digitalIdleOffsetValue = 550;
+ motorConfig->kv = 1960;
#ifdef USE_DSHOT_DMAR
motorConfig->dev.useBurstDshot = ENABLE_DSHOT_DMAR;
@@ -72,7 +73,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
}
#endif
- motorConfig->motorPoleCount = 14; // Most brushes motors that we use are 14 poles
+ motorConfig->motorPoleCount = 14; // Most brushless motors that we use are 14 poles
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motorConfig->dev.motorOutputReordering[i] = i;
diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h
index a2ea0fb5515..8c55a87b848 100644
--- a/src/main/pg/motor.h
+++ b/src/main/pg/motor.h
@@ -56,9 +56,10 @@ typedef struct motorConfig_s {
motorDevConfig_t dev;
uint16_t digitalIdleOffsetValue; // Idle value for DShot protocol, full motor output = 10000
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
- uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
+ uint16_t maxthrottle; // This is the maximum value for the ESCs at full power. This value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
- uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
+ uint16_t kv; // Motor velocity constant (Kv) to estimate RPM under no load (unloadedRpm = Kv * batteryVoltage)
+ uint8_t motorPoleCount; // Number of magnetic poles in the motor bell for calculating actual RPM from eRPM provided by ESC telemetry
} motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 15d5f2465de..211210f3e50 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -40,6 +40,7 @@
#include "fc/rc_controls.h"
#include "flight/mixer.h"
+#include "flight/mixer_init.h"
#include "io/beeper.h"
@@ -198,7 +199,6 @@ static bool isVoltageFromBat(void)
void batteryUpdatePresence(void)
{
-
if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()) {
// Battery has just been connected - calculate cells, warning voltages and reset state
@@ -217,6 +217,9 @@ void batteryUpdatePresence(void)
changePidProfileFromCellCount(batteryCellCount);
}
}
+#ifdef USE_RPM_LIMIT
+ mixerResetRpmLimiter();
+#endif
batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
batteryWarningHysteresisVoltage = (batteryWarningVoltage > batteryConfig()->vbathysteresis) ? batteryWarningVoltage - batteryConfig()->vbathysteresis : 0;
@@ -237,7 +240,7 @@ void batteryUpdatePresence(void)
}
}
-void batteryUpdateWhDrawn(void)
+void batteryUpdateWhDrawn(void)
{
static int32_t mAhDrawnPrev = 0;
const int32_t mAhDrawnCurrent = getMAhDrawn();
diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h
index d0edd0b56d0..b1f1f6c4c2c 100644
--- a/src/main/sensors/voltage.h
+++ b/src/main/sensors/voltage.h
@@ -41,7 +41,7 @@ extern const char * const voltageMeterSourceNames[VOLTAGE_METER_COUNT];
// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns.
typedef struct voltageMeter_s {
- uint16_t displayFiltered; // voltage in 0.01V steps
+ uint16_t displayFiltered; // voltage in 0.01V steps
uint16_t unfiltered; // voltage in 0.01V steps
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
uint16_t sagFiltered; // voltage in 0.01V steps