fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-12 12:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

[UAVCANv1] Subscriber multi topic subscription

Browse Source 
reg.drone.bms full support
legacy.equipment.bms support
This commit is contained in:
Peter van der Perk
2021-04-20 21:44:40 +02:00
committed by Lorenz Meier
parent 5e7ebbe47c
commit 399815469f
20 changed files with 483 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Uavcan.cpp
+5 -29
View File
@@ -168,13 +168,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
bms_port_id,
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_srv_battery_subscription);
canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet
CanardTransferKindMessage,
gps_port_id,
@@ -281,6 +274,8 @@ void UavcanNode::Run()
CanardFrame received_frame{};
received_frame.payload = &data;
/* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer */
if (_can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
@@ -297,22 +292,19 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == bms_port_id) {
result = handleBMSStatus(receive);
} else if (receive.port_id == gps_port_id) {
if (receive.port_id == gps_port_id) {
result = handleUORBSensorGPS(receive);
} else if (receive.port_id > 0) {
// If not a fixed port ID, check any subscribers which may have registered it
for (auto &subscriber : _subscribers) {
if (receive.port_id == subscriber->id()) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
for (auto &subscriber : _dynsubscribers) {
if (receive.port_id == subscriber->id()) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
@@ -449,22 +441,6 @@ void UavcanNode::sendHeartbeat()
}
}
int UavcanNode::handleBMSStatus(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
//TODO deserialize
/*
battery_status_s battery_status{};
battery_status.id = bms_status.battery_id;
battery_status.voltage_v = bms_status.voltage;
//battery_status.remaining = bms_status.remaining_capacity;
battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(battery_status);
*/
return 0;
}
int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);