AP_RangeFinder: add Hexsoon CAN radar support

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 @@ public:
 #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 @@ protected:
     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