Merge branch 'zr_rover4.1' of gitee.com:xmzrzk/zr-v4 into zr_rover4.1

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

@@ -44,25 +44,27 @@ void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
     }
 }
 
-AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id)
+AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id,uint8_t battery_id)
 {
     if (ap_uavcan == nullptr) {
         return nullptr;
     }
     for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
         if (AP::battery().drivers[i] == nullptr ||
-            AP::battery().get_type(i) != AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
+            AP::battery().get_type(i) != AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo ||
+            AP::battery()._params[i]._serial_number != battery_id ) {
             continue;
         }
         AP_BattMonitor_UAVCAN* driver = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
-        if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id) {
+        if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id && driver->_params._serial_number== battery_id ) {
             return driver;
         }
     }
     // find empty uavcan driver
     for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
         if (AP::battery().drivers[i] != nullptr &&
-            AP::battery().get_type(i) == AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo) {
+            AP::battery().get_type(i) == AP_BattMonitor_Params::BattMonitor_TYPE_UAVCAN_BatteryInfo&&
+             AP::battery()._params[i]._serial_number == battery_id) {
 
             AP_BattMonitor_UAVCAN* batmon = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i];
             batmon->_ap_uavcan = ap_uavcan;
@@ -85,7 +87,6 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
     _interim_state.temperature = cb.msg->temperature;
     _interim_state.voltage = cb.msg->voltage;
     _interim_state.current_amps = cb.msg->current;
-
     uint32_t tnow = AP_HAL::micros();
     uint32_t dt = tnow - _interim_state.last_time_micros;
 
@@ -105,7 +106,8 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb)
 
 void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb)
 {
-    AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
+    AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id,ap_uavcan->get_driver_index());
+
     if (driver == nullptr) {
         return;
     }

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h

@@ -29,7 +29,7 @@ public:
     }
 
     static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
-    static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
+    static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id,uint8_t baterry_id);
     static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb);
 
 private:

libraries/AP_HAL/AP_HAL_Boards.h

@@ -162,7 +162,7 @@
 #endif
 
 #ifndef HAL_WITH_UAVCAN
-#define HAL_WITH_UAVCAN 0
+#define HAL_WITH_UAVCAN 1
 #endif
 
 #ifndef HAL_RCINPUT_WITH_AP_RADIO

libraries/AP_Proximity/AP_Proximity.cpp

@@ -24,6 +24,7 @@
 #include "AP_Proximity_SITL.h"
 #include "AP_Proximity_MorseSITL.h"
 #include "AP_Proximity_AirSimSITL.h"
+#include "AP_Proximity_UAVCAN.h"
 #include <AP_AHRS/AP_AHRS.h>
 
 extern const AP_HAL::HAL &hal;
@@ -344,6 +345,10 @@ void AP_Proximity::detect_instance(uint8_t instance)
         drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
         return;
 #endif
+    case Type::UAVCAN:
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_UAVCAN(*this, state[instance]);
+        return;
     }
 }
 

libraries/AP_Proximity/AP_Proximity.h

@@ -47,6 +47,7 @@ public:
         RPLidarA2 = 5,
         TRTOWEREVO = 6,
         SF40C = 7,
