diff --git a/README.md b/README.md index bc0b041448..4ec53d6be2 100644 --- a/README.md +++ b/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 diff --git a/src/main/common/atomic.h b/src/main/common/atomic.h index 00008b585c..556f2ee966 100644 --- a/src/main/common/atomic.h +++ b/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 -// - lto is used for baseflight compillation, so function call is not memory barrier -// - use ATOMIC_BARRIER or propper 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 +// Be careful when using this, you must use some method to prevent optimizer form breaking things +// - lto is used for Cleanflight compilation, so function call is not memory barrier +// - use ATOMIC_BARRIER or proper volatile to protect used variables +// - 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)); \ diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 663e1c3531..278a8576b6 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -19,6 +19,7 @@ #include #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 - * 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 *** * */ +#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 - * reliably read by the sonar, -1 is returned instead. + * Get the distance that was measured by the last pulse, in centimeters. */ 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 diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index 9315668a75..a39f0f61d5 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; @@ -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); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 2a0162b8bd..5988f1e008 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" @@ -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) { - sonarTransition = (300 - sonarAlt) / 100.0f; + if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) { + // 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 { diff --git a/src/main/mw.c b/src/main/mw.c index 511c4a9c88..925175f567 100644 --- a/src/main/mw.c +++ b/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; diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 8921ff2068..730c1f7a53 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -30,15 +30,23 @@ #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; -const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) +const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) { #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R) static const sonarHardware_t sonarPWM56 = { @@ -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) - if (tiltAngle > 250) - calculatedAltitude = -1; + int16_t tiltAngle = sonarCalculateTiltAngle(rollDeciDegrees, pitchDeciDegrees); + // calculate sonar altitude only if the ground is in the sonar cone + 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) diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 333d2842d8..fc379ef13f 100644 --- a/src/main/sensors/sonar.h +++ b/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); diff --git a/src/test/Makefile b/src/test/Makefile index 5bbc50b7be..2e5f28bbeb 100644 --- a/src/test/Makefile +++ b/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 \ diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index 02bf80b9fa..aad9b7df47 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/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) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c575a74f59..ee4506d748 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/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]; diff --git a/src/test/unit/sonar_unittest.cc b/src/test/unit/sonar_unittest.cc new file mode 100644 index 0000000000..5f5a0aca50 --- /dev/null +++ b/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 . + */ + +#include + +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);} +} +