From 01035454de27aa9a683e3c18ed50af4561d6ebc3 Mon Sep 17 00:00:00 2001 From: binsir Date: Thu, 15 Jul 2021 20:54:19 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20uavcan=20proximity?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_HAL/AP_HAL_Boards.h | 2 +- libraries/AP_Proximity/AP_Proximity.cpp | 5 + libraries/AP_Proximity/AP_Proximity.h | 1 + .../AP_Proximity/AP_Proximity_UAVCAN.cpp | 216 ++++++++++++++++++ libraries/AP_Proximity/AP_Proximity_UAVCAN.h | 54 +++++ libraries/AP_UAVCAN/AP_UAVCAN.cpp | 4 +- .../range_sensor/26110.Proximity.uavcan | 23 ++ .../dsdl/zrzk/protocol/26160.Message.uavcan | 3 + .../dsdl/zrzk/protocol/26161.Request.uavcan | 6 + 9 files changed, 312 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_UAVCAN.h create mode 100644 libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan create mode 100644 libraries/AP_UAVCAN/dsdl/zrzk/protocol/26160.Message.uavcan create mode 100644 libraries/AP_UAVCAN/dsdl/zrzk/protocol/26161.Request.uavcan diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 374186d54b..221eabde3d 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/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 diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 439597e82b..e11d6dd1dc 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/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 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; } } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 556716198c..5c80bf75a3 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/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, diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp new file mode 100644 index 0000000000..81c813204b --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -0,0 +1,216 @@ +#include + +#if HAL_WITH_UAVCAN + +#include "AP_Proximity_UAVCAN.h" +#include +#include +#include + +#include + +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 *proximity_listener; + proximity_listener = new uavcan::Subscriber(*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 diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h new file mode 100644 index 0000000000..661e8e2c71 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h @@ -0,0 +1,54 @@ +#pragma once + +#include "AP_Proximity_Backend.h" + +#if HAL_WITH_UAVCAN +#include + +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 \ No newline at end of file diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 71344918d0..258b64f0b2 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -53,6 +53,7 @@ #include #include "AP_UAVCAN_Server.h" #include +#include #define LED_DELAY_US 50000 @@ -198,7 +199,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 +249,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(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan new file mode 100644 index 0000000000..9bbc1370c9 --- /dev/null +++ b/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 + \ No newline at end of file diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/protocol/26160.Message.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/protocol/26160.Message.uavcan new file mode 100644 index 0000000000..968aec19bf --- /dev/null +++ b/libraries/AP_UAVCAN/dsdl/zrzk/protocol/26160.Message.uavcan @@ -0,0 +1,3 @@ +uint8 id +uint8 type +uint8[<64] message \ No newline at end of file diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/protocol/26161.Request.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/protocol/26161.Request.uavcan new file mode 100644 index 0000000000..3cbea73853 --- /dev/null +++ b/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 \ No newline at end of file From f3b731955026c2edb1db8b8120697876ebb619e7 Mon Sep 17 00:00:00 2001 From: binsir Date: Sat, 17 Jul 2021 22:09:25 +0800 Subject: [PATCH 2/4] =?UTF-8?q?uavcan=E6=B7=BB=E5=8A=A0=E8=AE=BE=E5=A4=87?= =?UTF-8?q?=E6=B8=A9=E5=BA=A6=E6=95=B0=E6=8D=AE=E6=8E=A5=E6=94=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 17 +++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN.h | 2 ++ 2 files changed, 19 insertions(+) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 71344918d0..8aa4e9ab5a 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -129,6 +130,9 @@ static uavcan::Subscriber UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status); static uavcan::Subscriber *esc_status_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +UC_REGISTRY_BINDER(DeviceTemperatureCb, uavcan::equipment::device::Temperature); +static uavcan::Subscriber *device_temperature_listener[MAX_NUMBER_OF_CAN_DRIVERS]; + AP_UAVCAN::AP_UAVCAN() : _node_allocator() @@ -292,6 +296,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) if (esc_status_listener[driver_index]) { esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status)); } + + device_temperature_listener[driver_index] = new uavcan::Subscriber(*_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) { @@ -763,4 +772,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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 795ac77108..dfe84ca006 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/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_ */ From 74bd0b63546b52e557f173ad0f22c866025ecabf Mon Sep 17 00:00:00 2001 From: binsir Date: Tue, 3 Aug 2021 18:58:16 +0800 Subject: [PATCH 3/4] =?UTF-8?q?uavcan=E7=94=B5=E6=B1=A0=E5=90=8C=E6=97=B6?= =?UTF-8?q?=E6=8E=A5=E4=B8=A4=E8=B7=AF=20ID=E8=AF=86=E5=88=AB=EF=BC=88?= =?UTF-8?q?=E5=BD=93=E5=89=8D=E4=B8=BA=20can=20driver=20index=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 20 +++++++++++-------- .../AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 4 ++-- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index c19e57fc81..8ff6892281 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -24,6 +24,7 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor { // starts with not healthy _state.healthy = false; + _params._serial_number = AP_BATT_SERIAL_NUMBER_DEFAULT; } void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) @@ -44,25 +45,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, bool create_new) { 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; @@ -79,13 +82,13 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u return nullptr; } -void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb) +void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb,uint8_t index) { WITH_SEMAPHORE(_sem_battmon); + _params._serial_number.set_and_notify(index); _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,11 +108,12 @@ 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(),true); + if (driver == nullptr) { return; } - driver->handle_battery_info(cb); + driver->handle_battery_info(cb,ap_uavcan->get_driver_index()); } // read - read the voltage and current diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 0f70a4acda..165f0c3053 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -29,11 +29,11 @@ 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, bool create_new); static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb); private: - void handle_battery_info(const BattInfoCb &cb); + void handle_battery_info(const BattInfoCb &cb,uint8_t); AP_BattMonitor::BattMonitor_State _interim_state; BattMonitor_UAVCAN_Type _type; From 51377db2096ecff375590a66f8cca1f86d00b9c3 Mon Sep 17 00:00:00 2001 From: binsir Date: Tue, 3 Aug 2021 19:25:22 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E6=96=AD=E7=94=B5?= =?UTF-8?q?=E9=BB=98=E8=AE=A4id-1=EF=BC=8Cuavcan=E7=94=B5=E6=B5=81?= =?UTF-8?q?=E8=AE=A1=E5=8E=BB=E9=99=A4=E5=A4=9A=E4=BD=99=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 10 ++++------ libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h | 4 ++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 8ff6892281..fcbb625da5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -24,7 +24,6 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor { // starts with not healthy _state.healthy = false; - _params._serial_number = AP_BATT_SERIAL_NUMBER_DEFAULT; } void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) @@ -45,7 +44,7 @@ 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,uint8_t battery_id, bool create_new) +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; @@ -82,10 +81,9 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u return nullptr; } -void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb,uint8_t index) +void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb) { WITH_SEMAPHORE(_sem_battmon); - _params._serial_number.set_and_notify(index); _interim_state.temperature = cb.msg->temperature; _interim_state.voltage = cb.msg->voltage; _interim_state.current_amps = cb.msg->current; @@ -108,12 +106,12 @@ void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb,uint8_t ind 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_uavcan->get_driver_index(),true); + AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id,ap_uavcan->get_driver_index()); if (driver == nullptr) { return; } - driver->handle_battery_info(cb,ap_uavcan->get_driver_index()); + driver->handle_battery_info(cb); } // read - read the voltage and current diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 165f0c3053..9728a86ea2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h @@ -29,11 +29,11 @@ public: } static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id,uint8_t baterry_id, bool create_new); + 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: - void handle_battery_info(const BattInfoCb &cb,uint8_t); + void handle_battery_info(const BattInfoCb &cb); AP_BattMonitor::BattMonitor_State _interim_state; BattMonitor_UAVCAN_Type _type;