Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

uavcan_v1: Basic subscriber working

Browse Source
This commit is contained in:
JacobCrabill
2021-02-18 16:49:27 -08:00
committed by Lorenz Meier
parent b88e8b6684
commit 088014c2e1
3 changed files with 94 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Uavcan.cpp
+60 -13
View File
@@ -59,7 +59,7 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
void UavcanGpsSubscription::subscribe()
{
// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
canardRxSubscribe(_canard_instance,
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
@@ -69,6 +69,9 @@ void UavcanGpsSubscription::subscribe()
void UavcanGpsSubscription::callback(const CanardTransfer &receive)
{
// Test with Yakut:
// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
// yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
PX4_INFO("GpsCallback");
reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
@@ -77,14 +80,15 @@ void UavcanGpsSubscription::callback(const CanardTransfer &receive)
double lat = geo.latitude;
double lon = geo.longitude;
PX4_INFO("Latitude: %f, Longitude: %f", lat, lon);
double alt = geo.altitude.meter;
PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
/// do something with the data
}
void UavcanBmsSubscription::subscribe()
{
// Subscribe to messages reg.drone.service.battery.Status.0.1
canardRxSubscribe(_canard_instance,
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
_port_id,
reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
@@ -114,10 +118,10 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
_can_interface(interface),
_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"),
_gps0_sub(_canard_instance, "uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"),
_gps1_sub(_canard_instance, "uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"),
_bms0_sub(_canard_instance, "uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"),
_bms1_sub(_canard_instance, "uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"),
_subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
{
pthread_mutex_init(&_node_mutex, nullptr);
@@ -141,6 +145,12 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
} else {
_canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
}
PX4_INFO("main _canard_instance = %p", &_canard_instance);
for (auto &subscriber : _subscribers) {
subscriber->updateParam();
}
}
UavcanNode::~UavcanNode()
@@ -431,6 +441,10 @@ void UavcanNode::print_info()
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
for (auto &subscriber : _subscribers) {
subscriber->printInfo();
}
pthread_mutex_unlock(&_node_mutex);
}
@@ -611,7 +625,7 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
}
} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
msg.name.name.count) == 0) { //Battery status publisher
msg.name.name.count) == 0) { //Battery status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_node_register_last_received_index++;
@@ -646,6 +660,7 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
} else {
uavcan_register_Value_1_0 out_value;
if (GetParamByName(msg.name, out_value)) {
/// TODO: running into this error again:
// ../../src/drivers/uavcan_v1/Uavcan.cpp:685:1: error: the frame size of 2192 bytes is larger than 2048 bytes [-Werror=frame-larger-than=]
@@ -767,14 +782,33 @@ int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
bool UavcanNode::GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value)
{
/// TODO: More intelligent search needed? Only if using many parameters, I think.
for (auto &param : _uavcan_params) {
if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) {
param_t param_handle = param_find(param.px4_name);
if (param_handle == PARAM_INVALID) {
return false;
}
float out_val;
param_get(param_handle, &out_val); /// TODO: Type casting / setting
/// TODO: What will be our approach for handling other UAVCAN data-value types?
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t out_val {};
param_set(param_handle, &out_val);
value.integer32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_integer32_(&value);
break;
}
case PARAM_TYPE_FLOAT: {
float out_val {};
param_set(param_handle, &out_val);
value.natural32.value.elements[0] = out_val;
uavcan_register_Value_1_0_select_natural32_(&value);
break;
}
}
return true;
}
@@ -785,14 +819,27 @@ bool UavcanNode::GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_reg
bool UavcanNode::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
{
for (auto &param : _uavcan_params) {
for (auto &param : _uavcan_params) {
if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) {
param_t param_handle = param_find(param.px4_name);
if (param_handle == PARAM_INVALID) {
return false;
}
float out_val {};
param_set(param_handle, &out_val); /// TODO: Type casting / setting
switch (param_type(param_handle)) {
case PARAM_TYPE_INT32: {
int32_t in_val = value.integer32.value.elements[0];
param_set(param_handle, &in_val);
break;
}
case PARAM_TYPE_FLOAT: {
float in_val = value.natural32.value.elements[0];
param_set(param_handle, &in_val);
break;
}
}
return true;
}
src/drivers/uavcan_v1/Uavcan.hpp
+26 -13
View File
@@ -90,10 +90,11 @@ typedef struct {
class UavcanSubscription
{
public:
UavcanSubscription(CanardInstance *ins, const char *pname) : _canard_instance(ins), _uavcan_param(pname) { };
UavcanSubscription(CanardInstance &ins, const char *uavcan_pname, const char *px4_pname) :
_canard_instance(ins), _uavcan_param(uavcan_pname), _px4_param(px4_pname) { };
virtual void subscribe() = 0;
virtual void unsubscribe() { canardRxUnsubscribe(_canard_instance, CanardTransferKindMessage, _port_id); };
virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); };
virtual void callback(const CanardTransfer &msg) = 0;
@@ -103,8 +104,9 @@ public:
{
// Set _port_id from _uavcan_param
CanardPortID new_id {0};
int32_t new_id {0};
/// TODO: --- update parameter here: new_id = uavcan_param_get(_uavcan_param);
param_get(param_find(_px4_param), &new_id);
if (_port_id != new_id) {
if (new_id == 0) {
@@ -118,16 +120,25 @@ public:
}
// Subscribe on the new port ID
_port_id = new_id;
_port_id = (CanardPortID)new_id;
printf("Subscribing %s on port %d\n", _uavcan_param, _port_id);
subscribe();
}
}
};
void printInfo()
{
if (_port_id > 0) {
printf("Subscribed %s on port %d\n", _uavcan_param, _port_id);
}
}
protected:
CanardInstance *_canard_instance;
CanardInstance &_canard_instance;
CanardRxSubscription _canard_sub;
const char *_uavcan_param; // Port ID parameter
const char *_px4_param;
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
CanardPortID _port_id {0};
@@ -136,7 +147,8 @@ protected:
class UavcanGpsSubscription : public UavcanSubscription
{
public:
UavcanGpsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { };
UavcanGpsSubscription(CanardInstance &ins, const char *uavcan_pname, const char *px4_pname) :
UavcanSubscription(ins, uavcan_pname, px4_pname) { };
void subscribe() override;
@@ -149,7 +161,8 @@ private:
class UavcanBmsSubscription : public UavcanSubscription
{
public:
UavcanBmsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { };
UavcanBmsSubscription(CanardInstance &ins, const char *uavcan_pname, const char *px4_pname) :
UavcanSubscription(ins, uavcan_pname, px4_pname) { };
void subscribe() override;
@@ -286,11 +299,11 @@ private:
/// use qsort() to order alphabetically by UAVCAN name
/// copy over Ken's parameter find/get/set code
const UavcanParamBinder _uavcan_params[6] {
{"uavcan.pub.esc.0.id", "UAVCANV1_ESC0_PID"},
{"uavcan.pub.servo.0.id", "UAVCANV1_SERVO0_PID"},
{"uavcan.sub.gps.0.id", "UAVCANV1_GPS0_PID"},
{"uavcan.sub.gps.1.id", "UAVCANV1_GPS1_PID"},
{"uavcan.sub.bms.0.id", "UAVCANV1_BMS0_PID"},
{"uavcan.sub.bms.1.id", "UAVCANV1_BMS1_PID"},
{"uavcan.pub.esc.0.id", "UCAN1_ESC0_PID"},
{"uavcan.pub.servo.0.id", "UCAN1_SERVO0_PID"},
{"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"},
{"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"},
{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"},
{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"},
};
};
src/drivers/uavcan_v1/parameters.c
+8
View File
@@ -85,3 +85,11 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
* @group UAVCAN v1
*/
PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_SERVO0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);