From 3e95ecbba9cad52e11e6c62a330d3277ac99e7da Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Nov 2015 23:26:58 +0000 Subject: [PATCH] Replaced Sonar magic numbers with #defines. Added first cut of Sonar test code. Corrected spelling in comments. Minor improvements to comments. --- src/main/drivers/sonar_hcsr04.c | 10 ++++----- src/main/drivers/sonar_hcsr04.h | 8 +++++++ src/main/flight/altitudehold.c | 11 ++++----- src/main/sensors/sonar.c | 20 +++++++++-------- src/main/sensors/sonar.h | 4 +--- src/test/Makefile | 31 +++++++++++++++++++++++++ src/test/unit/sonar_unittest.cc | 40 +++++++++++++++++++++++++++++++++ 7 files changed, 102 insertions(+), 22 deletions(-) create mode 100644 src/test/unit/sonar_unittest.cc 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 . + */ + +#include + +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); +} +