mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-29 03:36:07 +08:00
uavcan_v1: More work on Subscriber class
This commit is contained in:
JacobCrabill
committed by
Lorenz Meier
Lorenz Meier
parent
c5184f3b46
commit
7d2a6afb79
@@ -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<double>(V_Min.volt);
|
||||
double vmax = static_cast<double>(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -54,10 +54,14 @@
|
||||
#include <canard.h>
|
||||
#include <canard_dsdl.h>
|
||||
|
||||
#include <reg/drone/srv/battery/Status_0_1.h>
|
||||
#include <reg/drone/srv/battery/Parameters_0_1.h>
|
||||
#include <reg/drone/service/battery/Status_0_1.h>
|
||||
#include <reg/drone/service/battery/Parameters_0_1.h>
|
||||
#include <uavcan/node/Heartbeat_1_0.h>
|
||||
|
||||
/// DSDL UPDATE WIP
|
||||
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
|
||||
/// ---------------
|
||||
|
||||
//Quick and Dirty PNP imlementation only V1 for now as well
|
||||
#include <uavcan/node/ID_1_0.h>
|
||||
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
|
||||
@@ -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 callback(const CanardTransfer &msg) { (void)msg; /* TODO virtual func; implement in subclass */ };
|
||||
virtual void subscribe() = 0;
|
||||
virtual void unsubscribe() { canardRxUnsubscribe(_canard_instance, CanardTransferKindMessage, _port_id); };
|
||||
|
||||
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
|
||||
|
||||
private:
|
||||
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();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
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<UavcanSubscription*>
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
||||
|
||||
Reference in New Issue
Block a user