From 6f4a219399ad972c93a08a00397600d9073b0fb6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 11 Jul 2024 14:36:54 +1200 Subject: [PATCH] uavcan: add OpenDroneID ArmStatus, operator ID In order to have operator ID be sent by QGC, we need to forward ArmStatus from the remote ID module (here on DroneCAN) to MAVLink. --- msg/CMakeLists.txt | 2 + msg/OpenDroneIdArmStatus.msg | 3 + msg/OpenDroneIdOperatorId.msg | 6 ++ src/drivers/uavcan/remoteid.cpp | 47 +++++++- src/drivers/uavcan/remoteid.hpp | 18 +++- src/modules/mavlink/mavlink_main.cpp | 4 + src/modules/mavlink/mavlink_messages.cpp | 4 + src/modules/mavlink/mavlink_receiver.cpp | 23 ++++ src/modules/mavlink/mavlink_receiver.h | 7 ++ .../streams/OPEN_DRONE_ID_ARM_STATUS.hpp | 100 ++++++++++++++++++ 10 files changed, 212 insertions(+), 2 deletions(-) create mode 100644 msg/OpenDroneIdArmStatus.msg create mode 100644 msg/OpenDroneIdOperatorId.msg create mode 100644 src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 65ec54d8b01e..92f5358c678a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -152,6 +152,8 @@ set(msg_files ObstacleDistance.msg OffboardControlMode.msg OnboardComputerStatus.msg + OpenDroneIdArmStatus.msg + OpenDroneIdOperatorId.msg OrbitStatus.msg OrbTest.msg OrbTestLarge.msg diff --git a/msg/OpenDroneIdArmStatus.msg b/msg/OpenDroneIdArmStatus.msg new file mode 100644 index 000000000000..1fa58c6a1f26 --- /dev/null +++ b/msg/OpenDroneIdArmStatus.msg @@ -0,0 +1,3 @@ +uint64 timestamp +uint8 status +char[50] error diff --git a/msg/OpenDroneIdOperatorId.msg b/msg/OpenDroneIdOperatorId.msg new file mode 100644 index 000000000000..d75a419717f5 --- /dev/null +++ b/msg/OpenDroneIdOperatorId.msg @@ -0,0 +1,6 @@ +uint64 timestamp +uint8 target_system +uint8 target_component +uint8[20] id_or_mac +uint8 operator_id_type +char[20] operator_id diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index 6e8bdcebeac8..7e93000a0d13 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -44,7 +44,9 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) : _node(node), _uavcan_pub_remoteid_basicid(node), _uavcan_pub_remoteid_location(node), - _uavcan_pub_remoteid_system(node) + _uavcan_pub_remoteid_system(node), + _uavcan_pub_operator_id(node), + _uavcan_sub_arm_status(node) { } @@ -53,6 +55,14 @@ int UavcanRemoteIDController::init() // Setup timer and call back function for periodic updates _timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update)); _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); + + int res = _uavcan_sub_arm_status.start(ArmStatusBinder(this, &UavcanRemoteIDController::arm_status_sub_cb)); + + if (res < 0) { + PX4_WARN("ArmStatus sub failed %i", res); + return res; + } + return 0; } @@ -63,6 +73,7 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &) send_basic_id(); send_location(); send_system(); + send_operator_id(); } void UavcanRemoteIDController::send_basic_id() @@ -254,3 +265,37 @@ void UavcanRemoteIDController::send_system() } } } + +void UavcanRemoteIDController::send_operator_id() +{ + open_drone_id_operator_id_s operator_id; + + if (_open_drone_id_operator_id.copy(&operator_id)) { + + dronecan::remoteid::OperatorID msg {}; + + for (unsigned i = 0; i < sizeof(operator_id.id_or_mac); ++i) { + msg.id_or_mac.push_back(operator_id.id_or_mac[i]); + } + + msg.operator_id_type = operator_id.operator_id_type; + + for (unsigned i = 0; i < sizeof(operator_id.operator_id); ++i) { + msg.operator_id.push_back(operator_id.operator_id[i]); + } + + _uavcan_pub_operator_id.broadcast(msg); + } +} + +void +UavcanRemoteIDController::arm_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + open_drone_id_arm_status_s arm_status{}; + + arm_status.timestamp = hrt_absolute_time(); + arm_status.status = msg.status; + memcpy(arm_status.error, msg.error.c_str(), sizeof(arm_status.error)); + + _open_drone_id_arm_status_pub.publish(arm_status); +} diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index 3d35e5095997..1ad0d9482985 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -33,18 +33,23 @@ #pragma once +#include +#include #include #include #include #include #include #include -#include +#include +#include #include #include #include #include +#include +#include #include @@ -68,6 +73,9 @@ class UavcanRemoteIDController : public ModuleParams void send_basic_id(); void send_location(); void send_system(); + void send_operator_id(); + + void arm_status_sub_cb(const uavcan::ReceivedDataStructure &msg); uavcan::INode &_node; @@ -77,8 +85,16 @@ class UavcanRemoteIDController : public ModuleParams uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)}; + uORB::Publication _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)}; uavcan::Publisher _uavcan_pub_remoteid_basicid; uavcan::Publisher _uavcan_pub_remoteid_location; uavcan::Publisher _uavcan_pub_remoteid_system; + uavcan::Publisher _uavcan_pub_operator_id; + + using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *, + void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure &) >; + + uavcan::Subscriber _uavcan_sub_arm_status; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 54e1a8549eeb..e0a4a9c2baa6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1429,6 +1429,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("OBSTACLE_DISTANCE", 1.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f); configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f); @@ -1498,6 +1499,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1666,6 +1668,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1758,6 +1761,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPEN_DRONE_ID_SYSTEM", 1.f); + configure_stream_local("OPEN_DRONE_ID_ARM_STATUS", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 0.1f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 638de9cce5f4..60f021a5b126 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -97,6 +97,7 @@ #include "streams/OPEN_DRONE_ID_BASIC_ID.hpp" #include "streams/OPEN_DRONE_ID_LOCATION.hpp" #include "streams/OPEN_DRONE_ID_SYSTEM.hpp" +#include "streams/OPEN_DRONE_ID_ARM_STATUS.hpp" #include "streams/OPTICAL_FLOW_RAD.hpp" #include "streams/ORBIT_EXECUTION_STATUS.hpp" #include "streams/PING.hpp" @@ -457,6 +458,9 @@ static const StreamListItem streams_list[] = { #if defined(OPEN_DRONE_ID_SYSTEM_HPP) create_stream_list_item(), #endif // OPEN_DRONE_ID_SYSTEM_HPP +#if defined(OPEN_DRONE_ID_ARM_STATUS_HPP) + create_stream_list_item(), +#endif // OPEN_DRONE_ID_ARM_STATUS_HPP #if defined(ESC_INFO_HPP) create_stream_list_item(), #endif // ESC_INFO_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..38923169dc4c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -280,6 +280,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_statustext(msg); break; + case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: + handle_message_open_drone_id_operator_id(msg); + break; + #if !defined(CONSTRAINED_FLASH) case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: @@ -3048,6 +3052,25 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } +void MavlinkReceiver::handle_message_open_drone_id_operator_id( + mavlink_message_t *msg) +{ + mavlink_open_drone_id_operator_id_t odid_module; + mavlink_msg_open_drone_id_operator_id_decode(msg, &odid_module); + + open_drone_id_operator_id_s odid_operator_id{}; + memset(&odid_operator_id, 0, sizeof(odid_operator_id)); + + odid_operator_id.timestamp = hrt_absolute_time(); + odid_operator_id.target_system = odid_module.target_system; + odid_operator_id.target_component = odid_module.target_component; + memcpy(odid_operator_id.id_or_mac, odid_module.id_or_mac, sizeof(odid_operator_id.id_or_mac)); + odid_operator_id.operator_id_type = odid_module.operator_id_type; + memcpy(odid_operator_id.operator_id, odid_module.operator_id, sizeof(odid_operator_id.operator_id)); + + _open_drone_id_operator_id_pub.publish(odid_operator_id); +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9f2882e84835..89d19f81a015 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -87,6 +87,7 @@ #include #include #include +#include #include #include #include @@ -181,6 +182,11 @@ class MavlinkReceiver : public ModuleParams void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_odometry(mavlink_message_t *msg); void handle_message_onboard_computer_status(mavlink_message_t *msg); + void handle_message_open_drone_id_arm_status(mavlink_message_t *msg); + void handle_message_open_drone_id_basic_id(mavlink_message_t *msg); + void handle_message_open_drone_id_operator_id(mavlink_message_t *msg); + void handle_message_open_drone_id_self_id(mavlink_message_t *msg); + void handle_message_open_drone_id_system(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -311,6 +317,7 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _velocity_limits_pub{ORB_ID(velocity_limits)}; + uORB::Publication _open_drone_id_operator_id_pub{ORB_ID(open_drone_id_operator_id)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp new file mode 100644 index 000000000000..b9886f071a91 --- /dev/null +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_ARM_STATUS.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#ifndef OPEN_DRONE_ID_ARM_STATUS_HPP +#define OPEN_DRONE_ID_ARM_STATUS_HPP + +#include + +class MavlinkStreamOpenDroneIdArmStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpenDroneIdArmStatus(mavlink); + } + + static constexpr const char *get_name_static() + { + return "OPEN_DRONE_ID_ARM_STATUS"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS; + } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _open_drone_id_arm_status_sub.advertised() + ? MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES + : 0; + } + +private: + explicit MavlinkStreamOpenDroneIdArmStatus(Mavlink *mavlink) + : MavlinkStream(mavlink) {} + + uORB::Subscription _open_drone_id_arm_status_sub{ORB_ID(open_drone_id_arm_status)}; + + bool send() override + { + open_drone_id_arm_status_s drone_id_arm; + + if (_open_drone_id_arm_status_sub.update(&drone_id_arm)) { + + mavlink_open_drone_id_arm_status_t msg{}; + + msg.status = drone_id_arm.status; + + for (uint8_t i = 0; i < sizeof(drone_id_arm.error); ++i) { + + msg.error[i] = drone_id_arm.error[i]; + + } + + mavlink_msg_open_drone_id_arm_status_send_struct(_mavlink->get_channel(), + &msg); + + return true; + } + + return false; + } +}; + +#endif // OPEN_DRONE_ID_ARM_STATUS_HPP +