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..9d3d6dd115
--- /dev/null
+++ b/src/main/drivers/rangefinder_hcsr04_i2c.c
@@ -0,0 +1,112 @@
+/*
+ * 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 I2C_DEVICE
+#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
+
+#define HCSR04_I2C_REGISTRY_STATUS 0x00
+#define HCSR04_I2C_REGISTRY_DISTANCE_HIGH 0x01
+#define HCSR04_I2C_REGISTRY_DISTANCE_LOW 0x02
+
+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;
+}
+
+static void hcsr04i2cInit(void) {
+}
+
+void hcsr04i2cUpdate(void) {
+ uint8_t response[3];
+
+ isHcsr04i2cResponding = i2cRead(I2C_DEVICE, HCSR04_I2C_Address, HCSR04_I2C_REGISTRY_STATUS, 3, response);
+
+ if (!isHcsr04i2cResponding) {
+ hcsr04i2cMeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
+ return;
+ }
+
+ if (response[HCSR04_I2C_REGISTRY_STATUS] == 0) {
+
+ hcsr04i2cMeasurementCm =
+ (int32_t)((int32_t)response[HCSR04_I2C_REGISTRY_DISTANCE_HIGH] << 8) +
+ response[HCSR04_I2C_REGISTRY_DISTANCE_LOW];
+
+ } 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.
+ */
+int32_t hcsr04i2cGetDistance(void) {
+ return hcsr04i2cMeasurementCm;
+}
+
+bool hcsr04i2c0Detect(rangefinderDev_t *dev)
+{
+ hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_STATUS);
+
+ 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;
+
+ dev->init = &hcsr04i2cInit;
+ dev->update = &hcsr04i2cUpdate;
+ dev->read = &hcsr04i2cGetDistance;
+
+ return true;
+ } else {
+ return false;
+ }
+}
+#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 cc25eb070a..886fc03c73 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -169,7 +169,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
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 37a7aec568..aca79cb2a1 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -86,6 +86,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