diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index f74a32c73..5fa40d624 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -175,12 +175,16 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
     return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
 }
 
+/*
+* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
+* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
+* (my best interpretation of scalar 'tiltAngle') or rename the function.
+*/
 int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
 {
     return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees));
 }
 
-
 int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
 {
     int32_t result = 0;
diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc
index b2e1e328c..bc2b7e467 100644
--- a/src/test/unit/altitude_hold_unittest.cc
+++ b/src/test/unit/altitude_hold_unittest.cc
@@ -54,6 +54,7 @@ extern "C" {
 
 extern "C" {
     bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
+    uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
 }
 
 typedef struct inclinationExpectation_s {
@@ -90,6 +91,36 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards)
     }
 }
 
+typedef struct inclinationAngleExpectations_s {
+    rollAndPitchInclination_t inclination;
+    uint16_t expected_angle;
+} inclinationAngleExpectations_t;
+
+TEST(AltitudeHoldTest, TestCalculateTiltAngle)
+{
+    inclinationAngleExpectations_t inclinationAngleExpectations[] = {
+        { {0, 0}, 0},
+        { {1, 0}, 1},
+        { {0, 1}, 1},
+        { {0, -1}, 1},
+        { {-1, 0}, 1},
+        { {-1, -2}, 2},
+        { {-2, -1}, 2},
+        { {1, 2}, 2},
+        { {2, 1}, 2}
+    };
+
+    rollAndPitchInclination_t inclination = {0, 0};
+    uint16_t tilt_angle = calculateTiltAngle(&inclination);
+    EXPECT_EQ(tilt_angle, 0);
+
+    for (uint8_t i = 0; i < 9; i++) {
+        inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
+        uint16_t result = calculateTiltAngle(&expectation->inclination);
+        EXPECT_EQ(expectation->expected_angle, result);
+    }
+}
+
 // STUBS
 
 extern "C" {