AP_RangeFinder: Add NanoRadar NRA24 CAN driver

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -54,6 +54,7 @@
 #include "AP_RangeFinder_Lua.h"
 #include "AP_RangeFinder_NoopLoop.h"
 #include "AP_RangeFinder_TOFSenseP_CAN.h"
+#include "AP_RangeFinder_NRA24_CAN.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -548,6 +549,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::TOFSenseP_CAN:
 #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
         _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);
+#endif
+        break;
+    case Type::NRA24_CAN:
+#if AP_RANGEFINDER_NRA24_CAN_ENABLED
+        _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);
 #endif
         break;
 

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -95,6 +95,7 @@ public:
         Lua_Scripting = 36,
         NoopLoop_P = 37,
         TOFSenseP_CAN = 38,
+        NRA24_CAN = 39,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp

@@ -0,0 +1,86 @@
+#include "AP_RangeFinder_NRA24_CAN.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#if AP_RANGEFINDER_NRA24_CAN_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+
+RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24;
+
+// constructor
+AP_RangeFinder_NRA24_CAN::AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+    AP_RangeFinder_Backend_CAN(_state, _params)
+{
+    if (multican_NRA24 == nullptr) {
+        multican_NRA24 = new RangeFinder_MultiCAN(AP_CAN::Protocol::NanoRadar_NRA24, "NRA24 MultiCAN");
+        if (multican_NRA24 == nullptr) {
+            AP_BoardConfig::allocation_error("Rangefinder_MultiCAN");
+        }
+    }
+
+    {
+        // add to linked list of drivers
+        WITH_SEMAPHORE(multican_NRA24->sem);
+        auto *prev = multican_NRA24->drivers;
+        next = prev;
+        multican_NRA24->drivers = this;
+    }
+}
+
+// update the state of the sensor
+void AP_RangeFinder_NRA24_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()) {
+            // don't have distance data but sensor is connected. This is a known issue when sensor is in static condition (example Copter waiting to take off). Set status to out of range to avoid pre arm error
+            set_status(RangeFinder::Status::OutOfRangeLow);
+        } else {
+            set_status(RangeFinder::Status::NotConnected);
+        }
+    }
+}
+
+// handler for incoming frames
+bool AP_RangeFinder_NRA24_CAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(_sem);
+    const uint32_t id = frame.id;
+
+    if (!is_correct_id(get_radar_id(id))) {
+        return false;
+    }
+
+    switch (id & 0xFU) {
+        case 0xAU:
+            // heart beat in the form of Radar Status. The contents of this message aren't really useful so we won't parse them for now
+            last_heartbeat_ms = AP_HAL::millis();
+            break;
+
+        case 0xCU:
+        {
+            // Target Information
+            const float dist_m = (frame.data[2] * 0x100U + frame.data[3]) * 0.01;
+            const uint8_t snr = frame.data[7] - 128;
+
+            if ((snr_min != 0 && snr < uint16_t(snr_min.get()))) {
+                // too low signal strength
+                return false;
+            }
+            set_distance_m(dist_m);
+        }
+            break;
+
+        default:
+            // not parsing these messages
+            break;
+    }
+
+    return true;
+}
+
+#endif  // AP_RANGEFINDER_NRA24_CAN_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "AP_RangeFinder_Backend_CAN.h"
+
+#ifndef AP_RANGEFINDER_NRA24_CAN_ENABLED
+#define AP_RANGEFINDER_NRA24_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
+#endif
+
+#if AP_RANGEFINDER_NRA24_CAN_ENABLED
+
+class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN {
+public:
+    AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+
+    void update(void) override;
+
+    // handler for incoming frames
+    bool handle_frame(AP_HAL::CANFrame &frame) override;
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U)/0x10U); }
+
+    static RangeFinder_MultiCAN *multican_NRA24;
+
+    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

@@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @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, 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, 100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),