AP_Rangefinder: add generic Chinese Sonar sensor

Author: khancyr

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;

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

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  driver for GenericCSonar_Serial : ME007YS; A02YYUW;
+ */
+#include "AP_RangeFinder_GenericCSonar_Serial.h"
+
+#if AP_RANGEFINDER_GENERICCSONAR_SERIAL_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/crc.h>
+#include <stdio.h>
+
+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

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

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),
 

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