Merge pull request #1518 from martinbudden/sonarcleanup

Sonarcleanup - replaced Sonar magic numbers with #defines. First cut of sonar test code.

README.md

@@ -105,7 +105,7 @@ Before creating new issues please check to see if there is an existing one, sear
 
 ## Developers
 
-Please refer to the development section in the `docs/development` folder.
+Please refer to the development section in the [docs/development](https://github.com/cleanflight/cleanflight/tree/master/docs/development) folder.
 
 TravisCI is used to run automatic builds
 

src/main/common/atomic.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-// only set_BASEPRI is implemented in device library. It does always create memory barrirer
+// only set_BASEPRI is implemented in device library. It does always create memory barrier
 // missing versions are implemented here
 
 // set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
@@ -68,10 +68,10 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
                                      __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
-// Be carefull when using this, you must use some method to prevent optimizer form breaking things
+// Be careful when using this, you must use some method to prevent optimizer form breaking things
-// - lto is used for baseflight compillation, so function call is not memory barrier
+// - lto is used for Cleanflight compilation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or propper volatile to protect used variables
+// - use ATOMIC_BARRIER or proper volatile to protect used variables
-// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much
+// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
 //    but that can change in future versions
 #define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
                                     __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
@@ -80,7 +80,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 // Create memory barrier
 // - at the beginning (all data must be reread from memory)
 // - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
-// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier
+// ideally this would only protect memory passed as parameter (any type should work), but gcc is currently creating almost full barrier
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
 #if (__GNUC__ > 4)
@@ -96,7 +96,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 # define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
 #endif
 
-// this macro uses local function for cleanup. CLang block can be substituded
+// this macro uses local function for cleanup. CLang block can be substituted
 #define ATOMIC_BARRIER(data)                                            \
     __extension__ void  __UNIQL(__barrierEnd)(typeof(data) **__d) {     \
         __asm__ volatile ("\t# barier(" #data ")  end\n" : : "m" (**__d));                          \

src/main/drivers/sonar_hcsr04.c

@@ -19,6 +19,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "build_config.h"
 
 #include "system.h"
 #include "gpio.h"
@@ -26,21 +27,21 @@
 
 #include "sonar_hcsr04.h"
 
-#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
+ * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
- * echo froman object. The distance between the unit and the object is calculated
+ * 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 ***
  *
  */
 
+#if defined(SONAR)
+STATIC_UNIT_TESTED volatile int32_t measurement = -1;
 static uint32_t lastMeasurementAt;
-static volatile int32_t measurement = -1;
 static sonarHardware_t const *sonarHardware;
 
+#if !defined(UNIT_TEST)
 static void ECHO_EXTI_IRQHandler(void)
 {
     static uint32_t timing_start;
@@ -72,14 +73,19 @@ void EXTI9_5_IRQHandler(void)
 {
     ECHO_EXTI_IRQHandler();
 }
+#endif
 
-void hcsr04_init(const sonarHardware_t *initialSonarHardware)
+void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
 {
+    sonarHardware = initialSonarHardware;
+    sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
+    sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
+    sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+#if !defined(UNIT_TEST)
     gpio_config_t gpio;
     EXTI_InitTypeDef EXTIInit;
 
-    sonarHardware = initialSonarHardware;
-
 #ifdef STM32F10X
     // enable AFIO for EXTI support
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
@@ -129,11 +135,15 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
     NVIC_Init(&NVIC_InitStructure);
 
     lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
+#else
+    lastMeasurementAt = 0; // to avoid "unused" compiler warning
+#endif
 }
 
 // measurement reading is done asynchronously, using interrupt
 void hcsr04_start_reading(void)
 {
+#if !defined(UNIT_TEST)
     uint32_t now = millis();
 
     if (now < (lastMeasurementAt + 60)) {
@@ -148,11 +158,11 @@ void hcsr04_start_reading(void)
     //  The width of trig signal must be greater than 10us
     delayMicroseconds(11);
     digitalLo(GPIOB, sonarHardware->trigger_pin);
+#endif
 }
 
 /**
- * Get the distance that was measured by the last pulse, in centimeters. When the ground is too far away to be
+ * Get the distance that was measured by the last pulse, in centimeters.
- * reliably read by the sonar, -1 is returned instead.
  */
 int32_t hcsr04_get_distance(void)
 {
@@ -163,10 +173,6 @@ int32_t hcsr04_get_distance(void)
     // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
     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;
-
     return distance;
 }
 #endif

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;
@@ -25,9 +27,19 @@ typedef struct sonarHardware_s {
     IRQn_Type exti_irqn;
 } sonarHardware_t;
 
+typedef struct sonarRange_s {
+    int16_t maxRangeCm;
+    // these are full detection cone angles, maximum tilt is half of this
+    int16_t detectionConeDeciDegrees; // detection cone angle as in HC-SR04 device spec
+    int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
+} sonarRange_t;
+
 #define SONAR_GPIO GPIOB
 
-void hcsr04_init(const sonarHardware_t *sonarHardware);
+#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
+#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
+#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
 
+void hcsr04_init(const sonarHardware_t *sonarHardware, sonarRange_t *sonarRange);
 void hcsr04_start_reading(void);
 int32_t hcsr04_get_distance(void);

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"
@@ -173,16 +174,6 @@ 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;
@@ -228,7 +219,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;
@@ -237,10 +228,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     static int32_t baroAlt_offset = 0;
     float sonarTransition;
 
-#ifdef SONAR
-    int16_t tiltAngle;
-#endif
-
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -260,18 +247,19 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 #endif
 
 #ifdef SONAR
-    tiltAngle = calculateTiltAngle(&inclination);
     sonarAlt = sonarRead();
-    sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
+    sonarAlt = sonarCalculateAltitude(sonarAlt, inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees);
 #endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+    if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
+        // just use the SONAR
         baroAlt_offset = BaroAlt - sonarAlt;
         BaroAlt = sonarAlt;
     } else {
         BaroAlt -= baroAlt_offset;
-        if (sonarAlt > 0  && sonarAlt <= 300) {
+        if (sonarAlt > 0  && sonarAlt <= sonarMaxAltWithTiltCm) {
-            sonarTransition = (300 - sonarAlt) / 100.0f;
+            // SONAR in range, so use complementary filter
+            sonarTransition = (sonarMaxAltWithTiltCm - sonarAlt) / 100.0f;
             BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
         }
     }
@@ -305,7 +293,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     }
 #endif
 
-    if (sonarAlt > 0 && sonarAlt < 200) {
+    if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
         // the sonar has the best range
         EstAlt = BaroAlt;
     } else {

src/main/mw.c

@@ -393,7 +393,7 @@ void updateInflightCalibrationState(void)
         InflightcalibratingA = 50;
         AccInflightCalibrationArmed = false;
     }
-    if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
+    if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
             InflightcalibratingA = 50;
         AccInflightCalibrationActive = true;

src/main/sensors/sonar.c

@@ -30,11 +30,19 @@
 #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
+    float baro_cf_vel;                      // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
+    float baro_cf_alt;                      // apply CF to use ACC for height estimation
 
 #ifdef SONAR
+int16_t sonarMaxRangeCm;
+int16_t sonarMaxAltWithTiltCm;
+int16_t sonarCfAltCm; // Complimentary Filter altitude
+STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
 
 static int32_t calculatedAltitude;
 
@@ -91,16 +99,30 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
         .exti_irqn = EXTI1_IRQn
     };
     return &sonarHardware;
+#elif defined(UNIT_TEST)
+    UNUSED(batteryConfig);
+    return 0;
 #else
 #error Sonar not defined for target
 #endif
 }
 
+// (PI/1800)^2/2, coefficient of x^2 in Taylor expansion of cos(x)
+#define coefX2 1.52309E-06f
+#define cosDeciDegrees(x) (1.0f - x * x * coefX2)
+
+
 void sonarInit(const sonarHardware_t *sonarHardware)
 {
-    hcsr04_init(sonarHardware);
+    sonarRange_t sonarRange;
+
+    hcsr04_init(sonarHardware, &sonarRange);
     sensorsSet(SENSOR_SONAR);
-    calculatedAltitude = -1;
+    sonarMaxRangeCm = sonarRange.maxRangeCm;
+    sonarCfAltCm = sonarMaxRangeCm / 2;
+    sonarMaxTiltDeciDegrees =  sonarRange.detectionConeExtendedDeciDegrees / 2;
+    sonarMaxAltWithTiltCm = sonarMaxRangeCm * cosDeciDegrees(sonarMaxTiltDeciDegrees);
+    calculatedAltitude = SONAR_OUT_OF_RANGE;
 }
 
 void sonarUpdate(void)
@@ -109,32 +131,46 @@ 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)
 {
-    return hcsr04_get_distance();
+    int32_t distance = hcsr04_get_distance();
+    if (distance > HCSR04_MAX_RANGE_CM)
+        distance = SONAR_OUT_OF_RANGE;
+    return distance;
+}
+
+/*
+* 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 sonarCalculateTiltAngle(int16_t rollDeciDegrees, int16_t pitchDeciDegrees)
+{
+    return MAX(ABS(rollDeciDegrees), ABS(pitchDeciDegrees));
 }
 
 /**
  * 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 rollDeciDegrees, int16_t pitchDeciDegrees)
 {
-    // calculate sonar altitude only if the sonar is facing downwards(<25deg)
+    int16_t tiltAngle = sonarCalculateTiltAngle(rollDeciDegrees, pitchDeciDegrees);
-    if (tiltAngle > 250)
+    // calculate sonar altitude only if the ground is in the sonar cone
-        calculatedAltitude = -1;
+    if (tiltAngle > sonarMaxTiltDeciDegrees)
+        calculatedAltitude = SONAR_OUT_OF_RANGE;
     else
-        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
+        // altitude = distance * cos(tiltAngle), use approximation
-
+        calculatedAltitude = sonarDistance * cosDeciDegrees(tiltAngle);
     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)

src/main/sensors/sonar.h

@@ -16,11 +16,16 @@
  */
 
 #pragma once
-#include "sensors/battery.h"
+
+#define SONAR_OUT_OF_RANGE (-1)
+
+extern int16_t sonarMaxRangeCm;
+extern int16_t sonarCfAltCm;
+extern int16_t sonarMaxAltWithTiltCm;
 
 void sonarUpdate(void);
-
 int32_t sonarRead(void);
-int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int16_t sonarCalculateTiltAngle(int16_t rollDeciDegrees, int16_t pitchDeciDegrees);
+int32_t sonarCalculateAltitude(int32_t sonarDistance, int16_t rollDeciDegrees, int16_t pitchDeciDegrees);
 int32_t sonarGetLatestAltitude(void);
 

src/test/Makefile

@@ -535,6 +535,38 @@ $(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) -DSONAR -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) -DSONAR -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 \

