From ea97a38b2b5edaa515d8d6feb836c45f302b3b95 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Wed, 15 Mar 2023 09:20:07 -0600 Subject: [PATCH] Initial Cannode PWM Support (#19748) * Add uavcannode esc/servo pwm control pipeline * Remove cannode rc.interface with mixer purge * Cannode add Kconfig options to reduce flash usage --- boards/ark/can-flow/default.px4board | 2 + boards/ark/can-gps/default.px4board | 7 + boards/ark/can-rtk-gps/default.px4board | 8 + boards/ark/cannode/default.px4board | 19 +++ boards/ark/cannode/init/rc.board_defaults | 7 +- boards/cuav/can-gps-v1/default.px4board | 7 + boards/freefly/can-rtk-gps/default.px4board | 8 + boards/holybro/can-gps-v1/default.px4board | 7 + boards/matek/gnss-m9n-f4/default.px4board | 8 + boards/nxp/ucans32k146/default.px4board | 15 ++ src/drivers/uavcan/CMakeLists.txt | 2 + src/drivers/uavcan/actuators/esc.cpp | 2 +- src/drivers/uavcan/actuators/esc.hpp | 2 - src/drivers/uavcan/arming_status.cpp | 81 ++++++++++ src/drivers/uavcan/arming_status.hpp | 83 +++++++++++ src/drivers/uavcan/uavcan_main.cpp | 17 ++- src/drivers/uavcan/uavcan_main.hpp | 2 + src/drivers/uavcan/uavcan_params.c | 12 ++ src/drivers/uavcannode/Kconfig | 73 ++++++++- .../uavcannode/Publishers/ESCStatus.hpp | 96 ++++++++++++ .../uavcannode/Subscribers/ArmingStatus.hpp | 98 ++++++++++++ .../uavcannode/Subscribers/ESCRawCommand.hpp | 105 +++++++++++++ .../Subscribers/ServoArrayCommand.hpp | 108 ++++++++++++++ src/drivers/uavcannode/UavcanNode.cpp | 141 ++++++++++++++++-- 24 files changed, 891 insertions(+), 19 deletions(-) create mode 100644 src/drivers/uavcan/arming_status.cpp create mode 100644 src/drivers/uavcan/arming_status.hpp create mode 100644 src/drivers/uavcannode/Publishers/ESCStatus.hpp create mode 100644 src/drivers/uavcannode/Subscribers/ArmingStatus.hpp create mode 100644 src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp create mode 100644 src/drivers/uavcannode/Subscribers/ServoArrayCommand.hpp diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board index b6d38f94a2..a120df6638 100644 --- a/boards/ark/can-flow/default.px4board +++ b/boards/ark/can-flow/default.px4board @@ -13,6 +13,8 @@ CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y +CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index 8f69c01983..86af45774c 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -13,6 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index 8f69c01983..b74e74a630 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -13,6 +13,14 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/ark/cannode/default.px4board b/boards/ark/cannode/default.px4board index ff8ac2c984..332237310a 100644 --- a/boards/ark/cannode/default.px4board +++ b/boards/ark/cannode/default.px4board @@ -9,6 +9,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_COMMON_HYGROMETERS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y @@ -17,10 +18,28 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_ARMING_STATUS=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y +CONFIG_UAVCANNODE_ESC_STATUS=y +CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y +CONFIG_UAVCANNODE_RAW_AIR_DATA=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_SENSORS=y +CONFIG_NSH_BUILTIN_APPS=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults index ba6faf3d0a..ac0793ec70 100644 --- a/boards/ark/cannode/init/rc.board_defaults +++ b/boards/ark/cannode/init/rc.board_defaults @@ -2,6 +2,11 @@ # # board specific defaults #------------------------------------------------------------------------------ + +param set-default CANNODE_ARM_STAT 1 +param set-default CANNODE_SUB_ESC 1 +param set-default CANNODE_SUB_SERV 1 + pwm_out start -control_allocator start +dshot start diff --git a/boards/cuav/can-gps-v1/default.px4board b/boards/cuav/can-gps-v1/default.px4board index 3a838e1cbe..6b0a5675f5 100644 --- a/boards/cuav/can-gps-v1/default.px4board +++ b/boards/cuav/can-gps-v1/default.px4board @@ -14,6 +14,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set diff --git a/boards/freefly/can-rtk-gps/default.px4board b/boards/freefly/can-rtk-gps/default.px4board index 85b1558597..c2759ba300 100644 --- a/boards/freefly/can-rtk-gps/default.px4board +++ b/boards/freefly/can-rtk-gps/default.px4board @@ -11,6 +11,14 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/holybro/can-gps-v1/default.px4board b/boards/holybro/can-gps-v1/default.px4board index 60ae33341f..41b3ceeafd 100644 --- a/boards/holybro/can-gps-v1/default.px4board +++ b/boards/holybro/can-gps-v1/default.px4board @@ -13,6 +13,13 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/matek/gnss-m9n-f4/default.px4board b/boards/matek/gnss-m9n-f4/default.px4board index 230f1e85a5..13ecd3e14c 100644 --- a/boards/matek/gnss-m9n-f4/default.px4board +++ b/boards/matek/gnss-m9n-f4/default.px4board @@ -12,6 +12,14 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SAFETY_BUTTON=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index 9b747e705e..eb28094dd9 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -13,6 +13,21 @@ CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_ARMING_STATUS=y +CONFIG_UAVCANNODE_BEEP_COMMAND=y +CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y +CONFIG_UAVCANNODE_ESC_STATUS=y +CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y +CONFIG_UAVCANNODE_GNSS_FIX=y +CONFIG_UAVCANNODE_HYGROMETER_MEASUREMENT=y +CONFIG_UAVCANNODE_LIGHTS_COMMAND=y +CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y +CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y +CONFIG_UAVCANNODE_RAW_AIR_DATA=y +CONFIG_UAVCANNODE_RTK_DATA=y +CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y +CONFIG_UAVCANNODE_STATIC_PRESSURE=y +CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 10dca7b64b..37993c97ce 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -142,6 +142,8 @@ px4_add_module( ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include SRCS + arming_status.cpp + arming_status.hpp beep.cpp beep.hpp rgbled.cpp diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index e808c5d01e..2031b31877 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -50,7 +50,7 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_pub_raw_cmd(node), _uavcan_sub_status(node) { - _uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY); + _uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority } int diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 4ff9c02ed8..516b147214 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -91,8 +91,6 @@ private: */ uint8_t check_escs_status(); - static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest - typedef uavcan::MethodBinder&)> StatusCbBinder; diff --git a/src/drivers/uavcan/arming_status.cpp b/src/drivers/uavcan/arming_status.cpp new file mode 100644 index 0000000000..3d4b720c98 --- /dev/null +++ b/src/drivers/uavcan/arming_status.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 arming_status.cpp + * + * @author Alex Klimaj + */ + +#include "arming_status.hpp" + +UavcanArmingStatus::UavcanArmingStatus(uavcan::INode &node) : + _arming_status_pub(node), + _timer(node) +{ + _arming_status_pub.setPriority(uavcan::TransferPriority::Default); +} + +int UavcanArmingStatus::init() +{ + /* + * Setup timer and call back function for periodic updates + */ + if (!_timer.isRunning()) { + _timer.setCallback(TimerCbBinder(this, &UavcanArmingStatus::periodic_update)); + _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); + } + + return 0; +} + +void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &) +{ + actuator_armed_s actuator_armed; + + if (_actuator_armed_sub.update(&actuator_armed)) { + uavcan::equipment::safety::ArmingStatus cmd; + + if (actuator_armed.lockdown || actuator_armed.manual_lockdown) { + cmd.status = cmd.STATUS_DISARMED; + + } else if (actuator_armed.armed || actuator_armed.prearmed) { + cmd.status = cmd.STATUS_FULLY_ARMED; + + } else { + cmd.status = cmd.STATUS_DISARMED; + } + + (void)_arming_status_pub.broadcast(cmd); + } +} diff --git a/src/drivers/uavcan/arming_status.hpp b/src/drivers/uavcan/arming_status.hpp new file mode 100644 index 0000000000..3c7bdd041b --- /dev/null +++ b/src/drivers/uavcan/arming_status.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** +* +* Copyright (c) 2022 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 arming_status.hpp + * + * @author Alex Klimaj + * + * @brief Publish actuator_armed to CAN ArmingStatus message + */ + +#pragma once + +#include +#include +#include +#include + + +class UavcanArmingStatus +{ +public: + UavcanArmingStatus(uavcan::INode &node); + + /* + * setup periodic updater + */ + int init(); + +private: + /* + * Max update rate to avoid exessive bus traffic + */ + static constexpr unsigned MAX_RATE_HZ = 10; + + /* + * Setup timer and call back function for periodic updates + */ + void periodic_update(const uavcan::TimerEvent &); + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * Publish CAN ArmingState + */ + uavcan::Publisher _arming_status_pub; + + uavcan::TimerEventForwarder _timer; + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + +}; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 6fcfd4faf9..d5e915d7e8 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -80,6 +80,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), ModuleParams(nullptr), _node(can_driver, system_clock, _pool_allocator), + _arming_status_controller(_node), _beep_controller(_node), _esc_controller(_node), _servo_controller(_node), @@ -478,7 +479,21 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) fill_node_info(); - int ret = _beep_controller.init(); + int ret; + + // UAVCAN_PUB_ARM + int32_t uavcan_pub_arm = 0; + param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm); + + if (uavcan_pub_arm == 1) { + ret = _arming_status_controller.init(); + + if (ret < 0) { + return ret; + } + } + + ret = _beep_controller.init(); if (ret < 0) { return ret; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index f896fc742e..7fcc86e968 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -50,6 +50,7 @@ #include "actuators/hardpoint.hpp" #include "actuators/servo.hpp" #include "allocator.hpp" +#include "arming_status.hpp" #include "beep.hpp" #include "logmessage.hpp" #include "rgbled.hpp" @@ -224,6 +225,7 @@ private: uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; + UavcanArmingStatus _arming_status_controller; UavcanBeepController _beep_controller; UavcanEscController _esc_controller; UavcanServoController _servo_controller; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index ce6b34480c..21672f7c1e 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -185,6 +185,18 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3); */ PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0); +/** + * publish Arming Status stream + * + * Enable UAVCAN Arming Status stream publication + * uavcan::equipment::safety::ArmingStatus + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_ARM, 0); + /** * publish RTCM stream * diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index eff5c446a3..fdea87a4b0 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/src/drivers/uavcannode/Kconfig @@ -2,4 +2,75 @@ menuconfig DRIVERS_UAVCANNODE bool "uavcannode" default n ---help--- - Enable support for uavcannode \ No newline at end of file + Enable support for uavcannode + +if DRIVERS_UAVCANNODE + config UAVCANNODE_ARMING_STATUS + bool "Include arming status" + default n + + config UAVCANNODE_BATTERY_INFO + bool "Include battery info" + default n + + config UAVCANNODE_BEEP_COMMAND + bool "Include beep command" + default n + + config UAVCANNODE_ESC_RAW_COMMAND + bool "Include ESC raw command" + default n + + config UAVCANNODE_ESC_STATUS + bool "Include ESC status" + default n + + config UAVCANNODE_FLOW_MEASUREMENT + bool "Include flow measurement" + default n + + config UAVCANNODE_GNSS_FIX + bool "Include GNSS fix" + default n + + config UAVCANNODE_HYDROMETER_MEASUREMENT + bool "Include hydrometer measurement" + default n + + config UAVCANNODE_LIGHTS_COMMAND + bool "Include lights command" + default n + + config UAVCANNODE_MAGNETIC_FIELD_STRENGTH + bool "Include magnetic field strength" + default n + + config UAVCANNODE_RANGE_SENSOR_MEASUREMENT + bool "Include range sensor measurement" + default n + + config UAVCANNODE_RAW_AIR_DATA + bool "Include raw air data" + default n + + config UAVCANNODE_RTK_DATA + bool "Include RTK data" + default n + + config UAVCANNODE_SAFETY_BUTTON + bool "Include safety button" + default n + + config UAVCANNODE_SERVO_ARRAY_COMMAND + bool "Include servo array command" + default n + + config UAVCANNODE_STATIC_PRESSURE + bool "Include static pressure" + default n + + config UAVCANNODE_STATIC_TEMPERATURE + bool "Include static temperature" + default n + +endif #DRIVERS_UAVCANNODE diff --git a/src/drivers/uavcannode/Publishers/ESCStatus.hpp b/src/drivers/uavcannode/Publishers/ESCStatus.hpp new file mode 100644 index 0000000000..fb0cef2f9d --- /dev/null +++ b/src/drivers/uavcannode/Publishers/ESCStatus.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class ESCStatus : + public UavcanPublisherBase, + private uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + ESCStatus(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::esc::Status::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(esc_status)), + uavcan::Publisher(node) + { + this->setPriority(uavcan::TransferPriority::MiddleLower); + } + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::esc::Status::getDataTypeFullName(), + uavcan::equipment::esc::Status::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // esc_status -> uavcan::equipment::esc::Status + esc_status_s esc_status; + + if (uORB::SubscriptionCallbackWorkItem::update(&esc_status)) { + + for (size_t i = 0; i < esc_status.esc_count; i++) { + uavcan::equipment::esc::Status status{}; + + status.esc_index = i; + status.error_count = esc_status.esc[i].esc_errorcount; + status.voltage = esc_status.esc[i].esc_voltage; + status.current = esc_status.esc[i].esc_current; + status.temperature = esc_status.esc[i].esc_temperature; + status.rpm = esc_status.esc[i].esc_rpm; + //status.power_rating_pct = NAN; + + uavcan::Publisher::broadcast(status); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } + } +}; +} // namespace uavcan diff --git a/src/drivers/uavcannode/Subscribers/ArmingStatus.hpp b/src/drivers/uavcannode/Subscribers/ArmingStatus.hpp new file mode 100644 index 0000000000..b3b2d4b538 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/ArmingStatus.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class ArmingStatus; + +typedef uavcan::MethodBinder&)> + ArmingStatusBinder; + +class ArmingStatus : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + ArmingStatus(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::safety::ArmingStatus::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(ArmingStatusBinder(this, &ArmingStatus::callback)) < 0) { + PX4_ERR("uavcan::equipment::safety::ArmingStatus subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::safety::ArmingStatus::getDataTypeFullName(), + uavcan::equipment::safety::ArmingStatus::DefaultDataTypeID, + _actuator_armed_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + actuator_armed_s actuator_armed{}; + + if (msg.status) { + actuator_armed.armed = true; + + } else { + actuator_armed.armed = false; + } + + actuator_armed.timestamp = hrt_absolute_time(); + _actuator_armed_pub.publish(actuator_armed); + } + + uORB::Publication _actuator_armed_pub{ORB_ID(actuator_armed)}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp b/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp new file mode 100644 index 0000000000..83604fd161 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/ESCRawCommand.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class ESCRawCommand; + +typedef uavcan::MethodBinder&)> + ESCRawCommandBinder; + +class ESCRawCommand : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + ESCRawCommand(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::esc::RawCommand::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(ESCRawCommandBinder(this, &ESCRawCommand::callback)) < 0) { + PX4_ERR("uavcan::equipment::esc::RawCommand subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::esc::RawCommand::getDataTypeFullName(), + uavcan::equipment::esc::RawCommand::DefaultDataTypeID, + _actuator_motors_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + + actuator_motors_s actuator_motors; + + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = actuator_motors.timestamp; + + for (unsigned i = 0; i < msg.cmd.size(); i++) { + if (i >= actuator_motors_s::NUM_CONTROLS) { + break; + } + + // Normalized to -8192, 8191 in uavcan. actuator_motors is -1 to 1 + actuator_motors.control[i] = msg.cmd[i] / 8192.f; + + } + + _actuator_motors_pub.publish(actuator_motors); + + } + + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Subscribers/ServoArrayCommand.hpp b/src/drivers/uavcannode/Subscribers/ServoArrayCommand.hpp new file mode 100644 index 0000000000..a92693bb40 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/ServoArrayCommand.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class ServoArrayCommand; + +typedef uavcan::MethodBinder&)> + ServoArrayCommandBinder; + +class ServoArrayCommand : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + ServoArrayCommand(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::actuator::ArrayCommand::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(ServoArrayCommandBinder(this, &ServoArrayCommand::callback)) < 0) { + PX4_ERR("uavcan::equipment::actuator::ArrayCommand subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::actuator::ArrayCommand::getDataTypeFullName(), + uavcan::equipment::actuator::ArrayCommand::DefaultDataTypeID, + _actuator_servos_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + + actuator_servos_s actuator_servos; + + actuator_servos.timestamp = hrt_absolute_time(); + actuator_servos.timestamp_sample = actuator_servos.timestamp; + + for (unsigned i = 0; i < msg.commands.size(); i++) { + if (i >= actuator_servos_s::NUM_CONTROLS) { + break; + } + + if (msg.commands[i].command_type == uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS) { + actuator_servos.control[i] = msg.commands[i].command_value; // -1.0 to +1.0 + + } else { + actuator_servos.control[i] = NAN; // disarmed + } + } + + _actuator_servos_pub.publish(actuator_servos); + + } + + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index b88c2b0565..2fcce1be6e 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -40,23 +40,77 @@ #include #include +#if defined(CONFIG_UAVCANNODE_BATTERY_INFO) #include "Publishers/BatteryInfo.hpp" -#include "Publishers/FlowMeasurement.hpp" -#include "Publishers/HygrometerMeasurement.hpp" -#include "Publishers/GnssFix2.hpp" -#include "Publishers/MagneticFieldStrength2.hpp" -#include "Publishers/MovingBaselineData.hpp" -#include "Publishers/RangeSensorMeasurement.hpp" -#include "Publishers/RawAirData.hpp" -#include "Publishers/RelPosHeading.hpp" -#include "Publishers/SafetyButton.hpp" -#include "Publishers/StaticPressure.hpp" -#include "Publishers/StaticTemperature.hpp" +#endif // CONFIG_UAVCANNODE_BATTERY_INFO +#if defined(CONFIG_UAVCANNODE_ESC_STATUS) +#include "Publishers/ESCStatus.hpp" +#endif // CONFIG_UAVCANNODE_ESC_STATUS + +#if defined(CONFIG_UAVCANNODE_FLOW_MEASUREMENT) +#include "Publishers/FlowMeasurement.hpp" +#endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT) +#include "Publishers/HygrometerMeasurement.hpp" +#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_GNSS_FIX) +#include "Publishers/GnssFix2.hpp" +#endif // CONFIG_UAVCANNODE_GNSS_FIX + +#if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH) +#include "Publishers/MagneticFieldStrength2.hpp" +#endif // CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH + +#if defined(CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT) +#include "Publishers/RangeSensorMeasurement.hpp" +#endif // CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_RAW_AIR_DATA) +#include "Publishers/RawAirData.hpp" +#endif // CONFIG_UAVCANNODE_RAW_AIR_DATA + +#if defined(CONFIG_UAVCANNODE_SAFETY_BUTTON) +#include "Publishers/SafetyButton.hpp" +#endif // CONFIG_UAVCANNODE_SAFETY_BUTTON + +#if defined(CONFIG_UAVCANNODE_STATIC_PRESSURE) +#include "Publishers/StaticPressure.hpp" +#endif // CONFIG_UAVCANNODE_STATIC_PRESSURE + +#if defined(CONFIG_UAVCANNODE_STATIC_TEMPERATURE) +#include "Publishers/StaticTemperature.hpp" +#endif // CONFIG_UAVCANNODE_STATIC_TEMPERATURE + +#if defined(CONFIG_UAVCANNODE_ARMING_STATUS) +#include "Subscribers/ArmingStatus.hpp" +#endif // CONFIG_UAVCANNODE_ARMING_STATUS + +#if defined(CONFIG_UAVCANNODE_BEEP_COMMAND) #include "Subscribers/BeepCommand.hpp" +#endif // CONFIG_UAVCANNODE_BEEP_COMMAND + +#if defined(CONFIG_UAVCANNODE_ESC_RAW_COMMAND) +#include "Subscribers/ESCRawCommand.hpp" +#endif // CONFIG_UAVCANNODE_ESC_RAW_COMMAND + +#if defined(CONFIG_UAVCANNODE_LIGHTS_COMMAND) #include "Subscribers/LightsCommand.hpp" +#endif // CONFIG_UAVCANNODE_LIGHTS_COMMAND + +#if defined(CONFIG_UAVCANNODE_RTK_DATA) +#include "Publishers/RelPosHeading.hpp" +#include "Publishers/MovingBaselineData.hpp" + #include "Subscribers/MovingBaselineData.hpp" #include "Subscribers/RTCMStream.hpp" +#endif // CONFIG_UAVCANNODE_RTK_DATA + +#if defined(CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND) +#include "Subscribers/ServoArrayCommand.hpp" +#endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND using namespace time_literals; @@ -294,14 +348,39 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events return PX4_ERROR; } - // TODO: make runtime (and build time?) configurable +#if defined(CONFIG_UAVCANNODE_BATTERY_INFO) _publisher_list.add(new BatteryInfo(this, _node)); +#endif // CONFIG_UAVCANNODE_BATTERY_INFO + +#if defined(CONFIG_UAVCANNODE_ESC_STATUS) + _publisher_list.add(new ESCStatus(this, _node)); +#endif // CONFIG_UAVCANNODE_ESC_STATUS + +#if defined(CONFIG_UAVCANNODE_FLOW_MEASUREMENT) _publisher_list.add(new FlowMeasurement(this, _node)); +#endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT) _publisher_list.add(new HygrometerMeasurement(this, _node)); +#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_GNSS_FIX) _publisher_list.add(new GnssFix2(this, _node)); +#endif // CONFIG_UAVCANNODE_GNSS_FIX + +#if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH) _publisher_list.add(new MagneticFieldStrength2(this, _node)); +#endif // CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH + +#if defined(CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT) _publisher_list.add(new RangeSensorMeasurement(this, _node)); +#endif // CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT + +#if defined(CONFIG_UAVCANNODE_RAW_AIR_DATA) _publisher_list.add(new RawAirData(this, _node)); +#endif // CONFIG_UAVCANNODE_RAW_AIR_DATA + +#if defined(CONFIG_UAVCANNODE_RTK_DATA) _publisher_list.add(new RelPosHeadingPub(this, _node)); int32_t cannode_pub_mbd = 0; @@ -311,13 +390,37 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _publisher_list.add(new MovingBaselineDataPub(this, _node)); } +#endif // CONFIG_UAVCANNODE_RTK_DATA + +#if defined(CONFIG_UAVCANNODE_SAFETY_BUTTON) _publisher_list.add(new SafetyButton(this, _node)); +#endif // CONFIG_UAVCANNODE_SAFETY_BUTTON + +#if defined(CONFIG_UAVCANNODE_STATIC_PRESSURE) _publisher_list.add(new StaticPressure(this, _node)); +#endif // CONFIG_UAVCANNODE_STATIC_PRESSURE + +#if defined(CONFIG_UAVCANNODE_STATIC_TEMPERATURE) _publisher_list.add(new StaticTemperature(this, _node)); +#endif // CONFIG_UAVCANNODE_STATIC_TEMPERATURE +#if defined(CONFIG_UAVCANNODE_ARMING_STATUS) + _subscriber_list.add(new ArmingStatus(_node)); +#endif // CONFIG_UAVCANNODE_ARMING_STATUS + +#if defined(CONFIG_UAVCANNODE_BEEP_COMMAND) _subscriber_list.add(new BeepCommand(_node)); - _subscriber_list.add(new LightsCommand(_node)); +#endif // CONFIG_UAVCANNODE_BEEP_COMMAND +#if defined(CONFIG_UAVCANNODE_ESC_RAW_COMMAND) + _subscriber_list.add(new ESCRawCommand(_node)); +#endif // CONFIG_UAVCANNODE_ESC_RAW_COMMAND + +#if defined(CONFIG_UAVCANNODE_LIGHTS_COMMAND) + _subscriber_list.add(new LightsCommand(_node)); +#endif // CONFIG_UAVCANNODE_LIGHTS_COMMAND + +#if defined(CONFIG_UAVCANNODE_RTK_DATA) int32_t cannode_sub_mbd = 0; param_get(param_find("CANNODE_SUB_MBD"), &cannode_sub_mbd); @@ -332,6 +435,18 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _subscriber_list.add(new RTCMStream(_node)); } +#endif // CONFIG_UAVCANNODE_RTK_DATA + +#if defined(CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND) + int32_t enable_servo_array_command_sub = 0; + param_get(param_find("CANNODE_SUB_SERV"), &enable_servo_array_command_sub); + + if (enable_servo_array_command_sub != 0) { + _subscriber_list.add(new ServoArrayCommand(_node)); + } + +#endif // CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND + for (auto &subscriber : _subscriber_list) { subscriber->init(); }