rangefinder gy-us42 added

make/source.mk

@@ -177,6 +177,7 @@ COMMON_SRC = \
             drivers/rangefinder/rangefinder_srf10.c \
             drivers/rangefinder/rangefinder_vl53l0x.c \
             drivers/rangefinder/rangefinder_virtual.c \
+            drivers/rangefinder/rangefinder_us42.c \
             drivers/opflow/opflow_fake.c \
             drivers/opflow/opflow_virtual.c \
             drivers/vtx_common.c \

src/main/drivers/bus.h

@@ -136,6 +136,7 @@ typedef enum {
     DEVHW_SRF10,
     DEVHW_HCSR04_I2C,   // DIY-style adapter
     DEVHW_VL53L0X,
+    DEVHW_US42,
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter

src/main/drivers/bus_i2c_hal.c

@@ -187,7 +187,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
 
     HAL_StatusTypeDef status;
 
-    if (reg_ == 0xFF && allowRawAccess) {
+    if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) {
         status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT);
     }
     else {

src/main/drivers/rangefinder/rangefinder_us42.c

@@ -0,0 +1,131 @@
+/*
+ * 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_US42)
+
+#include "build/build_config.h"
+
+#include "drivers/time.h"
+#include "drivers/bus_i2c.h"
+
+#include "drivers/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/rangefinder_us42.h"
+
+#include "build/debug.h"
+
+#define US42_MAX_RANGE_CM 500 // vcc=3.3v -> 500cm; vcc=5v -> 700cm
+#define US42_DETECTION_CONE_DECIDEGREES 250
+#define US42_DETECTION_CONE_EXTENDED_DECIDEGREES 300
+#define US42_MIN_PROBE_INTERVAL 50
+
+#define US42_I2C_ADDRESS 0x70
+#define US42_I2C_REGISTRY_BASE 0x00
+#define US42_I2C_REGISTRY_PROBE 0x51
+#define US42_I2C_REGISTRY_STATUS_OK 0x00
+#define US42_I2C_REGISTRY_DISTANCE_HIGH 0x00
+#define US42_I2C_REGISTRY_DISTANCE_LOW 0x01
+
+volatile int32_t us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
+static int16_t minimumReadingIntervalMs = US42_MIN_PROBE_INTERVAL;
+static uint32_t timeOfLastMeasurementMs;
+uint8_t nullProbeCommandValue[0];
+static bool isUs42Responding = false;
+
+static void us42Init(rangefinderDev_t *rangefinder)
+{
+    busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
+}
+
+void us42Update(rangefinderDev_t *rangefinder)
+{
+    uint8_t data[2];
+    isUs42Responding = busReadBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, data, 2);
+
+    if (isUs42Responding) {
+        us42MeasurementCm = (int32_t)data[0] << 8 | (int32_t)data[1];
+
+        if (us42MeasurementCm > US42_MAX_RANGE_CM) {
+             us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
+        }    
+
+    } else {
+        us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
+    }    
+
+    const timeMs_t timeNowMs = millis();
+    if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
+        // measurement repeat interval should be greater than minimumReadingIntervalMs
+        // to avoid interference between connective measurements.
+        timeOfLastMeasurementMs = timeNowMs;
+        busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
+    }
+}
+
+/**
+ * Get the distance that was measured by the last pulse, in centimeters.
+ */
+int32_t us42GetDistance(rangefinderDev_t *rangefinder)
+{
+    UNUSED(rangefinder);
+    return us42MeasurementCm;
+}
+
+static bool deviceDetect(busDevice_t * busDev)
+{
+    for (int retry = 0; retry < 5; retry++) {
+        uint8_t inquiryResult;
+
+        delay(150);
+
+        bool ack = busRead(busDev, US42_I2C_REGISTRY_BASE, &inquiryResult);
+        if (ack && inquiryResult == US42_I2C_REGISTRY_STATUS_OK) {
+            return true;
+        }
+    };
+
+    return false;
+}
+
+bool us42Detect(rangefinderDev_t *rangefinder)
+{
+    rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_US42, 0, OWNER_RANGEFINDER);
+    if (rangefinder->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(rangefinder->busDev)) {
+        busDeviceDeInit(rangefinder->busDev);
+        return false;
+    }
+
+    rangefinder->delayMs = RANGEFINDER_US42_TASK_PERIOD_MS;
+    rangefinder->maxRangeCm = US42_MAX_RANGE_CM;
+    rangefinder->detectionConeDeciDegrees = US42_DETECTION_CONE_DECIDEGREES;
+    rangefinder->detectionConeExtendedDeciDegrees = US42_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+    rangefinder->init = &us42Init;
+    rangefinder->update = &us42Update;
+    rangefinder->read = &us42GetDistance;
+
+    return true;
+}
+#endif

