diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
index 663e1c3531..736e8b199b 100644
--- a/src/main/drivers/sonar_hcsr04.c
+++ b/src/main/drivers/sonar_hcsr04.c
@@ -29,8 +29,8 @@
 #ifdef SONAR
 
 /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
- * When trigged it sends out a series of 40KHz ultrasonic pulses and receives
- * echo froman object. The distance between the unit and the object is calculated
+ * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
+ * echo from an object. The distance between the unit and the object is calculated
  * by measuring the traveling time of sound and output it as the width of a TTL pulse.
  *
  * *** Warning: HC-SR04 operates at +5V ***
@@ -152,7 +152,7 @@ void hcsr04_start_reading(void)
 
 /**
  * Get the distance that was measured by the last pulse, in centimeters. When the ground is too far away to be
- * reliably read by the sonar, -1 is returned instead.
+ * reliably read by the sonar, SONAR_OUT_OF_RANGE is returned instead.
  */
 int32_t hcsr04_get_distance(void)
 {
@@ -164,8 +164,8 @@ int32_t hcsr04_get_distance(void)
     int32_t distance = measurement / 59;
 
     // this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
-    if (distance > 300)
-        distance = -1;
+    if (distance > SONAR_MAX_RANGE_WITH_TILT)
+        distance = SONAR_OUT_OF_RANGE;
 
     return distance;
 }
diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h
index 9315668a75..d4e74bd16a 100644
--- a/src/main/drivers/sonar_hcsr04.h
+++ b/src/main/drivers/sonar_hcsr04.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "platform.h"
+
 typedef struct sonarHardware_s {
     uint16_t trigger_pin;
     uint16_t echo_pin;
@@ -27,6 +29,12 @@ typedef struct sonarHardware_s {
 
 #define SONAR_GPIO GPIOB
 
+#define SONAR_OUT_OF_RANGE (-1)
+#define SONAR_MAX_RANGE 400 // 4m
+#define SONAR_MAX_RANGE_WITH_TILT 300 // 3m, allow for tilt and roll
+#define SONAR_MAX_RANGE_ACCURACY_HIGH 200 // 2m
+#define SONAR_MAX_TILT_ANGLE 250 // 25 degrees
+
 void hcsr04_init(const sonarHardware_t *sonarHardware);
 
 void hcsr04_start_reading(void);
diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 2a0162b8bd..50cc81b4db 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -30,6 +30,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/sonar_hcsr04.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
@@ -228,7 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
-    int32_t sonarAlt = -1;
+    int32_t sonarAlt = SONAR_OUT_OF_RANGE;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
@@ -265,13 +266,13 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
 #endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+    if (sonarAlt > 0 && sonarAlt < SONAR_MAX_RANGE_ACCURACY_HIGH) {
         baroAlt_offset = BaroAlt - sonarAlt;
         BaroAlt = sonarAlt;
     } else {
         BaroAlt -= baroAlt_offset;
-        if (sonarAlt > 0  && sonarAlt <= 300) {
-            sonarTransition = (300 - sonarAlt) / 100.0f;
+        if (sonarAlt > 0  && sonarAlt <= SONAR_MAX_RANGE_WITH_TILT) {
+            sonarTransition = (SONAR_MAX_RANGE_WITH_TILT - sonarAlt) / 100.0f;
             BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
         }
     }
@@ -305,7 +306,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     }
 #endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+    if (sonarAlt > 0 && sonarAlt < SONAR_MAX_RANGE_ACCURACY_HIGH) {
         // the sonar has the best range
         EstAlt = BaroAlt;
     } else {
diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
index 8921ff2068..30f8d35853 100644
--- a/src/main/sensors/sonar.c
+++ b/src/main/sensors/sonar.c
@@ -30,9 +30,11 @@
 #include "config/config.h"
 
 #include "sensors/sensors.h"
+#include "sensors/battery.h"
 #include "sensors/sonar.h"
 
-// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
+// Inclination is adjusted by imu
 
 #ifdef SONAR
 
@@ -100,7 +102,7 @@ void sonarInit(const sonarHardware_t *sonarHardware)
 {
     hcsr04_init(sonarHardware);
     sensorsSet(SENSOR_SONAR);
-    calculatedAltitude = -1;
+    calculatedAltitude = SONAR_OUT_OF_RANGE;
 }
 
 void sonarUpdate(void)
@@ -109,7 +111,7 @@ void sonarUpdate(void)
 }
 
 /**
- * Get the last distance measured by the sonar in centimeters. When the ground is too far away, -1 is returned instead.
+ * Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned.
  */
 int32_t sonarRead(void)
 {
@@ -120,21 +122,21 @@ int32_t sonarRead(void)
  * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
  * the altitude. Returns the computed altitude in centimeters.
  *
- * When the ground is too far away or the tilt is too strong, -1 is returned instead.
+ * When the ground is too far away or the tilt is too large, SONAR_OUT_OF_RANGE is returned.
  */
-int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
+int32_t sonarCalculateAltitude(int32_t sonarDistance, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
-    if (tiltAngle > 250)
-        calculatedAltitude = -1;
+    if (tiltAngle > SONAR_MAX_TILT_ANGLE)
+        calculatedAltitude = SONAR_OUT_OF_RANGE;
     else
-        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
+        calculatedAltitude = sonarDistance * (900.0f - tiltAngle) / 900.0f;
 
     return calculatedAltitude;
 }
 
 /**
- * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or -1 if sonarCalculateAltitude
+ * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or SONAR_OUT_OF_RANGE if sonarCalculateAltitude
  * has never been called.
  */
 int32_t sonarGetLatestAltitude(void)
diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h
index 333d2842d8..ea4dfafcde 100644
--- a/src/main/sensors/sonar.h
+++ b/src/main/sensors/sonar.h
@@ -16,11 +16,9 @@
  */
 
 #pragma once
-#include "sensors/battery.h"
 
 void sonarUpdate(void);
-
 int32_t sonarRead(void);
-int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarCalculateAltitude(int32_t sonarDistance, int16_t tiltAngle);
 int32_t sonarGetLatestAltitude(void);
 
diff --git a/src/test/Makefile b/src/test/Makefile
index 5bbc50b7be..9b3da6d5f3 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -535,6 +535,37 @@ $(OBJECT_DIR)/baro_bmp280_unittest : \
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
+$(OBJECT_DIR)/drivers/sonar_hcsr04.o : \
+        $(USER_DIR)/drivers/sonar_hcsr04.c \
+        $(USER_DIR)/drivers/sonar_hcsr04.h \
+        $(GTEST_HEADERS)
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/sonar_hcsr04.c -o $@
+
+$(OBJECT_DIR)/sensors/sonar.o : \
+        $(USER_DIR)/sensors/sonar.c \
+        $(USER_DIR)/sensors/sonar.h \
+        $(GTEST_HEADERS)
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/sonar.c -o $@
+
+$(OBJECT_DIR)/sonar_unittest.o : \
+	$(TEST_DIR)/sonar_unittest.cc \
+	$(USER_DIR)/drivers/sonar_hcsr04.h \
+	$(USER_DIR)/sensors/sonar.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/sonar_unittest.cc -o $@
+
+$(OBJECT_DIR)/sonar_unittest : \
+	$(OBJECT_DIR)/drivers/sonar_hcsr04.o \
+	$(OBJECT_DIR)/sensors/sonar.o \
+	$(OBJECT_DIR)/sonar_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
 $(OBJECT_DIR)/sensors/boardalignment.o : \
 	$(USER_DIR)/sensors/boardalignment.c \
 	$(USER_DIR)/sensors/boardalignment.h \
diff --git a/src/test/unit/sonar_unittest.cc b/src/test/unit/sonar_unittest.cc
new file mode 100644
index 0000000000..2b0b1ccf27
--- /dev/null
+++ b/src/test/unit/sonar_unittest.cc
@@ -0,0 +1,40 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+extern "C" {
+    #include "drivers/sonar_hcsr04.h"
+    #include "sensors/sonar.h"
+    #include "build_config.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+TEST(SonarUnittest, TestConstants)
+{
+    // SONAR_OUT_OF_RANGE must be negative
+    EXPECT_LE(SONAR_OUT_OF_RANGE, 0);
+    // Check reasonable values for maximum tilt
+    EXPECT_GE(SONAR_MAX_TILT_ANGLE, 0);
+    EXPECT_LE(SONAR_MAX_TILT_ANGLE, 450);
+    // Check against gross errors in max range constants
+    EXPECT_LE(SONAR_MAX_RANGE_WITH_TILT, SONAR_MAX_RANGE);
+    EXPECT_LE(SONAR_MAX_RANGE_ACCURACY_HIGH, SONAR_MAX_RANGE);
+}
+