diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 95d4ca072c..50f8b038bf 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -74,8 +74,6 @@ px4_add_module( 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 index 5d9928edbe..7f37414aeb 100644 --- a/src/drivers/uavcan_v1/ParamManager.cpp +++ b/src/drivers/uavcan_v1/ParamManager.cpp @@ -55,7 +55,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ switch (param_type(param_handle)) { case PARAM_TYPE_INT32: { int32_t out_val {}; - param_set(param_handle, &out_val); + param_get(param_handle, &out_val); value.integer32.value.elements[0] = out_val; uavcan_register_Value_1_0_select_integer32_(&value); break; @@ -63,7 +63,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ case PARAM_TYPE_FLOAT: { float out_val {}; - param_set(param_handle, &out_val); + param_get(param_handle, &out_val); value.natural32.value.elements[0] = out_val; uavcan_register_Value_1_0_select_natural32_(&value); break; diff --git a/src/drivers/uavcan_v1/Subscriber.cpp b/src/drivers/uavcan_v1/Subscriber.cpp deleted file mode 100644 index 94a1561278..0000000000 --- a/src/drivers/uavcan_v1/Subscriber.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************** - * - * 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/Subscribers/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/Battery.hpp new file mode 100644 index 0000000000..c790351614 --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/Battery.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 Battery.hpp + * + * Defines basic functionality of UAVCAN v1 Battery subscription + * + * @author Jacob Crabill + */ + +#pragma once + +// DS-15 Specification Messages +#include +#include + +#include "Subscriber.hpp" + +class UavcanBmsSubscription : public UavcanSubscription +{ +public: + UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanSubscription(ins, pmgr, uavcan_pname) { }; + + void subscribe() override + { + // 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 callback(const CanardTransfer &receive) override + { + 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/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp new file mode 100644 index 0000000000..1d7642a3f0 --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 Gnss.hpp + * + * Defines basic functionality of UAVCAN v1 GNSS subscription + * + * @author Jacob Crabill + */ + +#pragma once + +// DS-15 Specification Messages +#include + +#include "Subscriber.hpp" + +class UavcanGpsSubscription : public UavcanSubscription +{ +public: + UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanSubscription(ins, pmgr, uavcan_pname) { }; + + void subscribe() override + { + // 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); + + /** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan): + * # A compliant implementation of this service should publish the following subjects: + * # + * # PUBLISHED SUBJECT NAME SUBJECT TYPE TYP. RATE [Hz] + * # point_kinematics reg.drone.physics.kinematics.geodetic.PointStateVarTs 1...100 + * # time reg.drone.service.gnss.Time 1...10 + * # heartbeat reg.drone.service.gnss.Heartbeat ~1 + * # sensor_status reg.drone.service.sensor.Status ~1 + * + * Not mentioned, but should also be included: Dilution of Precision + * (reg.drone.service.gnss.DilutionOfPrecision.0.1.uavcan) + * For PX4, only the PointStateVarTs, DilutionOfPrecision, and perhaps Time would be needed + * to publish 'sensor_gps' + */ + }; + + void callback(const CanardTransfer &receive) override + { + // 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 + }; + +}; diff --git a/src/drivers/uavcan_v1/Subscriber.hpp b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp similarity index 80% rename from src/drivers/uavcan_v1/Subscriber.hpp rename to src/drivers/uavcan_v1/Subscribers/Subscriber.hpp index 03a3e373f4..16558af3de 100644 --- a/src/drivers/uavcan_v1/Subscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp @@ -49,11 +49,6 @@ #include -#include "o1heap/o1heap.h" - -#include -#include - #include #include #include @@ -64,13 +59,8 @@ #include #include -// DS-15 Specification Messages -#include -#include -#include - -#include "CanardInterface.hpp" -#include "ParamManager.hpp" +#include "../CanardInterface.hpp" +#include "../ParamManager.hpp" class UavcanSubscription { @@ -88,7 +78,6 @@ public: 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]; @@ -128,31 +117,3 @@ protected: 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 eeba6384bd..31084d7139 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -56,12 +56,8 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), - _can_interface(interface), - _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} + _can_interface(interface)//, + // _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub} { pthread_mutex_init(&_node_mutex, nullptr); @@ -389,6 +385,8 @@ void UavcanNode::print_info() subscriber->printInfo(); } + _mixing_output.printInfo(); + pthread_mutex_unlock(&_node_mutex); } @@ -479,6 +477,13 @@ void UavcanNode::sendHeartbeat() } _uavcan_node_heartbeat_last = transfer.timestamp_usec; + + uint16_t outputs[8] {}; + outputs[0] = 1000; + outputs[1] = 2000; + outputs[2] = 3000; + outputs[3] = 4000; + _mixing_output.updateOutputs(false, outputs, 4, 1); } } @@ -720,3 +725,25 @@ int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive) return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1; } + + +bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + // Note: This gets called from MixingOutput from within its update() function + // Hence, the mutex lock in UavcanMixingInterface::Run() is in effect + + /// TODO: Need esc/servo metadata / bitmask(s) + _esc_controller.update_outputs(stop_motors, outputs, num_outputs); + // _servo_controller.update_outputs(stop_motors, outputs, num_outputs); + + return true; +} + +void UavcanMixingInterface::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(false); + pthread_mutex_unlock(&_node_mutex); +} diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 0fc5f55c4a..ec2cc57be6 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -76,7 +77,47 @@ #define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_ #include "CanardInterface.hpp" -#include "Subscriber.hpp" +#include "Publishers/Publisher.hpp" +#include "Subscribers/Subscriber.hpp" +#include "Subscribers/Gnss.hpp" +#include "Subscribers/Battery.hpp" +#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service + +/** + * UAVCAN mixing class. + * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling + * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at + * a fixed rate or upon bus updates). + * Both work items are expected to run on the same work queue. + */ +class UavcanMixingInterface : public OutputModuleInterface +{ +public: + UavcanMixingInterface(pthread_mutex_t &node_mutex, + UavcanEscController &esc_controller) //, UavcanServoController &servo_controller) + : OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan), + _node_mutex(node_mutex), + _esc_controller(esc_controller)/*, + _servo_controller(servo_controller)*/ {} + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + void mixerChanged() override {}; + + void printInfo() { _mixing_output.printStatus(); } + + MixingOutput &mixingOutput() { return _mixing_output; } + +protected: + void Run() override; +private: + friend class UavcanNode; + pthread_mutex_t &_node_mutex; + UavcanEscController &_esc_controller; + // UavcanServoController &_servo_controller; + MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; +}; class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem { @@ -184,13 +225,6 @@ private: hrt_abstime _regulated_drone_sensor_bmsstatus_last{0}; CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0}; - UavcanGpsSubscription _gps0_sub; - UavcanGpsSubscription _gps1_sub; - UavcanBmsSubscription _bms0_sub; - UavcanBmsSubscription _bms1_sub; - - UavcanSubscription *_subscribers[4]; /// TODO: turn into List - DEFINE_PARAMETERS( (ParamInt) _param_uavcan_v1_enable, (ParamInt) _param_uavcan_v1_id, @@ -200,4 +234,14 @@ private: ) UavcanParamManager _param_manager; + UavcanGpsSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"}; + UavcanGpsSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"}; + UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"}; + UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"}; + + UavcanSubscription *_subscribers[4] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}; /// TODO: turn into List + + UavcanEscController _esc_controller {_canard_instance, 22}; + + UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; };