diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 04e984c162..fc150cc150 100755
--- 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,
+            (float)pidProfile()->bank_mc.pid[PID_POS_XY].I / 100.0f,
+            (float)pidProfile()->bank_mc.pid[PID_POS_XY].D / 100.0f,
+            (float)pidProfile()->bank_mc.pid[PID_POS_XY].FF / 100.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,12 +3046,19 @@ 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,
+        (float)pidProfile()->bank_mc.pid[PID_POS_Z].I / 100.0f,
+        (float)pidProfile()->bank_mc.pid[PID_POS_Z].D / 100.0f,
+        (float)pidProfile()->bank_mc.pid[PID_POS_Z].FF / 100.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,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_mc.pid[PID_VEL_Z].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ
     );
 
@@ -3069,14 +3075,14 @@ void navigationUsePIDs(void)
     navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_XY].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ
     );
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ
     );
 }
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index bb311aabaf..dffa7ad56b 100755
--- 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
index 8c88a08120..5c8ec7b67a 100755
--- 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);