src/main/drivers/rangefinder/rangefinder_us42.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_US42_TASK_PERIOD_MS 100
+
+bool us42Detect(rangefinderDev_t *dev);

src/main/fc/settings.yaml

@@ -7,7 +7,7 @@ tables:
     values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
+    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE", "US42"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"]

src/main/sensors/rangefinder.c

@@ -41,6 +41,7 @@
 #include "drivers/rangefinder/rangefinder_hcsr04_i2c.h"
 #include "drivers/rangefinder/rangefinder_vl53l0x.h"
 #include "drivers/rangefinder/rangefinder_virtual.h"
+#include "drivers/rangefinder/rangefinder_us42.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
@@ -160,6 +161,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
             break;
 
+            case RANGEFINDER_US42:
+#ifdef USE_RANGEFINDER_US42
+            if (us42Detect(dev)) {
+                rangefinderHardware = RANGEFINDER_US42;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_US42_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
         case RANGEFINDER_NONE:
             rangefinderHardware = RANGEFINDER_NONE;
             break;

src/main/sensors/rangefinder.h

@@ -30,6 +30,7 @@ typedef enum {
     RANGEFINDER_MSP         = 5,
     RANGEFINDER_UIB         = 6,
     RANGEFINDER_BENEWAKE    = 7,
+    RANGEFINDER_US42        = 8,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {

src/main/target/MATEKF405/target.h

@@ -164,7 +164,8 @@
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
 #define USE_RANGEFINDER_HCSR04_I2C
-#define RANGEFINDER_I2C_BUS     DEFAULT_I2C_BUS
+#define USE_RANGEFINDER_US42
+#define RANGEFINDER_I2C_BUS         DEFAULT_I2C_BUS
 
 #define PITOT_I2C_BUS               DEFAULT_I2C_BUS
 

src/main/target/MATEKF405SE/target.h

@@ -71,6 +71,7 @@
 
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_HCSR04_I2C
+#define USE_RANGEFINDER_US42
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
 #define PITOT_I2C_BUS           BUS_I2C2

src/main/target/common.h

@@ -86,6 +86,7 @@
 #define USE_RANGEFINDER_BENEWAKE
 #define USE_RANGEFINDER_VL53L0X
 #define USE_RANGEFINDER_HCSR04_I2C
+#define USE_RANGEFINDER_US42
 
 // Allow default optic flow boards
 #define USE_OPFLOW

src/main/target/common_hardware.c

@@ -283,6 +283,15 @@
     #endif
 #endif
 
+#if defined(USE_RANGEFINDER_US42)
+    #if !defined(US42_I2C_BUS)
+        #define US42_I2C_BUS RANGEFINDER_I2C_BUS
+    #endif
+    #if defined(US42_I2C_BUS)
+    BUSDEV_REGISTER_I2C(busdev_us42,       DEVHW_US42,           US42_I2C_BUS,       0x70,               NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires null data to passthrough
+    #endif
+#endif
+
 
 /** AIRSPEED SENSORS **/