diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index 44aada0687..d82d12fb1d 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -24,6 +24,7 @@
 
 extern "C" {
     #include "common/axis.h"
+    #include "common/maths.h"
     #include "flight/flight.h"
 
     #include "sensors/sensors.h"
@@ -48,10 +49,23 @@ extern "C" {
 #define UPWARDS_THRUST false
 
 
-TEST(FlightImuTest, Placeholder)
+TEST(FlightImuTest, TestCalculateHeading)
 {
-    // TODO test things
-    EXPECT_EQ(true, true);
+    //TODO: Add test cases using the Z dimension.
+    t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}};
+    EXPECT_EQ(calculateHeading(&north), 0);
+
+    t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}};
+    EXPECT_EQ(calculateHeading(&east), 90);
+
+    t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}};
+    EXPECT_EQ(calculateHeading(&south), 180);
+
+    t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}};
+    EXPECT_EQ(calculateHeading(&west), 270);
+
+    t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}};
+    EXPECT_EQ(calculateHeading(&north_east), 45);
 }
 
 // STUBS