AP_RangeFinder: Hexsoon 77G 40M/80M MWW support

Tools/scripts/build_options.py

@@ -279,6 +279,7 @@ def config_option(self):
     Feature('Rangefinder', 'RANGEFINDER_VL53L1X', 'AP_RANGEFINDER_VL53L1X_ENABLED', "Enable Rangefinder - VL53L1X", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_WASP', 'AP_RANGEFINDER_WASP_ENABLED', "Enable Rangefinder - Wasp", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_RDS02UF', 'AP_RANGEFINDER_RDS02UF_ENABLED', "Enable Rangefinder - RDS02UF", 0, "RANGEFINDER"),   # NOQA: E501
+    Feature('Rangefinder', 'RANGEFINDER_HEXSOON_CAN', 'AP_RANGEFINDER_HEXSOON_CAN_ENABLED', "Enable Rangefinder - Hexsoon CAN", 0, "RANGEFINDER"),   # NOQA: E501
 
     Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None),
     Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"),

libraries/AP_Arming/AP_Arming.cpp

@@ -1240,6 +1240,7 @@ bool AP_Arming::can_checks(bool report)
                 case AP_CAN::Protocol::TOFSenseP:
                 case AP_CAN::Protocol::NanoRadar:
                 case AP_CAN::Protocol::Benewake:
+                case AP_CAN::Protocol::Hexsoon:
                 {
                     for (uint8_t j = i; j; j--) {
                         if (AP::can().get_driver_type(i) == AP::can().get_driver_type(j-1)) {

libraries/AP_CANManager/AP_CAN.h

@@ -29,5 +29,6 @@ class AP_CAN {
         Scripting2 = 12,
         TOFSenseP = 13,
         NanoRadar = 14,
+        Hexsoon = 15,
     };
 };

libraries/AP_CANManager/AP_CANManager_CANDriver_Params.cpp

@@ -27,7 +27,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL
     // @DisplayName: Enable use of specific protocol over virtual driver
     // @Description: Enabling this option starts selected protocol that will use this virtual driver
-    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
+    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar,15:Hexsoon
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, float(AP_CAN::Protocol::DroneCAN)),
@@ -51,7 +51,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL2
     // @DisplayName: Secondary protocol with 11 bit CAN addressing
     // @Description: Secondary protocol with 11 bit CAN addressing
-    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
+    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar,15:Hexsoon
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)),

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -64,6 +64,7 @@
 #include "AP_RangeFinder_JRE_Serial.h"
 #include "AP_RangeFinder_Ainstein_LR_D1.h"
 #include "AP_RangeFinder_RDS02UF.h"
+#include "AP_RangeFinder_Hexsoon_CAN.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -598,6 +599,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::RDS02UF:
         serial_create_fn = AP_RangeFinder_RDS02UF::create;
         break;
+#endif
+#if AP_RANGEFINDER_HEXSOON_CAN_ENABLED
+    case Type::Hexsoon_CAN:
+        _add_backend(NEW_NOTHROW AP_RangeFinder_Hexsoon_CAN(state[instance], params[instance]), instance);
+        break;
 #endif
     case Type::NONE:
         break;

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -182,6 +182,9 @@ class RangeFinder
 #if AP_RANGEFINDER_RDS02UF_ENABLED
         RDS02UF = 43,
 #endif
+#if AP_RANGEFINDER_HEXSOON_CAN_ENABLED
+        Hexsoon_CAN = 44,
+#endif
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h

@@ -31,6 +31,7 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend
     bool get_reading(float &reading_m);
 
     // it is essential that anyone relying on the base-class update to implement this
+    // returns true if packet consumed
     virtual bool handle_frame(AP_HAL::CANFrame &frame) = 0;
 
     // maximum time between readings before we change state to NoData:

libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.cpp

