diff --git a/make/source.mk b/make/source.mk
index cb16ae755ec..d641bea1bad 100644
--- a/make/source.mk
+++ b/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 \
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 7f7d06d092f..40e05c6ce60 100755
--- a/src/main/drivers/bus.h
+++ b/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
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
index e2dc6436c6f..da0c35cd79e 100644
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/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 {
diff --git a/src/main/drivers/rangefinder/rangefinder_us42.c b/src/main/drivers/rangefinder/rangefinder_us42.c
new file mode 100644
index 00000000000..316d59f3fcf
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+
+#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
diff --git a/src/main/drivers/rangefinder/rangefinder_us42.h b/src/main/drivers/rangefinder/rangefinder_us42.h
new file mode 100644
index 00000000000..2b4a58319c5
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#define RANGEFINDER_US42_TASK_PERIOD_MS 100
+
+bool us42Detect(rangefinderDev_t *dev);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 9f5880120b4..390b5482fa3 100644
--- a/src/main/fc/settings.yaml
+++ b/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"]
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index aec1e065aa7..343abf21d4b 100644
--- a/src/main/sensors/rangefinder.c
+++ b/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;
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index c80d18ac1eb..8c7ebd99ad0 100644
--- a/src/main/sensors/rangefinder.h
+++ b/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 {
diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h
index 077155b1ff3..2f978938b76 100644
--- a/src/main/target/MATEKF405/target.h
+++ b/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
diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h
index 513b14a8b25..75c4794ad1c 100644
--- a/src/main/target/MATEKF405SE/target.h
+++ b/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
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 52eb6eb819a..ee53ce39582 100755
--- a/src/main/target/common.h
+++ b/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
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 1e35d2baef5..5d62b5ac433 100755
--- a/src/main/target/common_hardware.c
+++ b/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 **/