Added test code for sonar distance and altitude functions

saperlot · Nov 25, 2015 · 83d3d00 · 83d3d00
1 parent 3e95ecb
commit 83d3d00
Show file tree

Hide file tree

Showing 4 changed files with 69 additions and 9 deletions.
diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c
@@ -19,15 +19,14 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "build_config.h"
 
 #include "system.h"
 #include "gpio.h"
 #include "nvic.h"
 
 #include "sonar_hcsr04.h"
 
-#ifdef SONAR
-
 /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
  * 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
@@ -37,8 +36,12 @@
  *
  */
 
+#if defined(SONAR) || defined(UNIT_TEST)
+STATIC_UNIT_TESTED volatile int32_t measurement = -1;
+#endif
+
+#ifdef SONAR
 static uint32_t lastMeasurementAt;
-static volatile int32_t measurement = -1;
 static sonarHardware_t const *sonarHardware;
 
 static void ECHO_EXTI_IRQHandler(void)
@@ -149,7 +152,8 @@ void hcsr04_start_reading(void)
     delayMicroseconds(11);
     digitalLo(GPIOB, sonarHardware->trigger_pin);
 }
-
+#endif
+#if defined(SONAR) || defined(UNIT_TEST)
 /**
  * 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, SONAR_OUT_OF_RANGE is returned instead.

diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
@@ -93,6 +93,9 @@ 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

diff --git a/src/test/Makefile b/src/test/Makefile
@@ -543,11 +543,12 @@ $(OBJECT_DIR)/drivers/sonar_hcsr04.o : \
 	$(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)
+	$(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 $@
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -DSONAR -c $(USER_DIR)/sensors/sonar.c -o $@
 
 $(OBJECT_DIR)/sonar_unittest.o : \
 	$(TEST_DIR)/sonar_unittest.cc \

diff --git a/src/test/unit/sonar_unittest.cc b/src/test/unit/sonar_unittest.cc
@@ -18,9 +18,10 @@
 #include <stdint.h>
 
 extern "C" {
+    #include "build_config.h"
     #include "drivers/sonar_hcsr04.h"
     #include "sensors/sonar.h"
-    #include "build_config.h"
+    extern int32_t measurement;
 }
 
 #include "unittest_macros.h"
@@ -38,3 +39,54 @@ TEST(SonarUnittest, TestConstants)
     EXPECT_LE(SONAR_MAX_RANGE_ACCURACY_HIGH, SONAR_MAX_RANGE);
 }
 
+TEST(SonarUnittest, TestDistance)
+{
+    // Check sonar pulse time converted correctly to cm
+    const int returnMicroSecondsPerCm = 59;
+    measurement =  0;
+    EXPECT_EQ(hcsr04_get_distance(), 0);
+
+    measurement =  returnMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), 1);
+
+    measurement =  10 * returnMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), 10);
+
+    measurement =  SONAR_MAX_RANGE_WITH_TILT * returnMicroSecondsPerCm;
+    EXPECT_EQ(hcsr04_get_distance(), SONAR_MAX_RANGE_WITH_TILT);
+}
+
+TEST(SonarUnittest, TestAltitude)
+{
+    // Check distance not modified if no tilt
+    EXPECT_EQ(sonarCalculateAltitude(0, 0), 0);
+    EXPECT_EQ(sonarGetLatestAltitude(), 0);
+    EXPECT_EQ(sonarCalculateAltitude(100, 0), 100);
+    EXPECT_EQ(sonarGetLatestAltitude(), 100);
+
+    // Check that out of range is returned if tilt is too large
+    EXPECT_EQ(sonarCalculateAltitude(0, SONAR_MAX_TILT_ANGLE+1), SONAR_OUT_OF_RANGE);
+    EXPECT_EQ(sonarGetLatestAltitude(), SONAR_OUT_OF_RANGE);
+
+    // Check distance at various tilt angles
+    // distance 400, 5 degrees of tilt
+    EXPECT_EQ(sonarCalculateAltitude(400, 50), 377);
+    EXPECT_EQ(sonarGetLatestAltitude(), 377);
+    // distance 400, 10 degrees of tilt
+    EXPECT_EQ(sonarCalculateAltitude(400, 100), 355);
+    EXPECT_EQ(sonarGetLatestAltitude(), 355);
+    // distance 400, 20 degrees of tilt
+    EXPECT_EQ(sonarCalculateAltitude(400, 200), 311);
+    EXPECT_EQ(sonarGetLatestAltitude(), 311);
+    // distance 400, maximum tilt
+    EXPECT_EQ(sonarCalculateAltitude(400, SONAR_MAX_TILT_ANGLE), 288);
+    EXPECT_EQ(sonarGetLatestAltitude(), 288);
+}
+
+// STUBS
+extern "C" {
+void sensorsSet(uint32_t mask) {UNUSED(mask);}
+void hcsr04_init(const sonarHardware_t *initialSonarHardware) {UNUSED(initialSonarHardware);}
+void hcsr04_start_reading(void) {}
+}
+