@@ -0,0 +1,82 @@
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_HEXSOON_CAN_ENABLED
+
+#include "AP_RangeFinder_Hexsoon_CAN.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+
+// update the state of the sensor
+void AP_RangeFinder_Hexsoon_CAN::update(void)
+{
+    WITH_SEMAPHORE(_sem);
+    if (get_reading(state.distance_m)) {
+        // update range_valid state based on distance measured
+        state.last_reading_ms = AP_HAL::millis();
+        update_status();
+    } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
+        if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) {
+            // no heartbeat, must be disconnected
+            set_status(RangeFinder::Status::NotConnected);
+        } else {
+            // Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar.
+            // This case has special pre-arm check handling
+            set_status(RangeFinder::Status::NoData);
+        }
+    }
+}
+
+// handler for incoming frames, returns true if packet consumed
+bool AP_RangeFinder_Hexsoon_CAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(_sem);
+
+    // exit immediately if the length is not 8 bytes
+    if (frame.dlc != 8) {
+        return false;
+    }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    switch (frame.id) {
+        case (uint16_t)MessageId::PARAMETER_CONFIGURATION:
+        case (uint16_t)MessageId::RADAR_STATUS_INFO:
+        case (uint16_t)MessageId::OBJECT_LIST_STATUS:
+        case (uint16_t)MessageId::VERSION_INFO:
+            // ignore but use for health monitoring
+            last_heartbeat_ms = now_ms;
+            break;
+
+        case (uint16_t)MessageId::OBJECT_GENERAL_INFO:
+        {
+            // distance and relative velocity information for a single tracked object
+            // only process middle sector
+            const uint8_t sector = (frame.data[6] >> 3) & 0x03;
+            if (sector != 1) {
+                return true;
+            }
+
+            // check signal strength
+            const float receiver_strength = (frame.data[7] * 0.5) - 64;
+            if ((snr_min != 0 && receiver_strength < uint16_t(snr_min.get()))) {
+                // too low signal strength
+                return true;
+            }
+
+            // calculate distance
+            const float dist_vert = ((int16_t(frame.data[1]) << 5) + (int16_t(frame.data[2] >> 3))) * 0.2 - 500;
+            const float dist_horiz = ((int16_t(frame.data[2] & 0x07) << 8) + frame.data[3]) * 0.2 - 204.6;
+            const float dist_m = sqrtF(sq(dist_vert) + sq(dist_horiz));
+            accumulate_distance_m(dist_m);
+            break;
+        }
+
+        default:
+            // not parsing these messages
+            return false;
+    }
+
+    return true;
+}
+
+#endif  // AP_RANGEFINDER_HEXSOON_CAN_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.h

@@ -0,0 +1,38 @@
+#pragma once
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_HEXSOON_CAN_ENABLED
+#include "AP_RangeFinder_Backend_CAN.h"
+#include <AP_HAL/utility/sparse-endian.h>
+
+class AP_RangeFinder_Hexsoon_CAN : public AP_RangeFinder_Backend_CAN {
+public:
+    AP_RangeFinder_Hexsoon_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+        AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::Hexsoon, "hexsoon")
+    {
+    }
+
+    void update(void) override;
+
+    // handler for incoming frames, returns true if packet consumed
+    bool handle_frame(AP_HAL::CANFrame &frame) override;
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    // message ids
+    enum class MessageId : uint16_t {
+        PARAMETER_CONFIGURATION = 0x200,
+        RADAR_STATUS_INFO = 0x201,
+        OBJECT_LIST_STATUS = 0x60A,
+        OBJECT_GENERAL_INFO = 0x60B,
+        VERSION_INFO = 0x700
+    };
+
+    // local variables
+    bool banner_sent;           // true once lidar version has been sent to the user
+    uint32_t last_heartbeat_ms; // last status message received from the sensor
+};
+
+#endif  // AP_RANGEFINDER_USD1_CAN_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:Hexsoon_CAN,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

@@ -190,3 +190,7 @@
 #ifndef AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
 #define AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
+
+#ifndef AP_RANGEFINDER_HEXSOON_CAN_ENABLED
+#define AP_RANGEFINDER_HEXSOON_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
+#endif