From db14bd80cbbac328adf2008ea389d7e847fc7a51 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 24 Dec 2014 01:30:47 +0000 Subject: [PATCH] Cleanup sonar driver * spend less time in interrupt handler. * avoid pointer usage to prevent the inclination adjusted reading being replaced by the interrupt handler's calculation. * only calculate the actual distance based on the measurement when required. --- src/main/drivers/sonar_hcsr04.c | 55 +++++++++++++++++++---------------------- src/main/drivers/sonar_hcsr04.h | 3 ++- src/main/flight/altitudehold.c | 3 +++ src/main/sensors/sonar.c | 8 ++++-- src/main/sensors/sonar.h | 1 + 5 files changed, 38 insertions(+), 32 deletions(-) diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 758194d39..ee1c78904 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -36,35 +36,21 @@ * */ -static uint32_t last_measurement; -static volatile int32_t *distance_ptr = 0; - -extern int16_t debug[4]; - +static uint32_t lastMeasurementAt; +static volatile int32_t measurement = -1; static sonarHardware_t const *sonarHardware; void ECHO_EXTI_IRQHandler(void) { static uint32_t timing_start; uint32_t timing_stop; + if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) { timing_start = micros(); } else { timing_stop = micros(); if (timing_stop > timing_start) { - // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. - // The ping travels out and back, so to find the distance of the - // object we take half of the distance traveled. - // - // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 - int32_t distance = (timing_stop - timing_start) / 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_ptr) { - *distance_ptr = distance; - } + measurement = timing_stop - timing_start; } } @@ -115,30 +101,41 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware) NVIC_EnableIRQ(sonarHardware->exti_irqn); - last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance() + lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance() } -// distance calculation is done asynchronously, using interrupt -void hcsr04_get_distance(volatile int32_t *distance) +// measurement reading is done asynchronously, using interrupt +void hcsr04_start_reading(void) { - uint32_t current_time = millis(); + uint32_t now = millis(); - if (current_time < (last_measurement + 60)) { + if (now < (lastMeasurementAt + 60)) { // the repeat interval of trig signal should be greater than 60ms // to avoid interference between connective measurements. return; } - last_measurement = current_time; - distance_ptr = distance; - -#if 0 - debug[0] = *distance; -#endif + lastMeasurementAt = now; digitalHi(GPIOB, sonarHardware->trigger_pin); // The width of trig signal must be greater than 10us delayMicroseconds(11); digitalLo(GPIOB, sonarHardware->trigger_pin); } + +int32_t hcsr04_get_distance(void) +{ + // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter. + // The ping travels out and back, so to find the distance of the + // object we take half of the distance traveled. + // + // 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 9a72bfc59..9ca57d397 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -27,4 +27,5 @@ typedef struct sonarHardware_s { void hcsr04_init(const sonarHardware_t *sonarHardware); -void hcsr04_get_distance(volatile int32_t *distance); +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 4d2257a01..d2bb2b146 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -46,6 +46,8 @@ #include "config/runtime_config.h" +extern int16_t debug[4]; + int32_t setVelocity = 0; uint8_t velocityControl = 0; int32_t errorVelocityI = 0; @@ -258,6 +260,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) #ifdef SONAR tiltAngle = calculateTiltAngle(&inclination); + sonarAlt = sonarRead(); sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle); #endif diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 0f002e0d1..518cb5605 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -33,7 +33,7 @@ #include "sensors/sensors.h" #include "sensors/sonar.h" -int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range +int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range - inclination adjusted by imu #ifdef SONAR @@ -79,7 +79,7 @@ void Sonar_init(void) void Sonar_update(void) { - hcsr04_get_distance(&sonarAlt); + hcsr04_start_reading(); } int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle) @@ -91,4 +91,8 @@ int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle) return sonarAlt * (900.0f - tiltAngle) / 900.0f; } +int32_t sonarRead(void) { + return hcsr04_get_distance(); +} + #endif diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 45987e754..a6a2a8b94 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -22,4 +22,5 @@ extern int32_t sonarAlt; void Sonar_init(void); void Sonar_update(void); +int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle); -- 2.11.4.GIT