idle

betaflight · Aug 2, 2019 · d474df3 · d474df3
1 parent 383ba1c
commit d474df3
Show file tree

Hide file tree

Showing 6 changed files with 60 additions and 5 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -1056,6 +1056,14 @@ const clivalue_t valueTable[] = {
     { "transient_throttle_limit",   VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
 #endif
 
+    { "idle_hz",                    VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_hz) },
+    { "idle_adjustment_speed",      VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_adjustment_speed) },
+    { "idle_throttle",              VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_throttle) },
+    { "idle_p",                     VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_p) },
+    { "idle_pid_limit",             VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_pid_limit) },
+    { "idle_max_increase",          VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_max_increase) },
+
+
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
@@ -57,6 +57,7 @@
 #include "flight/mixer.h"
 #include "flight/mixer_tricopter.h"
 #include "flight/pid.h"
+#include "flight/rpm_filter.h"
 
 #include "rx/rx.h"
 
@@ -464,6 +465,7 @@ static FAST_RAM_ZERO_INIT float motorRangeMax;
 static FAST_RAM_ZERO_INIT float motorOutputRange;
 static FAST_RAM_ZERO_INIT int8_t motorOutputMixSign;
 
+
 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 {
     static uint16_t rcThrottlePrevious = 0;   // Store the last throttle direction for deadband transitions
@@ -572,12 +574,23 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
             pidResetIterm();
         }
     } else {
+        static float motorRangeMinIncrease = 0;
+        if (currentPidProfile->idle_hz) {
+            static float oldMinRpm;
+            const float minRpm = rpmMinMotorSpeed();
+            const float targetRpmChangeRate = (currentPidProfile->idle_hz - minRpm) * currentPidProfile->idle_adjustment_speed;
+            const float error = targetRpmChangeRate - (minRpm - oldMinRpm) * pidFrequency;
+            const float pidSum = constrainf(currentPidProfile->idle_p * 0.0001f * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit);
+            motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * dT, 0.0f, currentPidProfile->idle_max_increase * 0.001);
+            oldMinRpm = minRpm;
+        }
+
         throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
         currentThrottleInputRange = rcCommandThrottleRange;
-        motorRangeMin = motorOutputLow;
+        motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow);
         motorRangeMax = motorOutputHigh;
-        motorOutputMin = motorOutputLow;
-        motorOutputRange = motorOutputHigh - motorOutputLow;
+        motorOutputMin = motorRangeMin;
+        motorOutputRange = motorOutputHigh - motorOutputMin;
         motorOutputMixSign = 1;
     }
 
@@ -832,6 +845,8 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     throttle = pidCompensateThrustLinearization(throttle);
 #endif
 
+    throttle += currentPidProfile->idle_throttle * 0.001f;
+
 #if defined(USE_THROTTLE_BOOST)
     if (throttleBoost > 0.0f) {
         const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -75,8 +75,8 @@ static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
 
 static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
 
-static FAST_RAM_ZERO_INIT float dT;
-static FAST_RAM_ZERO_INIT float pidFrequency;
+FAST_RAM_ZERO_INIT float dT;
+FAST_RAM_ZERO_INIT float pidFrequency;
 
 static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
 static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
@@ -206,6 +206,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
         .transient_throttle_limit = 15,
         .profileName = { 0 },
+        .idle_hz = 0,
+        .idle_adjustment_speed = 50,
+        .idle_throttle = 60,
+        .idle_p = 20,
+        .idle_pid_limit = 100,
+        .idle_max_increase = 150,
     );
 #ifndef USE_D_MIN
     pidProfile->pid[PID_ROLL].D = 30;

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -171,6 +171,15 @@ typedef struct pidProfile_s {
     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
     uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
     char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
+
+    uint8_t idle_hz;                        // minimum motor speed enforced by integrating p controller
+    uint8_t idle_throttle;                  // added to throttle, replaces dshot_idle_value
+    uint8_t idle_adjustment_speed;          // how quickly the integrating p controller tries to correct
+    uint8_t idle_p;                         // kP
+    uint8_t idle_pid_limit;                 // max P 
+    uint8_t idle_max_increase;              // max integrated correction
+
+
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -246,3 +255,7 @@ void dynLpfDTermUpdate(float throttle);
 void pidSetItermReset(bool enabled);
 float pidGetPreviousSetpoint(int axis);
 
+
+extern float dT;
+extern float pidFrequency;
+
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
@@ -224,4 +224,16 @@ bool isRpmFilterEnabled(void)
     return (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics));
 }
 
+float rpmMinMotorSpeed()
+{
+    float minSpeed = 10000.0f;
+    for (int i = getMotorCount(); i--;) {
+        if (motorFrequency[i] < minSpeed) {
+            minSpeed = motorFrequency[i];
+        }
+    }
+    return minSpeed;
+}
+
+
 #endif
diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h
@@ -43,3 +43,4 @@ float rpmFilterGyro(int axis, float values);
 float rpmFilterDterm(int axis, float values);
 void  rpmFilterUpdate();
 bool isRpmFilterEnabled(void);
+float rpmMinMotorSpeed();