Skip to content

Commit

Permalink
decouple uorb publication from the uavcan library
Browse files Browse the repository at this point in the history
  • Loading branch information
@vertiq-jordan
vertiq-jordan committed Nov 4, 2024
1 parent 2595534 commit cc77c15
Show file tree
Hide file tree
Showing 3 changed files with 123 additions and 69 deletions.
101 changes: 32 additions & 69 deletions src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,6 @@
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/dronecan_node_status.h>
#include <cassert>
#include <cstdlib>

Expand Down Expand Up @@ -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 };

Expand All @@ -79,14 +96,12 @@ class UAVCAN_EXPORT NodeStatusMonitor

Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;

dronecan_node_status_s _node_status{};
uORB::Publication<dronecan_node_status_s> _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<TimerCallback> 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;
Expand Down Expand Up @@ -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<protocol::NodeStatus>& msg)
{
Entry new_entry;
Expand All @@ -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&)
Expand Down Expand Up @@ -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;
}
Expand Down
81 changes: 81 additions & 0 deletions src/drivers/uavcan/uavcan_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
*/
Expand Down
10 changes: 10 additions & 0 deletions src/drivers/uavcan/uavcan_main.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/dronecan_node_status.h>

using namespace time_literals;

Expand Down Expand Up @@ -318,6 +319,10 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::PublicationMulti<can_interface_status_s> _can_status_pub{ORB_ID(can_interface_status)};
uORB::Publication<dronecan_node_status_s> _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};
Expand Down Expand Up @@ -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); }
Expand Down

0 comments on commit cc77c15

Please sign in to comment.