Merge pull request #1865 from iNavFlight/hcsr04-i2c-interface

HC-SR04 over I2C bus

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 \

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS 100
+
+bool hcsr04i2c0Detect(rangefinderDev_t *dev);

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"};
 

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;

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 {

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

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