Merge pull request #4624 from iNavFlight/dzikuvx-nav-pids-to-pidff

Convert all NAV P-controllers to PIDFF
iNavFlight · Apr 21, 2019 · dbf5147 · dbf5147
2 parents d9644e0 + b8ba10b
commit dbf5147
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 29 deletions.
diff --git a/docs/Cli.md b/docs/Cli.md
@@ -318,9 +318,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
 |  accgain_z  | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
 |  nav_mc_pos_z_p  | 50 | P gain of altitude PID controller (Multirotor) |
 |  nav_fw_pos_z_p  | 50 | P gain of altitude PID controller (Fixedwing) |
-|  nav_mc_pos_z_i  | 0 | I gain of altitude PID controller (Multirotor) |
 |  nav_fw_pos_z_i  | 0 | I gain of altitude PID controller (Fixedwing) |
-|  nav_mc_pos_z_d  | 0 | D gain of altitude PID controller (Multirotor) |
 |  nav_fw_pos_z_d  | 0 | D gain of altitude PID controller (Fixedwing) |
 |  nav_mc_vel_z_p  | 100 | P gain of velocity PID controller |
 |  nav_mc_vel_z_i  | 50 | I gain of velocity PID controller |

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -1023,16 +1023,6 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
-      - name: nav_mc_pos_z_i
-        field: bank_mc.pid[PID_POS_Z].I
-        condition: USE_NAV
-        min: 0
-        max: 255
-      - name: nav_mc_pos_z_d
-        field: bank_mc.pid[PID_POS_Z].D
-        condition: USE_NAV
-        min: 0
-        max: 255
       - name: nav_mc_vel_z_p
         field: bank_mc.pid[PID_VEL_Z].P
         condition: USE_NAV

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -1737,14 +1737,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
     navPidReset(pid);
 }
 
-/*-----------------------------------------------------------
- * Float point P-controller implementation
- *-----------------------------------------------------------*/
-void navPInit(pController_t *p, float _kP)
-{
-    p->param.kP = _kP;
-}
-
 /*-----------------------------------------------------------
  * Detects if thrust vector is facing downwards
  *-----------------------------------------------------------*/
@@ -3036,7 +3028,14 @@ void navigationUsePIDs(void)
 
     // Initialize position hold P-controller
     for (int axis = 0; axis < 2; axis++) {
-        navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f);
+        navPidInit(
+            &posControl.pids.pos[axis], 
+            (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
+            0.0f,
+            0.0f,
+            0.0f,
+            NAV_DTERM_CUT_HZ
+        );
 
         navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
@@ -3047,7 +3046,14 @@ void navigationUsePIDs(void)
     }
 
     // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
-    navPInit(&posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f);
+    navPidInit(
+        &posControl.pids.pos[Z], 
+        (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
+        0.0f,
+        0.0f,
+        0.0f,
+        NAV_DTERM_CUT_HZ
+    );
 
     navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -266,14 +266,9 @@ typedef struct {
     float output_constrained;           // controller output constrained
 } pidController_t;
 
-typedef struct {
-    pControllerParam_t param;
-    float output_constrained;
-} pController_t;
-
 typedef struct navigationPIDControllers_s {
     /* Multicopter PIDs */
-    pController_t   pos[XYZ_AXIS_COUNT];
+    pidController_t   pos[XYZ_AXIS_COUNT];
     pidController_t vel[XYZ_AXIS_COUNT];
     pidController_t surface;
 

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -357,7 +357,6 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
 float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
 void navPidReset(pidController_t *pid);
 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
-void navPInit(pController_t *p, float _kP);
 
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);