From 8ab940a7c59c9fccb17a0ba4efb747c9c5aadae1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 5 Jul 2017 23:36:56 +0200 Subject: [PATCH 1/4] Connected over I2C to HC-SR04 via I2C --- Makefile | 1 + src/main/drivers/rangefinder_hcsr04_i2c.c | 96 +++++++++++++++++++++++ src/main/drivers/rangefinder_hcsr04_i2c.h | 22 ++++++ src/main/fc/cli.c | 2 +- src/main/sensors/rangefinder.c | 12 ++- src/main/sensors/rangefinder.h | 7 +- src/main/target/AIRBOTF4/target.h | 4 + 7 files changed, 138 insertions(+), 6 deletions(-) create mode 100644 src/main/drivers/rangefinder_hcsr04_i2c.c create mode 100644 src/main/drivers/rangefinder_hcsr04_i2c.h diff --git a/Makefile b/Makefile index aca7ea3b5e..f27031e703 100644 --- a/Makefile +++ b/Makefile @@ -650,6 +650,7 @@ HIGHEND_SRC = \ common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ drivers/rangefinder_hcsr04.c \ + drivers/rangefinder_hcsr04_i2c.c \ drivers/rangefinder_srf10.c \ io/dashboard.c \ io/displayport_max7456.c \ diff --git a/src/main/drivers/rangefinder_hcsr04_i2c.c b/src/main/drivers/rangefinder_hcsr04_i2c.c new file mode 100644 index 0000000000..18f832c268 --- /dev/null +++ b/src/main/drivers/rangefinder_hcsr04_i2c.c @@ -0,0 +1,96 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_HCSR04_I2C) + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "drivers/rangefinder.h" +#include "drivers/rangefinder_hcsr04_i2c.h" + +#include "build/debug.h" + +#ifndef RANGEFINDER_HCSR04_I2C_I2C_INSTANCE +#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE I2CDEV_1 +#endif + +#define HCSR04_I2C_MAX_RANGE_CM 400 +#define HCSR04_I2C_DETECTION_CONE_DECIDEGREES 300 +#define HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450 + +#define HCSR04_I2C_Address 0x14 + +volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; +// static bool isSensorResponding = true; + +static void hcsr04i2c_init(void) +{ + // timeOfLastMeasurementMs = millis(); +} + +void hcsr04i2c_update(void) +{ + uint8_t buf; + + i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 0, 1, &buf); + debug[0] = buf; + i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 1, 1, &buf); + debug[1] = buf; + i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 2, 1, &buf); + debug[2] = buf; + + debug[3] = millis(); +} + +/** + * Get the distance that was measured by the last pulse, in centimeters. + */ +static int32_t hcsr04i2c_get_distance(void) +{ + // uint8_t buf[3]; + + // bool isSensorResponding = i2cRead(RANGEFINDER_HCSR04_I2C_I2C_INSTANCE, HCSR04_I2C_AddressI2C, 0, 3, &byte); + + // debug[0] = buf[0]; + // debug[1] = buf[1]; + // debug[2] = buf[3]; + return 7; + +} + +bool hcsr04i2c0Detect(rangefinderDev_t *dev) +{ + dev->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS; + dev->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM; + dev->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES; + dev->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES; + + dev->init = &hcsr04i2c_init; + dev->update = &hcsr04i2c_update; + dev->read = &hcsr04i2c_get_distance; + + return true; +} +#endif diff --git a/src/main/drivers/rangefinder_hcsr04_i2c.h b/src/main/drivers/rangefinder_hcsr04_i2c.h new file mode 100644 index 0000000000..7d2ed6265e --- /dev/null +++ b/src/main/drivers/rangefinder_hcsr04_i2c.h @@ -0,0 +1,22 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS 100 + +bool hcsr04i2c0Detect(rangefinderDev_t *dev); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 7620eb0f21..6861d919b8 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -167,7 +167,7 @@ static const char * const lookupTableBaroHardware[] = { "NONE", "AUTO", "BMP085" // sync with magSensor_e static const char * const lookupTableMagHardware[] = { "NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "FAKE"}; // sycn with rangefinderType_e -static const char * const lookupTableRangefinderHardware[] = { "NONE", "HCSR04", "SRF10"}; +static const char * const lookupTableRangefinderHardware[] = { "NONE", "HCSR04", "SRF10", "HCSR04I2C"}; // sync with pitotSensor_e static const char * const lookupTablePitotHardware[] = { "NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"}; diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index b7f828cc61..3a5ccbd68e 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/rangefinder_hcsr04.h" #include "drivers/rangefinder_srf10.h" +#include "drivers/rangefinder_hcsr04_i2c.h" #include "drivers/rangefinder.h" #include "fc/config.h" @@ -79,8 +80,6 @@ const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void) #elif defined(RANGEFINDER_HCSR04_TRIGGER_PIN) rangefinderHardwarePins.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN); rangefinderHardwarePins.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN); -#else -#error Rangefinder not defined for target #endif return &rangefinderHardwarePins; } @@ -115,6 +114,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_HCSR04I2C: +#ifdef USE_RANGEFINDER_HCSR04_I2C + if (hcsr04i2c0Detect(dev)) { + rangefinderHardware = RANGEFINDER_HCSR04I2C; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_NONE: rangefinderHardware = RANGEFINDER_NONE; break; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2e43f5d035..578d50642f 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -22,9 +22,10 @@ #include "drivers/rangefinder.h" typedef enum { - RANGEFINDER_NONE = 0, - RANGEFINDER_HCSR04 = 1, - RANGEFINDER_SRF10 = 2, + RANGEFINDER_NONE = 0, + RANGEFINDER_HCSR04 = 1, + RANGEFINDER_SRF10 = 2, + RANGEFINDER_HCSR04I2C = 3, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index f53aef2a75..7bcb60daab 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -112,6 +112,10 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE (I2C_DEVICE) + #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 From 4f28f6cab9df54153a08659af3cf4f8a92e2fe7a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 7 Jul 2017 09:06:00 +0200 Subject: [PATCH 2/4] Driver --- src/main/drivers/rangefinder_hcsr04_i2c.c | 102 ++++++++++++++-------- 1 file changed, 67 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/rangefinder_hcsr04_i2c.c b/src/main/drivers/rangefinder_hcsr04_i2c.c index 18f832c268..3e53cd893a 100644 --- a/src/main/drivers/rangefinder_hcsr04_i2c.c +++ b/src/main/drivers/rangefinder_hcsr04_i2c.c @@ -33,7 +33,7 @@ #include "build/debug.h" #ifndef RANGEFINDER_HCSR04_I2C_I2C_INSTANCE -#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE I2CDEV_1 +#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE I2C_DEVICE #endif #define HCSR04_I2C_MAX_RANGE_CM 400 @@ -42,55 +42,87 @@ #define HCSR04_I2C_Address 0x14 -volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; -// static bool isSensorResponding = true; +#define HCSR04_I2C_REGISTRY_STATUS 0x00 +#define HCSR04_I2C_REGISTRY_DISTANCE_HIGH 0x01 +#define HCSR04_I2C_REGISTRY_DISTANCE_LOW 0x02 -static void hcsr04i2c_init(void) -{ - // timeOfLastMeasurementMs = millis(); +volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static bool isHcsr04i2cResponding = false; + +static uint8_t hcsr04i2cReadByte(uint8_t registry) { + uint8_t buffer; + + isHcsr04i2cResponding = i2cRead(I2C_DEVICE, HCSR04_I2C_Address, registry, 1, &buffer); + return buffer; } -void hcsr04i2c_update(void) -{ - uint8_t buf; +static void hcsr04i2cInit(void) { +} - i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 0, 1, &buf); - debug[0] = buf; - i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 1, 1, &buf); - debug[1] = buf; - i2cRead(I2C_DEVICE, HCSR04_I2C_Address, 2, 1, &buf); - debug[2] = buf; +void hcsr04i2cUpdate(void) { + uint8_t response; - debug[3] = millis(); + /* + * Read status byte + */ + response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_STATUS); + if (!isHcsr04i2cResponding) { + hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + return; + } + + if (response == 0) { + /* + * Rangefinder is reporting everything in place and working + */ + response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_DISTANCE_HIGH); + if (!isHcsr04i2cResponding) { + hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + return; + } + + hcsr04i2cMeasurementCm = (int32_t)((int32_t)response << 8); + + response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_DISTANCE_HIGH); + if (!isHcsr04i2cResponding) { + hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + return; + } + + hcsr04i2cMeasurementCm += response; + + } else { + /* + * Rangefinder is reporting out-of-range situation + */ + hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE; + } } /** * Get the distance that was measured by the last pulse, in centimeters. */ -static int32_t hcsr04i2c_get_distance(void) -{ - // uint8_t buf[3]; - - // bool isSensorResponding = i2cRead(RANGEFINDER_HCSR04_I2C_I2C_INSTANCE, HCSR04_I2C_AddressI2C, 0, 3, &byte); - - // debug[0] = buf[0]; - // debug[1] = buf[1]; - // debug[2] = buf[3]; - return 7; - +int32_t hcsr04i2cGetDistance(void) { + return hcsr04i2cMeasurementCm; } bool hcsr04i2c0Detect(rangefinderDev_t *dev) { - dev->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS; - dev->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM; - dev->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES; - dev->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES; + hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_STATUS); - dev->init = &hcsr04i2c_init; - dev->update = &hcsr04i2c_update; - dev->read = &hcsr04i2c_get_distance; + if (isHcsr04i2cResponding) { + dev->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS; + dev->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM; + dev->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES; + dev->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES; - return true; + dev->init = &hcsr04i2cInit; + dev->update = &hcsr04i2cUpdate; + dev->read = &hcsr04i2cGetDistance; + + return true; + } else { + return false; + } } #endif From 23043d91813ef57e8d5c76d7503fbdd859289a6e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 10 Jul 2017 10:25:29 +0200 Subject: [PATCH 3/4] switched to use single read sequence --- src/main/drivers/rangefinder_hcsr04_i2c.c | 32 ++++++----------------- 1 file changed, 8 insertions(+), 24 deletions(-) diff --git a/src/main/drivers/rangefinder_hcsr04_i2c.c b/src/main/drivers/rangefinder_hcsr04_i2c.c index 3e53cd893a..9d3d6dd115 100644 --- a/src/main/drivers/rangefinder_hcsr04_i2c.c +++ b/src/main/drivers/rangefinder_hcsr04_i2c.c @@ -60,36 +60,20 @@ static void hcsr04i2cInit(void) { } void hcsr04i2cUpdate(void) { - uint8_t response; + uint8_t response[3]; - /* - * Read status byte - */ - response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_STATUS); + isHcsr04i2cResponding = i2cRead(I2C_DEVICE, HCSR04_I2C_Address, HCSR04_I2C_REGISTRY_STATUS, 3, response); + if (!isHcsr04i2cResponding) { hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; return; - } - - if (response == 0) { - /* - * Rangefinder is reporting everything in place and working - */ - response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_DISTANCE_HIGH); - if (!isHcsr04i2cResponding) { - hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; - return; - } + } - hcsr04i2cMeasurementCm = (int32_t)((int32_t)response << 8); + if (response[HCSR04_I2C_REGISTRY_STATUS] == 0) { - response = hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_DISTANCE_HIGH); - if (!isHcsr04i2cResponding) { - hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - hcsr04i2cMeasurementCm += response; + hcsr04i2cMeasurementCm = + (int32_t)((int32_t)response[HCSR04_I2C_REGISTRY_DISTANCE_HIGH] << 8) + + response[HCSR04_I2C_REGISTRY_DISTANCE_LOW]; } else { /* From 5c8d090fb3c808f3dac92d606ab4c54919878698 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 10 Jul 2017 10:29:11 +0200 Subject: [PATCH 4/4] enabled on OmnibusF4 target --- src/main/target/OMNIBUSF4/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index a8d16e15df..c2befade10 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -80,6 +80,10 @@ #define USE_PITOT_MS4525 #define PITOT_I2C_INSTANCE I2C_DEVICE +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE (I2C_DEVICE) + #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED