Merge pull request #6792 from emtrax-ltd/rangefinder-gy-us42-master

Added rangefinder "GY-US42(v2) Ultrasonic Range Sensor"

src/main/CMakeLists.txt

@@ -238,7 +238,9 @@ main_sources(COMMON_SRC
     drivers/rangefinder/rangefinder_vl53l1x.h
     drivers/rangefinder/rangefinder_virtual.c
     drivers/rangefinder/rangefinder_virtual.h
-
+    drivers/rangefinder/rangefinder_us42.c
+    drivers/rangefinder/rangefinder_us42.h
+    
     drivers/resource.c
     drivers/resource.h
     drivers/rcc.c

src/main/drivers/bus.h

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

src/main/drivers/bus_i2c_hal.c

@@ -174,7 +174,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(&state->handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT);
     }
     else {

src/main/drivers/rangefinder/rangefinder_us42.c

@@ -0,0 +1,132 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  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"
+
+// GY-US42(v2) Ultrasonic Range Sensor
+#define US42_MAX_RANGE_CM 400 // vcc=3.3v -> 500cm; vcc=5v -> 700cm; Ardupilot recommends a maximum of 400cm
+#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 Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight.  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", "BMI088", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"]
+    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"]
     enum: rangefinderType_e
   - name: secondary_imu_hardware
     values: ["NONE", "BNO055"]

src/main/sensors/rangefinder.c

@@ -42,6 +42,7 @@
 #include "drivers/rangefinder/rangefinder_vl53l0x.h"
 #include "drivers/rangefinder/rangefinder_vl53l1x.h"
 #include "drivers/rangefinder/rangefinder_virtual.h"
+#include "drivers/rangefinder/rangefinder_us42.h"
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
@@ -160,7 +161,16 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
             break;
 
-        case RANGEFINDER_NONE:
+            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

@@ -31,6 +31,7 @@ typedef enum {
     RANGEFINDER_UNUSED      = 6,    // Was UIB
     RANGEFINDER_BENEWAKE    = 7,
     RANGEFINDER_VL53L1X     = 8,
+    RANGEFINDER_US42        = 9,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {

src/main/target/MATEKF405SE/target.h

@@ -72,6 +72,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
 #define TEMPERATURE_I2C_BUS     BUS_I2C2

src/main/target/common.h

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

src/main/target/common_hardware.c

@@ -320,6 +320,15 @@
     #endif
 #endif
 
+#if defined(USE_RANGEFINDER_US42)
+    #if !defined(US42_I2C_BUS) && defined(RANGEFINDER_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 **/
 
 #if defined(PITOT_I2C_BUS) && !defined(MS4525_I2C_BUS)