增加proximity uavcan

AP_Proximity,增加uavcan类型=9
AP_Proximity_UAVCAN,uavcan驱动
dsdl,增加range_sensor

ArduCopter/APM_Config.h

@@ -51,11 +51,11 @@
 // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
 //#define USERHOOK_VARIABLES "UserVariables.h"
 // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
-//#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
+#define USERHOOK_INIT userhook_init();                      // for code to be run once at startup
-//#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
+#define USERHOOK_FASTLOOP userhook_FastLoop();            // for code to be run at 100hz
-//#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
+#define USERHOOK_50HZLOOP userhook_50Hz();                  // for code to be run at 50hz
-//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
+#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();        // for code to be run at 10hz
-//#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
+#define USERHOOK_SLOWLOOP userhook_SlowLoop();            // for code to be run at 3.3hz
-//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
+#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();  // for code to be run at 1hz
-//#define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
+// #define USERHOOK_AUXSWITCH ENABLED                        // for code to handle user aux switches
 //#define USER_PARAMS_ENABLED ENABLED                       // to enable user parameters

libraries/AP_Proximity/AP_Proximity.cpp

@@ -26,6 +26,7 @@
 #include "AP_Proximity_SITL.h"
 #include "AP_Proximity_AirSimSITL.h"
 #include "AP_Proximity_Cygbot_D1.h"
+#include "AP_Proximity_UAVCAN.h"
 
 #include <AP_Logger/AP_Logger.h>
 
@@ -345,6 +346,13 @@ void AP_Proximity::detect_instance(uint8_t instance)
             return;
         }
         break;
+    case Type::UAVCAN:
+#if HAL_CANMANAGER_ENABLED
+        state[instance].instance = instance;
+        drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
+        return;
+#endif
+        break;
 
     case Type::CYGBOT_D1:
 #if AP_PROXIMITY_CYGBOT_ENABLED

libraries/AP_Proximity/AP_Proximity.h

@@ -51,6 +51,7 @@ public:
         TRTOWEREVO = 6,
         SF40C = 7,
         SF45B = 8,
+        UAVCAN = 9,
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         SITL    = 10,
         AirSimSITL = 12,

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -0,0 +1,250 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_CANMANAGER_ENABLED
+
+#include <GCS_MAVLink/GCS.h>
+#include "AP_Proximity_UAVCAN.h"
+#include <AP_CANManager/AP_CANManager.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+#include <zrzk/equipment/range_sensor/Proximity.hpp>
+#define PRX_DEBUG 0
+#if PRX_DEBUG
+#include <GCS_MAVLink/GCS.h>
+    # define Prx_Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args)
+#else
+    # define Prx_Debug(fmt, args ...)
+#endif
+
+extern const AP_HAL::HAL &hal;
+
+#define debug_proximity_uavcan(level_debug, can_driver, fmt, args...)      \
+    do                                                                     \
+    {                                                                      \
+        if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \
+        {                                                                  \
+            hal.console->printf(fmt, ##args);                              \
+        }                                                                  \
+    } while (0)
+
+HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry;
+UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
+
+uint8_t AP_Proximity_UAVCAN::_node_id = 0;
+AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr;
+AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr;
+
+AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0};
+
+AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend,
+                                         AP_Proximity::Proximity_State &_state) : 
+                                         AP_Proximity_Backend(_frontend, _state)
+{
+    _driver = this;
+}
+
+void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
+{
+    if (ap_uavcan == nullptr)
+    {
+        Prx_Debug("ap_uavcan:nullptr" );
+        return;
+    }
+
+        Prx_Debug("ap_uavcan:subscribe_msgs");
+    auto *node = ap_uavcan->get_node();
+
+    uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb> *proximity_listener;
+    proximity_listener = new uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb>(*node);
+    // Register method to handle incoming RangeFinder measurement
+    const int proximity_listener_res = proximity_listener->start(ProximityCb(ap_uavcan, &handle_proximity_data_trampoline));
+    if (proximity_listener_res < 0)
+    {
+        AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r");
+        return;
+    }
+}
+
+AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
+                                                 AP_Proximity::Proximity_State &_state)
+{
+    WITH_SEMAPHORE(_sem_registry);
+
+    AP_Proximity_UAVCAN *backend = nullptr;
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------");
+    backend = new AP_Proximity_UAVCAN(_frontend, _state);
+    if (backend != nullptr)
+    {
+        Prx_Debug("backend:%d",(int)backend);
+    }else{
+        
+        Prx_Debug("faild,backend");
+    }
+
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n");
+    return backend;
+}
+
+AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id)
+{
+    if (ap_uavcan == nullptr)
+    {
+        return nullptr;
+    }
+    Prx_Debug("get_uavcan:%d",node_id);
+    for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
+    {
+        if (_detected_modules[i].driver != nullptr &&
+            _detected_modules[i].ap_uavcan == ap_uavcan&&
+            _detected_modules[i].node_id == node_id)    
+        {
+            return _detected_modules[i].driver;
+        }
+    }
+
+    bool detected = false;
+    for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
+    {
+        if (_detected_modules[i].ap_uavcan == ap_uavcan &&
+            _detected_modules[i].node_id == node_id)
+        {
+            // detected
+            detected = true;
+            break;
+        }
+    }
+
+    if (!detected)
+    {
+        for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
+        {
+            if (_detected_modules[i].ap_uavcan == nullptr)
+            {
+                _detected_modules[i].ap_uavcan = ap_uavcan;
+                _detected_modules[i].node_id = node_id;
+                break;
+            }
+        }
+    }
+
+    return nullptr;
+}
+
+float AP_Proximity_UAVCAN::distance_max() const
+{
+    return 40.0f;
+}
+
+float AP_Proximity_UAVCAN::distance_min() const
+{
+    return 0.20f;
+}
+
+void AP_Proximity_UAVCAN::update(void)
+{
+    WITH_SEMAPHORE(_sem_proximity);
+
+    // update_sector_data(0, _interim_data.d1_cm);   // d1
+    // update_sector_data(45, _interim_data.d2_cm);  // d8
+    // update_sector_data(90, _interim_data.d3_cm);  // d7
+    // update_sector_data(135, _interim_data.d4_cm); // d6
+    // update_sector_data(180, _interim_data.d5_cm); // d5
+    // update_sector_data(225, _interim_data.d6_cm); // d4
+    // update_sector_data(270, _interim_data.d7_cm); // d3
+    // update_sector_data(315, _interim_data.d8_cm); // d2
+
+    update_sector_data(0, _interim_data.d1_cm);   // d1
+    update_sector_data(45, _interim_data.d2_cm);  // d8
+    update_sector_data(315, _interim_data.d3_cm);  // d7
+
+    if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
+    {
+        set_status(AP_Proximity::Status::NoData);
+        // gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n");
+    }
+    else
+    {
+        set_status(AP_Proximity::Status::Good);
+    }
+}
+
+void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb)
+{
+    
+    if (_driver == nullptr) {
+        Prx_Debug("----nullptr : ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
+
+        return;
+    }
+    
+    if (_ap_uavcan == nullptr) {
+        _ap_uavcan = ap_uavcan;
+        _node_id = node_id;
+        Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
+    }
+    Prx_Debug("handle_proximity: ap:%d,id:%d ,%d,addr:%d",(int)_ap_uavcan,_node_id,cb.msg->d0,cb.msg->sensor_id);
+    if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
+        WITH_SEMAPHORE(_sem_registry);
+            _driver->_interim_data.d1_cm = cb.msg->d0;
+            _driver->_interim_data.d2_cm = cb.msg->d45;
+            _driver->_interim_data.d3_cm = cb.msg->d90;
+            _driver->_interim_data.d4_cm = cb.msg->d135;
+            _driver->_interim_data.d5_cm = cb.msg->d180;
+            _driver->_interim_data.d6_cm = cb.msg->d225;
+            _driver->_interim_data.d7_cm = cb.msg->d270;
+            _driver->_interim_data.d8_cm = cb.msg->d315;
+            _driver->_last_distance_received_ms = AP_HAL::millis();
+    }
+    /*
+    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id);
+    if (driver == nullptr)
+    {
+        return;
+    }
+    driver->handle_proximity_data(cb);
+    */
+}
+
+void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
+{
+
+    WITH_SEMAPHORE(_sem_proximity);
+
+    _interim_data.d1_cm = cb.msg->d0;
+    _interim_data.d2_cm = cb.msg->d45;
+    _interim_data.d3_cm = cb.msg->d90;
+    _interim_data.d4_cm = cb.msg->d135;
+    _interim_data.d5_cm = cb.msg->d180;
+    _interim_data.d6_cm = cb.msg->d225;
+    _interim_data.d7_cm = cb.msg->d270;
+    _interim_data.d8_cm = cb.msg->d315;
+    _last_distance_received_ms = AP_HAL::millis();
+    // _last_sample_time_ms = AP_HAL::millis();
+}
+
+void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
+{
+    // Get location on 3-D boundary based on angle to the object
+    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+    if ((distance_cm != 0xffff) && !ignore_reading(angle_deg, distance_cm * 0.01f, false)) {
+        boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 100);
+        // update OA database
+        database_push(angle_deg, ((float) distance_cm) / 100);
+    } else {
+        boundary.reset_face(face);
+    }
+    _last_distance_received_ms = AP_HAL::millis();
+
+
+    /*
+    uint8_t sector;
+    if(convert_angle_to_sector(angle_deg,sector)){
+        _angle[sector] = angle_deg;
+        _distance[sector] = ((float)distance_cm) / 100;
+        _distance_valid[sector] = (_distance[sector] > distance_min() && _distance[sector] < distance_max());
+        update_boundary_for_sector(sector, true);
+    }
+    */
+}
+
+#endif