+        UAVCAN =8,
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         SITL    = 10,
         MorseSITL = 11,

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -0,0 +1,216 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_WITH_UAVCAN
+
+#include "AP_Proximity_UAVCAN.h"
+#include <GCS_MAVLink/GCS.h>
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+#include<zrzk/equipment/range_sensor/Proximity.hpp>
+
+extern const AP_HAL::HAL& hal;
+AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0};
+UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
+
+#define debug_baro_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0)
+
+//HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry;
+
+AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(
+    AP_Proximity &_frontend,
+    AP_Proximity::Proximity_State &_state):
+AP_Proximity_Backend(_frontend,_state)
+{
+    AP_Proximity_UAVCAN *backend = nullptr;
+    for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
+    {
+        if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr)
+        {
+            backend = new AP_Proximity_UAVCAN(_frontend, _state);
+            if (backend == nullptr)
+            {
+                debug_baro_uavcan(2,
+                                  _detected_modules[i].ap_uavcan->get_driver_index(),
+                                  "Failed register UAVCAN Proximity Node %d on Bus %d\n",
+                                  _detected_modules[i].node_id,
+                                  _detected_modules[i].ap_uavcan->get_driver_index());
+            }
+            else
+            {
+                _detected_modules[i].driver = backend;
+                backend->_ap_uavcan = _detected_modules[i].ap_uavcan;
+                backend->_node_id = _detected_modules[i].node_id;
+                backend->register_sensor();
+                debug_baro_uavcan(2,
+                                  _detected_modules[i].ap_uavcan->get_driver_index(),
+                                  "Registered UAVCAN Proximity Node %d on Bus %d\n",
+                                  _detected_modules[i].node_id,
+                                  _detected_modules[i].ap_uavcan->get_driver_index());
+            }
+            break;
+        }
+    }
+}
+
+void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
+{
+    if (ap_uavcan == nullptr)
+    {
+        return;
+    }
+
+    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));
+    if (proximity_listener_res < 0)
+    {
+        AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r");
+        return;
+    }
+}
+
+bool AP_Proximity_UAVCAN::detect(AP_Proximity &_frontend,
+                                 AP_Proximity::Proximity_State &_state)
+{
+    AP_Proximity_UAVCAN *backend = nullptr;
+    for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
+    {
+        if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr)
+        {
+            backend = new AP_Proximity_UAVCAN(_frontend, _state);
+            if (backend == nullptr)
+            {
+                debug_baro_uavcan(2,
+                                  _detected_modules[i].ap_uavcan->get_driver_index(),
+                                  "Failed register UAVCAN Proximity Node %d on Bus %d\n",
+                                  _detected_modules[i].node_id,
+                                  _detected_modules[i].ap_uavcan->get_driver_index());
+            }
+            else
+            {
+                _detected_modules[i].driver = backend;
+                backend->_ap_uavcan = _detected_modules[i].ap_uavcan;
+                backend->_node_id = _detected_modules[i].node_id;
+                // backend->register_sensor();
+                debug_baro_uavcan(2,
+                                  _detected_modules[i].ap_uavcan->get_driver_index(),
+                                  "Registered UAVCAN Proximity Node %d on Bus %d\n",
+                                  _detected_modules[i].node_id,
+                                  _detected_modules[i].ap_uavcan->get_driver_index());
+            }
+            break;
+        }
+    }
+    return backend;
+}
+
+AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan, uint8_t node_id, bool create_new)
+{
+    if (ap_uavcan == nullptr)
+    {
+        return nullptr;
+    }
+
+ 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;
+        }
+    }
+    
+    if (create_new) {
+        bool already_detected = false;
+        //Check if there's an empty spot for possible registeration
+        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) {
+                //Already Detected
+                already_detected = true;
+                break;
+            }
+        }
+        if (!already_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_registry);
+    update_sector_data(0, d1_cm);   // d1
+    update_sector_data(45, d8_cm);  // d8
+    update_sector_data(90, d7_cm);  // d7
+    update_sector_data(135, d6_cm); // d6
+    update_sector_data(180, d5_cm); // d5
+    update_sector_data(225, d4_cm); // d4
+    update_sector_data(270, d3_cm); // d3
+    update_sector_data(315, d2_cm); // d2
+
+    gcs().send_text(MAV_SEVERITY_INFO,"update data\r\n");
+    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(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb)
+{
+    // WITH_SEMAPHORE(_sem_registry);
+    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan, node_id, true);
+    if (driver == nullptr)
+    {
+        gcs().send_text(MAV_SEVERITY_INFO, "driver == nullptr\r\n");
+        return;
+    }
+    // WITH_SEMAPHORE(driver->_sem_proximity);
+    driver->d1_cm = cb.msg->d0;
+    driver->d2_cm = cb.msg->d45;
+    driver->d3_cm = cb.msg->d90;
+    driver->d5_cm = cb.msg->d90;
+    driver->d6_cm = cb.msg->d90;
+    driver->d7_cm = cb.msg->d90;
+    driver->d8_cm = cb.msg->d90;
+    gcs().send_text(MAV_SEVERITY_INFO, "get_proximity_data\r\n");
+}
+
+void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
+{
+    const uint8_t sector = convert_angle_to_sector(angle_deg);
+    _angle[sector] = angle_deg;
+    _distance[sector] = ((float)distance_cm) / 100;
+    _distance_valid[sector] = distance_cm != 0xffff;
+    _last_distance_received_ms = AP_HAL::millis();
+    // update boundary used for avoidance
+    update_boundary_for_sector(sector, true);
+}
+
+#endif

