diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 45acdfcf4bef..4ce4560eeb5d 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -10,8 +10,6 @@ #include #include #include -#include -#include #include #include @@ -68,6 +66,25 @@ class UAVCAN_EXPORT NodeStatusMonitor { } }; + bool HasFreshNodeStatus(){ + return _has_fresh_node_status_data; + } + + uint8_t GetLastNodeStatusData(protocol::NodeStatus * node_status_to_fill){ + //Just go ahead and fill in the data with whatever we heard last. It's up to whoever is getting the data to do something with it. + node_status_to_fill->uptime_sec = _last_received_node_status_data.uptime_sec; + node_status_to_fill->health = _last_received_node_status_data.health; + node_status_to_fill->mode = _last_received_node_status_data.mode; + node_status_to_fill->sub_mode = _last_received_node_status_data.sub_mode; + node_status_to_fill->vendor_specific_status_code = _last_received_node_status_data.vendor_specific_status_code; + + //Someone read the data, it's no longer fresh + _has_fresh_node_status_data = false; + + //Return the node ID responsible for this data + return _last_received_node_status_id; + } + private: enum { TimerPeriodMs100 = 2 }; @@ -79,14 +96,12 @@ class UAVCAN_EXPORT NodeStatusMonitor Subscriber sub_; - dronecan_node_status_s _node_status{}; - uORB::Publication _node_status_pub{ORB_ID(dronecan_node_status)}; + protocol::NodeStatus _last_received_node_status_data; + uint8_t _last_received_node_status_id; + bool _has_fresh_node_status_data; TimerEventForwarder timer_; - uint8_t _module_id_to_logging_index[dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED]; //Only handle as many node statuses as the DronecanNodeStatus message tells us we can - uint8_t _module_ids_being_logged = 0; //Keep track of how many node statuses are in play - struct Entry { NodeStatus status; @@ -127,43 +142,6 @@ class UAVCAN_EXPORT NodeStatusMonitor entry = new_entry_value; } - bool loggingModuleIdNodeStatus(uint8_t reporting_node_id) - { - for(uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++){ - if(_module_id_to_logging_index[array_index] == reporting_node_id){ - return true; - } - } - - return false; - } - - bool addModuleIdToLoggedStatuses(uint8_t module_id_to_add) - { - if(_module_ids_being_logged < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED){ - _module_id_to_logging_index[_module_ids_being_logged] = module_id_to_add; - _module_ids_being_logged++; - - return true; - } - - return false; - } - - uint8_t getModuleIdLogIndex(uint8_t module_id) - { - uint8_t return_array_index = 0; - - for(uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++){ - if(_module_id_to_logging_index[array_index] == module_id){ - return_array_index = array_index; - break; - } - } - - return return_array_index; - } - void handleNodeStatus(const ReceivedDataStructure& msg) { Entry new_entry; @@ -176,32 +154,18 @@ class UAVCAN_EXPORT NodeStatusMonitor handleNodeStatusMessage(msg); - uint8_t received_node_id = msg.getSrcNodeID().get(); - - //Check to see if we're logging this ID already. Use it to determine our next steps - bool already_logging_this_id = loggingModuleIdNodeStatus(received_node_id); + //Update our last received data so the outside world can come get it + _last_received_node_status_data.uptime_sec = msg.uptime_sec; + _last_received_node_status_data.health = msg.health; + _last_received_node_status_data.mode = msg.mode; + _last_received_node_status_data.sub_mode = msg.sub_mode; + _last_received_node_status_data.vendor_specific_status_code = msg.vendor_specific_status_code; - //Check to see if we're already logging this module ID, if not, try to add it to our list of module IDs to log. - //If we are, deal with publishing a new message - if(!already_logging_this_id){ - addModuleIdToLoggedStatuses(received_node_id); - }else if(already_logging_this_id){ - //Make a new message with the data we've got - auto &node_status_message = _node_status.node_status_id[getModuleIdLogIndex(received_node_id)]; + //Make sure to record who sent this data + _last_received_node_status_id = msg.getSrcNodeID().get(); - //Fill in the actual node's status information - node_status_message.timestamp = hrt_absolute_time(); - node_status_message.uptime_sec = msg.uptime_sec; - node_status_message.node_id = received_node_id; - node_status_message.health = msg.health; - node_status_message.mode = msg.mode; - node_status_message.sub_mode = msg.sub_mode; - node_status_message.vendor_specific_status_code = msg.vendor_specific_status_code; - - _node_status.timestamp = hrt_absolute_time(); - - _node_status_pub.publish(_node_status); - } + //Make sure to mark our stored data as new + _has_fresh_node_status_data = true; } void handleTimerEvent(const TimerEvent&) @@ -269,7 +233,6 @@ class UAVCAN_EXPORT NodeStatusMonitor { timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent)); timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); - _node_status_pub.advertise(); } return res; } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 87f3110cac7d..0385418798e6 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -635,6 +635,10 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } } + // Init Node Status logging + _node_ids_being_logged = 0; + _node_status_pub.advertise(); + // Start the Node return _node.start(); } @@ -1004,6 +1008,41 @@ UavcanNode::Run() } } + //Check to see if our node status monitor has any new data for us to put out over uORB + if (_node_status_monitor.HasFreshNodeStatus()) { + //Go ahead and grab the data from the node status monitor + uavcan::protocol::NodeStatus received_node_status_data; + uint8_t reporting_node_id = _node_status_monitor.GetLastNodeStatusData(&received_node_status_data); + + //We now have everything we need to publish node status properly + //Check to see if we're logging this ID already. Use it to determine our next steps + bool already_logging_this_id = check_if_already_logging_node_id_status(reporting_node_id); + + //Check to see if we're already logging this module ID, if not, try to add it to our list of module IDs to log. + //If we are, deal with publishing a new message + if (!already_logging_this_id) { + add_node_id_to_logged_statuses(reporting_node_id); + + } else if (already_logging_this_id) { + //Now we can finally fill in our publisher's data and publish + auto &node_status_message = _node_status.node_status_id[get_node_id_log_index(reporting_node_id)]; + + //Fill in the actual node's status information + node_status_message.timestamp = hrt_absolute_time(); + node_status_message.uptime_sec = received_node_status_data.uptime_sec; + node_status_message.node_id = reporting_node_id; + node_status_message.health = received_node_status_data.health; + node_status_message.mode = received_node_status_data.mode; + node_status_message.sub_mode = received_node_status_data.sub_mode; + node_status_message.vendor_specific_status_code = received_node_status_data.vendor_specific_status_code; + + _node_status.timestamp = hrt_absolute_time(); + + _node_status_pub.publish(_node_status); + } + + } + perf_end(_cycle_perf); pthread_mutex_unlock(&_node_mutex); @@ -1361,6 +1400,48 @@ UavcanNode::get_next_dirty_node_id(uint8_t base) return base; } +bool +UavcanNode::check_if_already_logging_node_id_status(uint8_t reporting_node_id) +{ + for (uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++) { + if (_node_id_to_logging_index[array_index] == reporting_node_id) { + return true; + } + } + + return false; +} + +bool +UavcanNode::add_node_id_to_logged_statuses(uint8_t node_id_to_add) +{ + //If we have space for a new logged ID, and that ID is possible + if ((_node_ids_being_logged < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED) + && (node_id_to_add < uavcan::NodeID::Max)) { + _node_id_to_logging_index[_node_ids_being_logged] = node_id_to_add; + _node_ids_being_logged++; + + return true; + } + + return false; +} + +uint8_t +UavcanNode::get_node_id_log_index(uint8_t node_id) +{ + uint8_t return_array_index = 0; + + for (uint8_t array_index = 0; array_index < dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED; array_index++) { + if (_node_id_to_logging_index[array_index] == node_id) { + return_array_index = array_index; + break; + } + } + + return return_array_index; +} + /* * App entry point */ diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index d249e5d838cc..d2652678e664 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -105,6 +105,7 @@ #include #include #include +#include using namespace time_literals; @@ -318,6 +319,10 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::PublicationMulti _can_status_pub{ORB_ID(can_interface_status)}; + uORB::Publication _node_status_pub{ORB_ID(dronecan_node_status)}; + dronecan_node_status_s _node_status{}; + uint8_t _node_id_to_logging_index[dronecan_node_status_s::MAX_NODE_STATUSES_LOGGED]; //Only handle as many node statuses as the DronecanNodeStatus message tells us we can + uint8_t _node_ids_being_logged; //Keep track of how many node statuses are in play hrt_abstime _last_can_status_pub{0}; orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr}; @@ -359,6 +364,11 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams void param_count(uavcan::NodeID node_id); void param_opcode(uavcan::NodeID node_id); + // A few functions that will help us properly log Node Status information + bool check_if_already_logging_node_id_status(uint8_t reporting_node_id); + bool add_node_id_to_logged_statuses(uint8_t node_id_to_add); + uint8_t get_node_id_log_index(uint8_t node_id); + uint8_t get_next_active_node_id(uint8_t base); uint8_t get_next_dirty_node_id(uint8_t base); void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }