diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index c19e57fc81..fcbb625da5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/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; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h index 0f70a4acda..9728a86ea2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/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: 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..4782739d75 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 @@ -53,6 +54,7 @@ #include #include "AP_UAVCAN_Server.h" #include +#include #define LED_DELAY_US 50000 @@ -129,6 +131,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() @@ -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(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); @@ -292,6 +298,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 +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 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_ */ 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