diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index 0de0172edd..f0c353dfee 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -144,6 +144,7 @@ void resetTest(void) {
         pidData[axis].P = 0;
         pidData[axis].I = 0;
         pidData[axis].D = 0;
+        pidData[axis].F = 0;
         pidData[axis].Sum = 0;
         simulatedSetpointRate[axis] = 0;
         simulatedRcDeflection[axis] = 0;
@@ -266,7 +267,7 @@ TEST(pidControllerTest, testPidLoop) {
     ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
-    EXPECT_FLOAT_EQ(-132.25, pidData[FD_YAW].D);
+    ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25));
 
     // Match the stick to gyro to stop error
     simulatedSetpointRate[FD_ROLL] = 100;
@@ -458,8 +459,46 @@ TEST(pidControllerTest, testCrashRecoveryMode) {
     // Add additional verifications
 }
 
-TEST(pidControllerTest, pidSetpointTransition) {
-// TODO
+TEST(pidControllerTest, testFeedForward) {
+    resetTest();
+    ENABLE_ARMING_FLAG(ARMED);
+    pidStabilisationState(PID_STABILISATION_ON);
+
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
+
+    // Match the stick to gyro to stop error
+    setStickPosition(FD_ROLL, 1.0f);
+    setStickPosition(FD_PITCH, -1.0f);
+    setStickPosition(FD_YAW, -1.0f);
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    ASSERT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78));
+    ASSERT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03));
+    ASSERT_NEAR(-82.52, pidData[FD_YAW].F, calculateTolerance(-82.5));
+
+    // Match the stick to gyro to stop error
+    setStickPosition(FD_ROLL, 0.5f);
+    setStickPosition(FD_PITCH, -0.5f);
+    setStickPosition(FD_YAW, -0.5f);
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    ASSERT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20));
+    ASSERT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
+    ASSERT_NEAR(-41.26, pidData[FD_YAW].F, calculateTolerance(-41.26));
+
+    for (int loop =0; loop <= 15; loop++) {
+        gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
+        pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+    }
+
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
+
 }
 
 TEST(pidControllerTest, testDtermFiltering) {