diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 8b6b200fc9..95d4ca072c 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -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} diff --git a/src/drivers/uavcan_v1/ParamManager.cpp b/src/drivers/uavcan_v1/ParamManager.cpp new file mode 100644 index 0000000000..5d9928edbe --- /dev/null +++ b/src/drivers/uavcan_v1/ParamManager.cpp @@ -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 + */ + +#include "ParamManager.hpp" + +bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value) +{ + for (auto ¶m : _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 ¶m : _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 ¶m : _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; +} diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp new file mode 100644 index 0000000000..cc078df40c --- /dev/null +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -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 + */ + +#pragma once + +#include + +#include +#include + +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"}, + }; +}; diff --git a/src/drivers/uavcan_v1/Subscriber.cpp b/src/drivers/uavcan_v1/Subscriber.cpp new file mode 100644 index 0000000000..94a1561278 --- /dev/null +++ b/src/drivers/uavcan_v1/Subscriber.cpp @@ -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 + */ + +#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(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 +} diff --git a/src/drivers/uavcan_v1/Subscriber.hpp b/src/drivers/uavcan_v1/Subscriber.hpp new file mode 100644 index 0000000000..03a3e373f4 --- /dev/null +++ b/src/drivers/uavcan_v1/Subscriber.hpp @@ -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 + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "o1heap/o1heap.h" + +#include +#include + +#include +#include +#include +#include + +//Quick and Dirty PNP imlementation only V1 for now as well +#include +#include +#include + +// DS-15 Specification Messages +#include +#include +#include + +#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: + +}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 8f35224da5..eeba6384bd 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -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(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(_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 ¶m : _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 ¶m : _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; -} diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index ad887b0d82..0fc5f55c4a 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -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 #include -#include -#include -#include - -/// DSDL UPDATE WIP -#include +#include +#include #include +#include #include -/// --------------- //Quick and Dirty PNP imlementation only V1 for now as well #include #include #include -//Quick and Dirty UAVCAN register implementation -#include -#include +// DS-15 Specification Messages +#include +#include +#include #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) _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; };