libraries/AP_Proximity/AP_Proximity_UAVCAN.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include "AP_Proximity_Backend.h"
+
+#if HAL_WITH_UAVCAN
+#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_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan, uint8_t node_id, bool create_new);
+    static bool detect(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+    static void handle_proximity_data(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb);
+    //uint8_t _instance;
+    AP_Proximity::Proximity_State _status;
+    AP_UAVCAN *_ap_uavcan;
+    uint8_t _node_id;
+
+    inline void register_sensor()
+    {
+      //  instance = frontend.register_sensor();
+    }
+    // HAL_Semaphore _sem_proximity;
+    //bool new_data;
+    //MAV_DISTANCE_SENSOR _sensor_type;
+    uint32_t _last_distance_received_ms;
+    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;
+    void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
+
+    static struct DetectedModules
+    {
+        AP_UAVCAN *ap_uavcan;
+        uint8_t node_id;
+        AP_Proximity_UAVCAN *driver;
+    } _detected_modules[PROXIMITY_MAX_INSTANCES];
+};
+#endif //HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -41,6 +41,7 @@
 #include <ardupilot/indication/Button.hpp>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
+#include <uavcan/equipment/device/Temperature.hpp>
 
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@@ -53,6 +54,7 @@
 #include <AP_ADSB/AP_ADSB.h>
 #include "AP_UAVCAN_Server.h"
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Proximity/AP_Proximity_UAVCAN.h>
 
 #define LED_DELAY_US 50000
 
@@ -129,6 +131,9 @@ static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb>
 UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
 static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
 
+UC_REGISTRY_BINDER(DeviceTemperatureCb, uavcan::equipment::device::Temperature);
+static uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperatureCb> *device_temperature_listener[MAX_NUMBER_OF_CAN_DRIVERS];
+
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -198,7 +203,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     _node->setNodeID(self_node_id);
 
     char ndname[20];
-    snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
+    snprintf(ndname, sizeof(ndname), "zrzk.rover:%u", driver_index);
 
     uavcan::NodeStatusProvider::NodeName name(ndname);
     _node->setName(name);
@@ -248,6 +253,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     AP_Airspeed_UAVCAN::subscribe_msgs(this);
     AP_OpticalFlow_HereFlow::subscribe_msgs(this);
     AP_RangeFinder_UAVCAN::subscribe_msgs(this);
+    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));
@@ -293,6 +299,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
     }
 
+    device_temperature_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperatureCb>(*_node);
+    if (device_temperature_listener[driver_index]) {
+        device_temperature_listener[driver_index]->start(DeviceTemperatureCb(this, &handle_device_temperature));
+    }
+    
     _led_conf.devices_count = 0;
     if (enable_filters) {
         configureCanAcceptanceFilters(*_node);
@@ -763,4 +774,12 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
 
 }
 
+void AP_UAVCAN::handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb)
+{
+    if(cb.msg->device_id == 13)
+    {
+        gcs().send_text(MAV_SEVERITY_INFO,"tep:%f",cb.msg->temperature);
+    }
+}
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -51,6 +51,7 @@ class ButtonCb;
 class TrafficReportCb;
 class ActuatorStatusCb;
 class ESCStatusCb;
+class DeviceTemperatureCb;
 
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
@@ -230,6 +231,7 @@ private:
     static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
     static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
     static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
+    static void handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb);
 };
 
 #endif /* AP_UAVCAN_H_ */

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 
+                   

libraries/AP_UAVCAN/dsdl/zrzk/protocol/26160.Message.uavcan

@@ -0,0 +1,3 @@
+uint8 id
+uint8 type
+uint8[<64] message

libraries/AP_UAVCAN/dsdl/zrzk/protocol/26161.Request.uavcan

@@ -0,0 +1,6 @@
+uint8 id
+uint8 command
+uint8 param1
+uint8 param2
+uint8 param3
+uint8 param4