From 7d2a6afb79d878fdf5838ae282acd20d6f32ac7a Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Thu, 18 Feb 2021 09:52:13 -0800 Subject: [PATCH] uavcan_v1: More work on Subscriber class --- src/drivers/uavcan_v1/Uavcan.cpp | 80 +++++++++++++++++++++++++---- src/drivers/uavcan_v1/Uavcan.hpp | 87 +++++++++++++++++++++++++++----- 2 files changed, 146 insertions(+), 21 deletions(-) diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index df1a04b6d7..6843f7752b 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -53,15 +53,72 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree # endif // CONFIG_CAN #endif // NuttX +/** Begin UavcanSubscription Class Definitions **/ +/// TODO: Reorganize into separate files + +void UavcanGpsSubscription::subscribe() +{ + // Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1 + canardRxSubscribe(_canard_instance, + CanardTransferKindMessage, + _port_id, + reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); +} + +void UavcanGpsSubscription::callback(const CanardTransfer &receive) +{ + PX4_INFO("GpsCallback"); + + reg_drone_physics_kinematics_geodetic_Point_0_1 geo {}; + size_t geo_size_in_bits = receive.payload_size; + reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits); + + double lat = geo.latitude; + double lon = geo.longitude; + PX4_INFO("Latitude: %f, Longitude: %f", lat, lon); + /// do something with the data +} + +void UavcanBmsSubscription::subscribe() +{ + // Subscribe to messages reg.drone.service.battery.Status.0.1 + canardRxSubscribe(_canard_instance, + CanardTransferKindMessage, + _port_id, + reg_drone_service_battery_Status_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); +} + +void UavcanBmsSubscription::callback(const CanardTransfer &receive) +{ + PX4_INFO("BmsCallback"); + + reg_drone_service_battery_Status_0_1 bat {}; + size_t bat_size_in_bits = receive.payload_size; + reg_drone_service_battery_Status_0_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bits); + + uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0]; + uavcan_si_unit_voltage_Scalar_1_0 V_Max = bat.cell_voltage_min_max[1]; + double vmin = static_cast(V_Min.volt); + double vmax = static_cast(V_Max.volt); + PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax); + /// do something with the data +} + +/** End UavcanSubscription Class Definitions **/ + UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), _can_interface(interface), - _gps0_sub("uavcan.sub.gps.0.id"), - _gps1_sub("uavcan.sub.gps.1.id"), - _bms0_sub("uavcan.sub.bms.0.id"), - _bms1_sub("uavcan.sub.bms.1.id"), - _subscribers({_gps0_sub, _gps1_sub, _bms0_sub, _bms1_sub}) + _gps0_sub(&_canard_instance, "uavcan.sub.gps.0.id"), + _gps1_sub(&_canard_instance, "uavcan.sub.gps.1.id"), + _bms0_sub(&_canard_instance, "uavcan.sub.bms.0.id"), + _bms1_sub(&_canard_instance, "uavcan.sub.bms.1.id"), + _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub} { pthread_mutex_init(&_node_mutex, nullptr); @@ -153,6 +210,8 @@ void UavcanNode::Run() if (_can_interface) { if (_can_interface->init() == PX4_OK) { + // We can't accept just any message; we must fist subscribe to specific IDs + // Subscribe to messages uavcan.node.Heartbeat. canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, @@ -188,7 +247,7 @@ void UavcanNode::Run() canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, bms_port_id, - reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_, + reg_drone_service_battery_Status_0_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_drone_srv_battery_subscription); @@ -221,7 +280,10 @@ void UavcanNode::Run() updateParams(); for (auto &subscriber : _subscribers) { - subscriber.updateParam(); + // Have the subscriber update its associated port-id parameter + // If the port-id changes, (re)start the subscription + // Setting the port-id to 0 disables the subscription + subscriber->updateParam(); } } @@ -363,8 +425,8 @@ void UavcanNode::Run() } else if (receive.port_id > 0) { for (auto &subscriber : _subscribers) { - if (receive.port_id == subscriber.id()) { - subscriber.callback(receive); + if (receive.port_id == subscriber->id()) { + subscriber->callback(receive); } } } diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 3be1dd1fe3..b27ae96970 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -54,10 +54,14 @@ #include #include -#include -#include +#include +#include #include +/// DSDL UPDATE WIP +#include +/// --------------- + //Quick and Dirty PNP imlementation only V1 for now as well #include #include @@ -82,20 +86,74 @@ typedef struct { class UavcanSubscription { public: - UavcanSubscription(const char *pname) : _uavcan_param(pname) {}; + UavcanSubscription(CanardInstance *ins, const char *pname) : _canard_instance(ins), _uavcan_param(pname) { }; + + virtual void subscribe() = 0; + virtual void unsubscribe() { canardRxUnsubscribe(_canard_instance, CanardTransferKindMessage, _port_id); }; - virtual void callback(const CanardTransfer &msg) { (void)msg; /* TODO virtual func; implement in subclass */ }; + virtual void callback(const CanardTransfer &msg) = 0; CanardPortID id() { return _port_id; }; - void updateParam() { /* TODO: Set _port_id from _uavcan_param */ }; + void updateParam() + { + // Set _port_id from _uavcan_param + + CanardPortID new_id {0}; + /// TODO: --- update parameter here: new_id = uavcan_param_get(_uavcan_param); + + if (_port_id != new_id) { + if (new_id == 0) { + // Cancel subscription + unsubscribe(); + + } else { + if (_port_id > 0) { + // Already active; unsubscribe first + unsubscribe(); + } + + // Subscribe on the new port ID + _port_id = new_id; + subscribe(); + } + } + }; -private: +protected: + CanardInstance *_canard_instance; + CanardRxSubscription _canard_sub; const char *_uavcan_param; CanardPortID _port_id {0}; }; +class UavcanGpsSubscription : public UavcanSubscription +{ +public: + UavcanGpsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { }; + + void subscribe() override; + + void callback(const CanardTransfer &msg) override; + +private: + +}; + +class UavcanBmsSubscription : public UavcanSubscription +{ +public: + UavcanBmsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { }; + + void subscribe() override; + + void callback(const CanardTransfer &msg) override; + +private: + +}; + class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem { /* @@ -156,6 +214,11 @@ private: CanardRxSubscription _register_access_subscription; CanardRxSubscription _register_list_subscription; + CanardRxSubscription _canard_gps0_sub; + CanardRxSubscription _canard_gps1_sub; + CanardRxSubscription _canard_bms0_sub; + CanardRxSubscription _canard_bms1_sub; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -188,16 +251,16 @@ private: int32_t _node_register_last_received_index = -1; // regulated::drone::sensor::BMSStatus_1_0 - uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_]; + uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_service_battery_Status_0_1_EXTENT_BYTES_]; hrt_abstime _regulated_drone_sensor_bmsstatus_last{0}; CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0}; - UavcanSubscription _gps0_sub; - UavcanSubscription _gps1_sub; - UavcanSubscription _bms0_sub; - UavcanSubscription _bms1_sub; + UavcanGpsSubscription _gps0_sub; + UavcanGpsSubscription _gps1_sub; + UavcanBmsSubscription _bms0_sub; + UavcanBmsSubscription _bms1_sub; - UavcanSubscription _subscribers[4]; + UavcanSubscription *_subscribers[4]; /// TODO: turn into List DEFINE_PARAMETERS( (ParamInt) _param_uavcan_v1_enable,