Add setpoint boost

rotorflight · Sep 13, 2024 · 8becefb · 8becefb
1 parent 0f33614
commit 8becefb
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 0 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -1103,6 +1103,8 @@ const clivalue_t valueTable[] = {
     { "yaw_gyro_cutoff",            VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, gyro_cutoff[PID_YAW]) },
     { "yaw_error_cutoff",           VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, error_cutoff[PID_YAW]) },
 
+    { "setpoint_boost",             VAR_UINT16 | PROFILE_VALUE | MODE_ARRAY, .config.array.length = 3, PG_PID_PROFILE, offsetof(pidProfile_t, setpoint_boost) },
+
     { "yaw_cw_stop_gain",           VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_cw_stop_gain) },
     { "yaw_ccw_stop_gain",          VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_ccw_stop_gain) },
 

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -160,6 +160,11 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile)
     pid.coef[PID_YAW].Kf = YAW_F_TERM_SCALE * pidProfile->pid[PID_YAW].F;
     pid.coef[PID_YAW].Kb = YAW_B_TERM_SCALE * pidProfile->pid[PID_YAW].B;
 
+    // Setpoint boost
+    pid.boostGain[PID_ROLL]   = ROLL_B_TERM_SCALE  * pidProfile->setpoint_boost[PID_ROLL];
+    pid.boostGain[PID_PITCH]  = PITCH_B_TERM_SCALE * pidProfile->setpoint_boost[PID_PITCH];
+    pid.boostGain[PID_YAW]    = YAW_B_TERM_SCALE   * pidProfile->setpoint_boost[PID_YAW];
+
     // Bleed conversion for pitch
     if (pidProfile->pid[PID_PITCH].O > 0 && pidProfile->pid[PID_PITCH].I > 0)
       pid.coef[PID_PITCH].Kc = pid.coef[PID_PITCH].Ko / pid.coef[PID_PITCH].Ki;
@@ -196,6 +201,7 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile)
         lowpassFilterInit(&pid.errorFilter[i], LPF_ORDER1, pidProfile->error_cutoff[i], pid.freq, 0);
         difFilterInit(&pid.dtermFilter[i], pidProfile->dterm_cutoff[i], pid.freq);
         difFilterInit(&pid.btermFilter[i], pidProfile->bterm_cutoff[i], pid.freq);
+        difFilterInit(&pid.boostFilter[i], pidProfile->bterm_cutoff[i], pid.freq);
     }
 
     // Error relax
@@ -357,6 +363,9 @@ static float pidApplySetpoint(uint8_t axis)
     setpoint = rescueApply(axis, setpoint);
 #endif
 
+    // Apply boost
+    setpoint += difFilterApply(&pid.boostFilter[axis], setpoint) * pid.boostGain[axis];
+
     // Save setpoint
     pid.data[axis].setPoint = setpoint;
 

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -121,6 +121,8 @@ typedef struct pid_s {
     float yawCWStopGain;
     float yawCCWStopGain;
 
+    float boostGain[PID_AXIS_COUNT];
+
     float cyclicCrossCouplingGain[XY_AXIS_COUNT];
 
     float collective;
@@ -137,6 +139,7 @@ typedef struct pid_s {
 
     difFilter_t dtermFilter[PID_AXIS_COUNT];
     difFilter_t btermFilter[PID_AXIS_COUNT];
+    difFilter_t boostFilter[PID_AXIS_COUNT];
 
     order1Filter_t crossCouplingFilter[XY_AXIS_COUNT];
 

diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h
@@ -179,6 +179,8 @@ typedef struct pidProfile_s {
     uint8_t             cyclic_cross_coupling_ratio;
     uint8_t             cyclic_cross_coupling_cutoff;
 
+    uint16_t            setpoint_boost[3];
+
     pidAngleMode_t      angle;
     pidHorizonMode_t    horizon;
     pidTrainerMode_t    trainer;