Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Fixed rangefinder reading.

Browse source
This commit is contained in:
mikeller 2020-06-01 15:19:44 +12:00
parent 6bada9c5e9
commit 4923057eb3
6 changed files with 62 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

70
src/main/drivers/rangefinder/rangefinder_hcsr04.c
View file

@ -25,19 +25,17 @@
#if defined(USE_RANGEFINDER_HCSR04)
#include "build/build_config.h"
#include "build/debug.h"
#include "common/time.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "rangefinder_hcsr04.h"
#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
@ -55,12 +53,10 @@
static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
static volatile timeMs_t lastMeasurementReceivedAt;
static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
static timeMs_t lastMeasurementStartedAt = 0;
#ifdef USE_EXTI
static extiCallbackRec_t hcsr04_extiCallbackRec;
#endif
static IO_t echoIO;
static IO_t triggerIO;
@ -68,15 +64,18 @@ static IO_t triggerIO;
#if !defined(UNIT_TEST)
void hcsr04_extiHandler(extiCallbackRec_t* cb)
{
static timeUs_t timing_start;
UNUSED(cb);
static timeUs_t timing_start;
const timeUs_t currentTimeUs = microsISR();
if (IORead(echoIO) != 0) {
timing_start = micros();
timing_start = currentTimeUs;
} else {
const timeUs_t timing_stop = micros();
const timeUs_t timing_stop = currentTimeUs;
if (timing_stop > timing_start) {
lastMeasurementReceivedAt = millis();
lastMeasurementReceivedAt = currentTimeUs / 1000;
hcsr04SonarPulseTravelTime = timing_stop - timing_start;
}
}
@ -88,7 +87,7 @@ void hcsr04_init(rangefinderDev_t *dev)
UNUSED(dev);
}
#define HCSR04_MinimumFiringIntervalMs 60
#define HCSR04_MINIMUM_FIRING_INTERVAL_MS 60
/*
* Start a range reading
@ -98,28 +97,20 @@ void hcsr04_init(rangefinderDev_t *dev)
void hcsr04_start_reading(void)
{
#if !defined(UNIT_TEST)
#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
IOLo(triggerIO);
delayMicroseconds(11);
IOHi(triggerIO);
#else
IOHi(triggerIO);
delayMicroseconds(11);
IOLo(triggerIO);
#endif
#endif
}
void hcsr04_update(rangefinderDev_t *dev)
{
UNUSED(dev);
const timeMs_t timeNowMs = millis();
// the firing interval of the trigger signal should be greater than 60ms
// to avoid interference between consecutive measurements
if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
// We should have a valid measurement within 60ms of trigger
if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
if (lastMeasurementReceivedAt > lastMeasurementStartedAt && (lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
// 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.
@ -129,12 +120,14 @@ void hcsr04_update(rangefinderDev_t *dev)
if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
}
}
else {
} else if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) > HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
// No measurement within reasonable time - indicate failure
lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
}
// the firing interval of the trigger signal should be greater than 60ms
// to avoid interference between consecutive measurements
if (timeNowMs > lastMeasurementStartedAt + HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
// Trigger a new measurement
lastMeasurementStartedAt = timeNowMs;
hcsr04_start_reading();
@ -147,30 +140,22 @@ void hcsr04_update(rangefinderDev_t *dev)
int32_t hcsr04_get_distance(rangefinderDev_t *dev)
{
UNUSED(dev);
return lastCalculatedDistance;
const int32_t result = lastCalculatedDistance;
lastCalculatedDistance = RANGEFINDER_NO_NEW_DATA;
return result;
}
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
{
bool detected = false;
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
#endif
#if defined(STM32F3) || defined(STM32F4)
RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
#endif
triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
if (IOGetOwner(triggerIO) != OWNER_FREE) {
return false;
}
if (IOGetOwner(echoIO) != OWNER_FREE) {
if (IOGetOwner(triggerIO) || IOGetOwner(echoIO)) {
return false;
}
@ -190,7 +175,7 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
timeMs_t requestTime = millis();
hcsr04_start_reading();
while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
while ((millis() - requestTime) < HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
if (IORead(echoIO) == true) {
detected = true;
break;
@ -201,11 +186,9 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
if (detected) {
// Hardware detected - configure the driver
#ifdef USE_EXTI
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_BOTH); // TODO - priority!
EXTIEnable(echoIO, true);
#endif
dev->delayMs = 100;
dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
@ -225,5 +208,4 @@ bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwa
return false;
}
}
#endif

17
src/main/fc/tasks.c
View file

@ -189,6 +189,21 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_RANGEFINDER)
void taskUpdateRangefinder(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (!sensors(SENSOR_RANGEFINDER)) {
return;
}
rangefinderUpdate();
rangefinderProcess(getCosTiltAngle());
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
static void taskCalculateAltitude(timeUs_t currentTimeUs)
{
@ -476,7 +491,7 @@ task_t tasks[TASK_COUNT] = {
#endif
#ifdef USE_RANGEFINDER
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, rangefinderUpdate, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
#endif
};

10
src/main/sensors/rangefinder.c
View file

@ -254,16 +254,11 @@ static int16_t computePseudoSnr(int32_t newReading) {
/*
* This is called periodically by the scheduler
*/
// XXX Returns timeDelta_t for iNav for pseudo-RT scheduling.
void rangefinderUpdate(timeUs_t currentTimeUs)
void rangefinderUpdate(void)
{
UNUSED(currentTimeUs);
if (rangefinder.dev.update) {
rangefinder.dev.update(&rangefinder.dev);
}
// return rangefinder.dev.delayMs * 1000; // to microseconds XXX iNav only
}
bool isSurfaceAltitudeValid() {
@ -310,8 +305,7 @@ bool rangefinderProcess(float cosTiltAngle)
if (distance >= 0) {
rangefinder.lastValidResponseTimeMs = millis();
rangefinder.rawAltitude = applyMedianFilter(distance);
}
else if (distance == RANGEFINDER_OUT_OF_RANGE) {
} else if (distance == RANGEFINDER_OUT_OF_RANGE) {
rangefinder.lastValidResponseTimeMs = millis();
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
}

4
src/main/sensors/rangefinder.h
View file

@ -56,14 +56,12 @@ typedef struct rangefinder_s {
int16_t snr;
} rangefinder_t;
extern rangefinder_t rangefinder;
void rangefinderResetDynamicThreshold(void);
bool rangefinderInit(void);
int32_t rangefinderGetLatestAltitude(void);
int32_t rangefinderGetLatestRawAltitude(void);
void rangefinderUpdate(timeUs_t currentTimeUs);
void rangefinderUpdate(void);
bool rangefinderProcess(float cosTiltAngle);
bool rangefinderIsHealthy(void);

1
src/main/target/STM32_UNIFIED/target.h
View file

@ -210,7 +210,6 @@
#define USE_TRANSPONDER
//TODO: Make this actually work by making the pins configurable
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define USE_RANGEFINDER_TF

5
src/main/target/common_post.h
View file

@ -378,6 +378,11 @@ extern uint8_t __config_end;
#define USE_CUSTOM_DEFAULTS_ADDRESS
#endif
#if !defined(USE_EXTI)
#undef USE_RX_SPI
#undef USE_RANGEFINDER_HCSR04
#endif
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#define USE_RX_BIND
#endif