diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 47cb31eea8..6e2d28c864 100644 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index fd8239964c..eefa81eb10 100755 --- a/src/main/drivers/bus.h +++ b/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 diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 0531f2af5b..91644b895a 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/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 { diff --git a/src/main/drivers/rangefinder/rangefinder_us42.c b/src/main/drivers/rangefinder/rangefinder_us42.c new file mode 100644 index 0000000000..eabb571d92 --- /dev/null +++ b/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 . + */ + +#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" + +// 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 \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_us42.h b/src/main/drivers/rangefinder/rangefinder_us42.h new file mode 100644 index 0000000000..e6424c6c85 --- /dev/null +++ b/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 . + */ + +#pragma once + +#define RANGEFINDER_US42_TASK_PERIOD_MS 100 + +bool us42Detect(rangefinderDev_t *dev); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b7a835717f..8a8364370a 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", "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"] diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 0129cdfb3b..cec9b00faf 100644 --- a/src/main/sensors/rangefinder.c +++ b/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; } diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index a814a11423..568c01ae12 100644 --- a/src/main/sensors/rangefinder.h +++ b/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 { diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 79fc334751..75afdea322 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/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 diff --git a/src/main/target/common.h b/src/main/target/common.h index 34c1ca9110..7344fd21f3 100755 --- a/src/main/target/common.h +++ b/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 diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 437e9c7de5..bc32325ced 100755 --- a/src/main/target/common_hardware.c +++ b/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)