src/test/unit/altitude_hold_unittest.cc

@@ -99,36 +99,6 @@ 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" {
@@ -155,6 +125,8 @@ uint16_t flightModeFlags;
 uint8_t armingFlags;
 
 int32_t sonarAlt;
+int16_t sonarCfAltCm;
+int16_t sonarMaxAltWithTiltCm;
 
 
 uint16_t enableFlightMode(flightModeFlags_e mask)

src/test/unit/flight_imu_unittest.cc

@@ -91,6 +91,8 @@ uint16_t flightModeFlags;
 uint8_t armingFlags;
 
 int32_t sonarAlt;
+int16_t sonarCfAltCm;
+int16_t sonarMaxAltWithTiltCm;
 int16_t accADC[XYZ_AXIS_COUNT];
 int16_t gyroADC[XYZ_AXIS_COUNT];
 

src/test/unit/sonar_unittest.cc

@@ -0,0 +1,149 @@
+/*
+ * 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 "build_config.h"
+    #include "drivers/sonar_hcsr04.h"
+    #include "sensors/sonar.h"
+    extern int32_t measurement;
+    extern int16_t sonarMaxTiltDeciDegrees;
+    void sonarInit(const sonarHardware_t *sonarHardware);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+TEST(SonarUnittest, TestConstants)
+{
+    sonarInit(0);
+    // SONAR_OUT_OF_RANGE must be negative
+    EXPECT_LE(SONAR_OUT_OF_RANGE, 0);
+    // Check against gross errors in max range constants
+    EXPECT_GT(HCSR04_MAX_RANGE_CM, 100);
+}
+
+TEST(SonarUnittest, TestSonarInit)
+{
+    sonarInit(0);
+    EXPECT_EQ(sonarMaxRangeCm, HCSR04_MAX_RANGE_CM);
+    // Check against gross errors in max range values
+    EXPECT_GE(sonarMaxAltWithTiltCm, 100);
+    EXPECT_LE(sonarMaxAltWithTiltCm, sonarMaxRangeCm);
+    EXPECT_GE(sonarCfAltCm, 100);
+    EXPECT_LE(sonarCfAltCm, sonarMaxRangeCm);
+    EXPECT_LE(sonarCfAltCm, sonarMaxAltWithTiltCm);
+    // Check reasonable values for maximum tilt
+    EXPECT_GE(sonarMaxTiltDeciDegrees, 0);
+    EXPECT_LE(sonarMaxTiltDeciDegrees, 450);
+}
+
+TEST(SonarUnittest, TestDistance)
+{
+    // Check sonar pulse time converted correctly to cm
+    const int echoMicroSecondsPerCm = 59;
+    measurement =  0;
+    EXPECT_EQ(hcsr04_get_distance(), 0);
+
+    measurement =  echoMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), 1);
+
+    measurement =  10 * echoMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), 10);
+
+    measurement =  HCSR04_MAX_RANGE_CM * echoMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), HCSR04_MAX_RANGE_CM);
+}
+
+TEST(SonarUnittest, TestAltitude)
+{
+    sonarInit(0);
+    // Check distance not modified if no tilt
+    EXPECT_EQ(sonarCalculateAltitude(0, 0, 0), 0);
+    EXPECT_EQ(sonarGetLatestAltitude(), 0);
+    EXPECT_EQ(sonarCalculateAltitude(100, 0, 0), 100);
+    EXPECT_EQ(sonarGetLatestAltitude(), 100);
+
+    // Check that out of range is returned if tilt is too large
+    EXPECT_EQ(sonarCalculateAltitude(0, sonarMaxTiltDeciDegrees+1, 0), SONAR_OUT_OF_RANGE);
+    EXPECT_EQ(sonarGetLatestAltitude(), SONAR_OUT_OF_RANGE);
+
+    // Check distance at various roll angles
+    // distance 400, 5 degrees of roll
+    EXPECT_EQ(sonarCalculateAltitude(400, 50, 0), 398);
+    EXPECT_EQ(sonarGetLatestAltitude(), 398);
+    // distance 400, 10 degrees of roll
+    EXPECT_EQ(sonarCalculateAltitude(400, 100, 0), 393);
+    EXPECT_EQ(sonarGetLatestAltitude(), 393);
+    // distance 400, 15 degrees of roll, this corresponds to HC-SR04 specified max detection angle
+    EXPECT_EQ(sonarCalculateAltitude(400, 150, 0), 386);
+    EXPECT_EQ(sonarGetLatestAltitude(), 386);
+    // distance 400, 20 degrees of roll
+    EXPECT_EQ(sonarCalculateAltitude(400, 200, 0), 375);
+    EXPECT_EQ(sonarGetLatestAltitude(), 375);
+    // distance 400, 22.5 degrees of roll, this corresponds to HC-SR04 effective max detection angle
+    EXPECT_EQ(sonarCalculateAltitude(400, 225, 0), 369);
+    EXPECT_EQ(sonarGetLatestAltitude(), 369);
+    // max range, max tilt
+    EXPECT_EQ(sonarCalculateAltitude(sonarMaxRangeCm, sonarMaxTiltDeciDegrees, 0), sonarMaxAltWithTiltCm);
+    EXPECT_EQ(sonarGetLatestAltitude(), sonarMaxAltWithTiltCm);
+}
+
+typedef struct rollAndPitch_s {
+    // absolute angle inclination in multiple of 0.1 degree (deci degrees): 180 degrees = 1800
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+} rollAndPitch_t;
+
+typedef struct inclinationAngleExpectations_s {
+    rollAndPitch_t inclination;
+    int16_t expected_angle;
+} inclinationAngleExpectations_t;
+
+TEST(SonarUnittest, TestCalculateTiltAngle)
+{
+    const int testCount = 9;
+    inclinationAngleExpectations_t inclinationAngleExpectations[testCount] = {
+        { { 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}
+    };
+
+    rollAndPitch_t inclination = {0, 0};
+    int tilt_angle = sonarCalculateTiltAngle(inclination.rollDeciDegrees, inclination.pitchDeciDegrees);
+    EXPECT_EQ(tilt_angle, 0);
+
+    for (int i = 0; i < testCount; i++) {
+        inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
+        int result = sonarCalculateTiltAngle(expectation->inclination.rollDeciDegrees, expectation->inclination.pitchDeciDegrees);
+        EXPECT_EQ(expectation->expected_angle, result);
+    }
+}
+
+
+// STUBS
+extern "C" {
+void sensorsSet(uint32_t mask) {UNUSED(mask);}
+}
+