libraries/AP_Proximity/AP_Proximity_UAVCAN.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "AP_Proximity_Backend.h"
+
+#if HAL_CANMANAGER_ENABLED
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+class ProximityCb;
+
+#define PROXIMITY_UAVCAN_TIMEOUT_MS 300
+class AP_Proximity_UAVCAN : public AP_Proximity_Backend
+{
+public:
+  AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+  void update(void) override;
+  // get maximum and minimum distances (in meters) of sensor
+  float distance_max() const override;
+
+  float distance_min() const override;
+
+  static void subscribe_msgs(AP_UAVCAN *ap_uavcan);
+
+  static AP_Proximity_Backend *probe(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+  static void handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb);
+
+  //uint8_t _instance;
+  //AP_Proximity::Proximity_State _status;
+
+private:
+  static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id);
+  
+  static AP_Proximity_UAVCAN* _driver;
+  // inline void register_sensor()
+  // {
+  //   //  instance = frontend.register_sensor();
+  // }
+  
+  uint32_t _last_distance_received_ms;
+  void handle_proximity_data(const ProximityCb &cb);
+  struct Proximity_Data
+  {
+    uint16_t d1_cm;
+    uint16_t d2_cm;
+    uint16_t d3_cm;
+    uint16_t d4_cm;
+    uint16_t d5_cm;
+    uint16_t d6_cm;
+    uint16_t d7_cm;
+    uint16_t d8_cm;
+  } _interim_data;
+  void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
+
+  uint16_t handle_data_valid(uint16_t data);
+
+  static struct DetectedModules
+  {
+    AP_UAVCAN *ap_uavcan;
+    uint8_t node_id;
+    AP_Proximity_UAVCAN *driver;
+  } _detected_modules[PROXIMITY_MAX_INSTANCES];
+
+  static HAL_Semaphore _sem_registry;
+  HAL_Semaphore _sem_proximity;
+  
+    static AP_UAVCAN* _ap_uavcan;
+    static uint8_t _node_id;
+
+
+};
+#endif //HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -62,6 +62,7 @@
 #include "AP_UAVCAN_pool.h"
 
 #include <zrzk/equipment/indication/ZrRGB.hpp>
+#include <AP_Proximity/AP_Proximity_UAVCAN.h>
 
 #define LED_DELAY_US 50000
 
@@ -358,7 +359,8 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 #if HAL_EFI_ENABLED
     AP_EFI_DroneCAN::subscribe_msgs(this);
 #endif
-
+    AP_Proximity_UAVCAN::subscribe_msgs(this);
+    
     act_out_array[driver_index] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*_node);
     act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
     act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan

@@ -0,0 +1,23 @@
+uint8 sensor_id
+
+uint5 SENSOR_TYPE_UNDEFINED = 0
+uint5 SENSOR_TYPE_SONAR     = 1
+uint5 SENSOR_TYPE_LIDAR     = 2
+uint5 SENSOR_TYPE_RADAR     = 3
+uint5 sensor_type
+
+uint3 READING_TYPE_UNDEFINED   = 0   # Range is unknown
+uint3 READING_TYPE_VALID_RANGE = 1   # Range field contains valid distance
+uint3 READING_TYPE_TOO_CLOSE   = 2   # Range field contains min range for the sensor
+uint3 READING_TYPE_TOO_FAR     = 3   # Range field contains max range for the sensor
+uint3 reading_type
+
+uint16 d0                 # Meters distance_cm
+uint16 d45
+uint16 d90
+uint16 d135
+uint16 d180
+uint16 d225
+uint16 d270
+uint16 d315 
+