fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Reorganize uavcan params and Subscriber

Browse Source
This commit is contained in:
JacobCrabill 2021-02-18 20:32:01 -08:00 committed by Lorenz Meier
parent 088014c2e1
commit 00814815f4
7 changed files with 565 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/uavcan_v1/CMakeLists.txt
View File

@ -72,6 +72,10 @@ px4_add_module(
${LIBCANARD_DIR}/libcanard/
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
SRCS
ParamManager.hpp
ParamManager.cpp
Subscriber.cpp
Subscriber.hpp
Uavcan.cpp
Uavcan.hpp
${SRCS}

145
src/drivers/uavcan_v1/ParamManager.cpp Normal file
View File

@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ParamManager.cpp
*
* Implements basic functionality of UAVCAN parameter management class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#include "ParamManager.hpp"
bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value)
{
for (auto &param : _uavcan_params) {
if (strcmp(param_name, param.uavcan_name) == 0) {
param_t param_handle = param_find(param.px4_name);
if (param_handle == PARAM_INVALID) {
return false;
}
/// 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;
}
}
return false;
}
bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value)
{
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;
}
/// 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;
}
}
return false;
}
bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
{
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;
}
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;
}
}
return false;
}

76
src/drivers/uavcan_v1/ParamManager.hpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ParamManager.hpp
*
* Defines basic functionality of UAVCAN parameter management class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
#include <lib/parameters/param.h>
#include <uavcan/_register/Name_1_0.h>
#include <uavcan/_register/Value_1_0.h>
typedef struct {
const char *uavcan_name;
const char *px4_name;
bool is_mutable {true};
bool is_persistent {true};
} UavcanParamBinder;
class UavcanParamManager
{
public:
bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value);
bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value);
bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
private:
/// TODO:
/// 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", "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"},
};
};

102
src/drivers/uavcan_v1/Subscriber.cpp Normal file
View File

@ -0,0 +1,102 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Subscriber.cpp
*
* Implements basic functionality of UAVCAN v1 subscriber class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#include "Subscriber.hpp"
/** ----- GPS Position Subscription ----- */
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)
{
// 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 {};
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;
double alt = geo.altitude.meter;
PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
/// do something with the data
}
/** ----- Battery Status Subscription ----- */
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
}

158
src/drivers/uavcan_v1/Subscriber.hpp Normal file
View File

@ -0,0 +1,158 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Subscriber.hpp
*
* Defines basic functionality of UAVCAN v1 subscriber class
*
* @author Jacob Crabill <jacob@flyvoly.com>
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <lib/parameters/param.h>
#include "o1heap/o1heap.h"
#include <canard.h>
#include <canard_dsdl.h>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/primitive/Empty_1_0.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>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include <reg/drone/service/battery/Parameters_0_1.h>
#include <reg/drone/service/battery/Status_0_1.h>
#include "CanardInterface.hpp"
#include "ParamManager.hpp"
class UavcanSubscription
{
public:
UavcanSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
_canard_instance(ins), _param_manager(pmgr), _uavcan_param(uavcan_pname) { };
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()
{
// Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value;
_param_manager.GetParamByName(_uavcan_param, value);
int32_t new_id = value.integer32.value.elements[0];
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 = (CanardPortID)new_id;
PX4_INFO("Subscribing %s on port %d", _uavcan_param, _port_id);
subscribe();
}
}
};
void printInfo()
{
if (_port_id > 0) {
PX4_INFO("Subscribed %s on port %d", _uavcan_param, _port_id);
}
}
protected:
CanardInstance &_canard_instance;
UavcanParamManager &_param_manager;
CanardRxSubscription _canard_sub;
const char *_uavcan_param; // Port ID parameter
/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
CanardPortID _port_id {0};
};
class UavcanGpsSubscription : public UavcanSubscription
{
public:
UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};
class UavcanBmsSubscription : public UavcanSubscription
{
public:
UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) :
UavcanSubscription(ins, pmgr, uavcan_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};

