From f77aeee95c60aaa7c7fb3c6a7c18de8a5011dfc2 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 25 Nov 2024 18:15:06 +0100 Subject: [PATCH] AP_Rangefinder: add generic Chinese Sonar sensor --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 8 +++ libraries/AP_RangeFinder/AP_RangeFinder.h | 3 + .../AP_RangeFinder_GenericCSonar_Serial.cpp | 65 +++++++++++++++++++ .../AP_RangeFinder_GenericCSonar_Serial.h | 38 +++++++++++ .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_config.h | 4 ++ 6 files changed, 119 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 2134162eab2486..251e69df6d109e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder.h" +#include "AP_RangeFinder_GenericCSonar_Serial.h" + #if AP_RANGEFINDER_ENABLED #include "AP_RangeFinder_analog.h" @@ -45,6 +47,7 @@ #include "AP_RangeFinder_Benewake_TFMini.h" #include "AP_RangeFinder_Benewake_TFMiniPlus.h" #include "AP_RangeFinder_PWM.h" +#include "AP_RangeFinder_GenericCSonar_Serial.h" #include "AP_RangeFinder_GYUS42v2.h" #include "AP_RangeFinder_HC_SR04.h" #include "AP_RangeFinder_Bebop.h" @@ -513,6 +516,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; #endif +#if AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED + case Type::GENERICCSONAR_SERIAL: + serial_create_fn = AP_RangeFinder_GenericCSonar_Serial::create; + break; +#endif #if AP_RANGEFINDER_GYUS42V2_ENABLED case Type::GYUS42v2: serial_create_fn = AP_RangeFinder_GYUS42v2::create; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 4d66101b8b6ab9..923efb8a0a7457 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -182,6 +182,9 @@ class RangeFinder #if AP_RANGEFINDER_RDS02UF_ENABLED RDS02UF = 43, #endif +#if AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED + GENERICCSONAR_SERIAL = 44, +#endif #if AP_RANGEFINDER_SIM_ENABLED SIM = 100, #endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.cpp new file mode 100644 index 00000000000000..e103dcaa36acdf --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.cpp @@ -0,0 +1,65 @@ +/* + This program 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. + + This program 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 this program. If not, see . + */ +/* + driver for GenericCSonar_Serial : ME007YS; A02YYUW; + */ +#include "AP_RangeFinder_GenericCSonar_Serial.h" + +#if AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +static constexpr float GENERICCSONAR_SERIAL_MAX_RANGE_M = 4.5; +static constexpr uint8_t GENERICCSONAR_HEADER = 0xFF; + +// read - return last value measured by sensor +bool AP_RangeFinder_GenericCSonar_Serial::get_reading(float &reading_m) +{ + if (uart == nullptr) { + return false; + } + + // format is: [ 0xFF | DATA_H | DATA_L | SUM ] + + // read any available lines from the lidar + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { + break; + } + if (buf_len == 0 && b != GENERICCSONAR_HEADER) { + // discard + continue; + } + buf[buf_len++] = b; + if (buf_len == sizeof(buf)) { + buf_len = 0; + const uint16_t crc = (buf[0] + buf[1] + buf[2]) & 0x00FF; + if (crc != buf[3]) { + // bad CRC, discard + continue; + } + reading_m = ((buf[1] << 8) + buf[2]) * 0.001; + return reading_m <= GENERICCSONAR_SERIAL_MAX_RANGE_M; + } + } + return false; +} + +#endif // AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.h new file mode 100644 index 00000000000000..e0ae2c849d7552 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GenericCSonar_Serial.h @@ -0,0 +1,38 @@ +#pragma once + +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED + +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + +class AP_RangeFinder_GenericCSonar_Serial : public AP_RangeFinder_Backend_Serial +{ + +public: + + static AP_RangeFinder_Backend_Serial *create( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params) { + return NEW_NOTHROW AP_RangeFinder_GenericCSonar_Serial(_state, _params); + } + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_ULTRASOUND; + } + +private: + + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; + + // get a reading + bool get_reading(float &reading_m) override; + + uint8_t buf[4]; + uint8_t buf_len = 0; +}; + +#endif // AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 72630f95f3949c..a9cc157ff73422 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -15,7 +15,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @DisplayName: Rangefinder type // @Description: Type of connected rangefinder // @SortValues: AlphabeticalZeroAtTop - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,44:GenericCSonar_Serial,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 1f5407adaf5e73..72b2350da9fe6a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -67,6 +67,10 @@ #define AP_RANGEFINDER_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) #endif +#ifndef AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED +#define AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_RANGEFINDER_GYUS42V2_ENABLED #define AP_RANGEFINDER_GYUS42V2_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif