Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Merge pull request #1518 from martinbudden/sonarcleanup

Browse source 
Sonarcleanup - replaced Sonar magic numbers with #defines. First cut of sonar test code.
This commit is contained in:
Dominic Clifton 2015-11-28 13:23:06 +00:00
commit 50f3be3d19
12 changed files with 294 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

2
README.md
View file

@ -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

14
src/main/common/atomic.h
View file

@ -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)); \

34
src/main/drivers/sonar_hcsr04.c
View file

@ -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
* 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

14
src/main/drivers/sonar_hcsr04.h
View file

@ -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);

30
src/main/flight/altitudehold.c
View file

@ -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 {

2
src/main/mw.c
View file

@ -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;

62
src/main/sensors/sonar.c
View file

@ -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)
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)

11
src/main/sensors/sonar.h
View file

@ -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);

32
src/test/Makefile
View file

@ -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 \

32
src/test/unit/altitude_hold_unittest.cc
View file

@ -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)

2
src/test/unit/flight_imu_unittest.cc
View file

@ -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];

149
src/test/unit/sonar_unittest.cc Normal file
View file

@ -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);}
}