265
src/drivers/uavcan_v1/Uavcan.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -53,75 +53,14 @@ 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)
{
// 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 {};
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;
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,
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(_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"),
_gps0_sub(_canard_instance, _param_manager, "uavcan.sub.gps.0.id"),
_gps1_sub(_canard_instance, _param_manager, "uavcan.sub.gps.1.id"),
_bms0_sub(_canard_instance, _param_manager, "uavcan.sub.bms.0.id"),
_bms1_sub(_canard_instance, _param_manager, "uavcan.sub.bms.1.id"),
_subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
{
pthread_mutex_init(&_node_mutex, nullptr);
@ -211,67 +150,72 @@ int UavcanNode::start(uint32_t node_id, uint32_t bitrate)
return PX4_OK;
}
void UavcanNode::init()
{
// interface init
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,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_pnp_v1_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_access_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_list_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,
sizeof(struct sensor_gps_s),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_srv_gps_subscription);
_initialized = true;
}
}
}
void UavcanNode::Run()
{
pthread_mutex_lock(&_node_mutex);
if (!_initialized) {
// interface init
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,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_pnp_v1_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_access_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_list_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,
sizeof(struct sensor_gps_s),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_srv_gps_subscription);
_initialized = true;
}
}
init();
// return early if still not initialized
if (!_initialized) {
@ -661,10 +605,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=]
if (_param_manager.GetParamByName(msg.name, out_value)) {
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_register_last_received_index++;
@ -716,11 +657,11 @@ int UavcanNode::handleRegisterAccess(const CanardTransfer &receive)
/// TODO: get/set parameter based on whether empty or not
if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0
// Value is empty -- 'Get' only
result = GetParamByName(name, value) ? 0 : -1;
result = _param_manager.GetParamByName(name, value) ? 0 : -1;
} else {
// Set value
result = SetParamByName(name, value) ? 0 : -1;
result = _param_manager.SetParamByName(name, value) ? 0 : -1;
}
@ -779,71 +720,3 @@ int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
}
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;
}
/// 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;
}
}
return false;
}
bool UavcanNode::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
{
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;
}
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;
}
}
return false;
}

128
src/drivers/uavcan_v1/Uavcan.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -54,24 +54,21 @@
#include <canard.h>
#include <canard_dsdl.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>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/primitive/Empty_1_0.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>
#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
//Quick and Dirty UAVCAN register implementation
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Access_1_0.h>
// DS-15 Specification Messages
#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
#include <reg/drone/service/battery/Parameters_0_1.h>
#include <reg/drone/service/battery/Status_0_1.h>
#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_
@ -79,98 +76,7 @@
#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_
#include "CanardInterface.hpp"
typedef struct {
const char *uavcan_name;
const char *px4_name;
bool is_mutable {true};
bool is_persistent {true};
} UavcanParamBinder;
class UavcanSubscription
{
public:
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 callback(const CanardTransfer &msg) = 0;
CanardPortID id() { return _port_id; };
void updateParam()
{
// Set _port_id from _uavcan_param
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) {
// Cancel subscription
unsubscribe();
} else {
if (_port_id > 0) {
// Already active; unsubscribe first
unsubscribe();
}
// Subscribe on the new port 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;
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};
};
class UavcanGpsSubscription : public UavcanSubscription
{
public:
UavcanGpsSubscription(CanardInstance &ins, const char *uavcan_pname, const char *px4_pname) :
UavcanSubscription(ins, uavcan_pname, px4_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};
class UavcanBmsSubscription : public UavcanSubscription
{
public:
UavcanBmsSubscription(CanardInstance &ins, const char *uavcan_pname, const char *px4_pname) :
UavcanSubscription(ins, uavcan_pname, px4_pname) { };
void subscribe() override;
void callback(const CanardTransfer &msg) override;
private:
};
#include "Subscriber.hpp"
class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
{
@ -203,6 +109,7 @@ public:
int32_t active_bitrate{0};
private:
void init();
void Run() override;
void fill_node_info();
@ -215,9 +122,6 @@ private:
int handleBMSStatus(const CanardTransfer &receive);
int handleUORBSensorGPS(const CanardTransfer &receive);
bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value);
bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
void *_uavcan_heap{nullptr};
CanardInterface *const _can_interface;
@ -295,15 +199,5 @@ private:
(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id
)
/// TODO:
/// 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", "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"},
};
UavcanParamManager _param_manager;
};