From 0af87ec745fc2fcf5be162d0ef9d2bfdb66c5eb6 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Tue, 9 Aug 2022 21:02:51 +0200 Subject: [PATCH] mavlink: initial OPEN_DRONE_ID_BASIC_ID/OPEN_DRONE_ID_LOCATION support --- msg/telemetry_status.msg | 2 + msg/vehicle_status.msg | 5 +- src/modules/commander/Commander.cpp | 28 ++ src/modules/commander/Commander.hpp | 2 + src/modules/mavlink/mavlink_main.cpp | 12 +- src/modules/mavlink/mavlink_messages.cpp | 10 +- src/modules/mavlink/mavlink_receiver.cpp | 7 + src/modules/mavlink/mavlink_receiver.h | 1 + .../streams/OPEN_DRONE_ID_BASIC_ID.hpp | 185 ++++++++++++ .../streams/OPEN_DRONE_ID_LOCATION.hpp | 273 ++++++++++++++++++ 10 files changed, 518 insertions(+), 7 deletions(-) create mode 100644 src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp create mode 100644 src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 74a08efe91..48d85f934d 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -45,6 +45,7 @@ bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL bool heartbeat_type_adsb # MAV_TYPE_ADSB bool heartbeat_type_camera # MAV_TYPE_CAMERA bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE +bool heartbeat_type_open_drone_id # MAV_TYPE_ODID # Heartbeats per component bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO @@ -58,4 +59,5 @@ bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health bool avoidance_system_healthy +bool open_drone_id_system_healthy bool parachute_system_healthy diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c9507a73d1..d5f11afcb5 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -112,10 +112,11 @@ bool auto_mission_available bool power_input_valid # set if input power is valid bool usb_connected # set to true (never cleared) once telemetry received from usb link +bool open_drone_id_system_present +bool open_drone_id_system_healthy + bool parachute_system_present bool parachute_system_healthy bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter bool avoidance_system_valid # Status of the obstacle avoidance system - - diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6a4d1ed68..dc23100cf1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3022,6 +3022,23 @@ void Commander::data_link_check() _vehicle_status.parachute_system_healthy = healthy; } + if (telemetry.heartbeat_type_open_drone_id) { + if (_open_drone_id_system_lost) { + _open_drone_id_system_lost = false; + + if (_datalink_last_heartbeat_open_drone_id_system != 0) { + mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t"); + events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained"); + } + } + + bool healthy = telemetry.open_drone_id_system_healthy; + + _datalink_last_heartbeat_open_drone_id_system = telemetry.timestamp; + _vehicle_status.open_drone_id_system_present = true; + _vehicle_status.open_drone_id_system_healthy = healthy; + } + if (telemetry.heartbeat_component_obstacle_avoidance) { if (_avoidance_system_lost) { _avoidance_system_lost = false; @@ -3072,6 +3089,17 @@ void Commander::data_link_check() _status_changed = true; } + // OpenDroneID system + if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s) + && !_open_drone_id_system_lost) { + mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost"); + events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost"); + _vehicle_status.open_drone_id_system_present = false; + _vehicle_status.open_drone_id_system_healthy = false; + _open_drone_id_system_lost = true; + _status_changed = true; + } + // AVOIDANCE SYSTEM state check (only if it is enabled) if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { // if heartbeats stop diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 78fa26971c..e3c7e0bacb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -296,9 +296,11 @@ private: hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; + hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; bool _onboard_controller_lost{false}; bool _avoidance_system_lost{false}; bool _parachute_system_lost{true}; + bool _open_drone_id_system_lost{true}; hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index effcfbe5be..ed54d899ad 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1490,8 +1490,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ALTITUDE", 1.0f); configure_stream_local("ATTITUDE", 15.0f); - configure_stream_local("ATTITUDE_TARGET", 2.0f); configure_stream_local("ATTITUDE_QUATERNION", 10.0f); + configure_stream_local("ATTITUDE_TARGET", 2.0f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); @@ -1511,16 +1511,17 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 0.1f); - configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("LOCAL_POSITION_NED", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f); configure_stream_local("OBSTACLE_DISTANCE", 1.0f); + configure_stream_local("OPEN_DRONE_ID_LOCATION", 0.1f); configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f); configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); configure_stream_local("RAW_RPM", 2.0f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); @@ -1574,8 +1575,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_STATUS", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 1.0f); - configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1583,6 +1584,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); + configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); @@ -1723,10 +1725,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("HIGHRES_IMU", 50.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 1.0f); - configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("MAG_CAL_REPORT", 1.0f); configure_stream_local("MANUAL_CONTROL", 5.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f); configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); @@ -1736,6 +1738,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SCALED_IMU", 25.0f); configure_stream_local("SCALED_IMU2", 25.0f); configure_stream_local("SCALED_IMU3", 25.0f); + configure_stream_local("SCALED_PRESSURE", 1.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream_local("SYS_STATUS", 1.0f); @@ -1806,6 +1809,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", unlimited_rate); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream_local("OPEN_DRONE_ID_LOCATION", 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 a4d8fd7f85..9c0edd4a42 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -95,6 +95,8 @@ #include "streams/MOUNT_ORIENTATION.hpp" #include "streams/NAV_CONTROLLER_OUTPUT.hpp" #include "streams/OBSTACLE_DISTANCE.hpp" +#include "streams/OPEN_DRONE_ID_BASIC_ID.hpp" +#include "streams/OPEN_DRONE_ID_LOCATION.hpp" #include "streams/OPTICAL_FLOW_RAD.hpp" #include "streams/ORBIT_EXECUTION_STATUS.hpp" #include "streams/PING.hpp" @@ -517,6 +519,12 @@ static const StreamListItem streams_list[] = { #if defined(OBSTACLE_DISTANCE_HPP) create_stream_list_item(), #endif // OBSTACLE_DISTANCE_HPP +#if defined(OPEN_DRONE_ID_BASIC_ID_HPP) + create_stream_list_item(), +#endif // OPEN_DRONE_ID_BASIC_ID_HPP +#if defined(OPEN_DRONE_ID_LOCATION) + create_stream_list_item(), +#endif // OPEN_DRONE_ID_LOCATION #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 8335d87fc6..0f396421a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2206,6 +2206,12 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); break; + case MAV_TYPE_ODID: + _heartbeat_type_open_drone_id = now; + _mavlink->telemetry_status().open_drone_id_system_healthy = + (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); + break; + default: PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid, msg->compid); @@ -3013,6 +3019,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_type_adsb = (t <= TIMEOUT + _heartbeat_type_adsb); tstatus.heartbeat_type_camera = (t <= TIMEOUT + _heartbeat_type_camera); tstatus.heartbeat_type_parachute = (t <= TIMEOUT + _heartbeat_type_parachute); + tstatus.heartbeat_type_open_drone_id = (t <= TIMEOUT + _heartbeat_type_open_drone_id); tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ae652d0d56..ba43ff3fc0 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -386,6 +386,7 @@ private: hrt_abstime _heartbeat_type_adsb{0}; hrt_abstime _heartbeat_type_camera{0}; hrt_abstime _heartbeat_type_parachute{0}; + hrt_abstime _heartbeat_type_open_drone_id{0}; hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp new file mode 100644 index 0000000000..cfa3444a90 --- /dev/null +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef OPEN_DRONE_ID_BASIC_ID_HPP +#define OPEN_DRONE_ID_BASIC_ID_HPP + +#include + +class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdBasicId(mavlink); } + + static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_BASIC_ID"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _vehicle_status_sub.advertised() ? MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type) + { + switch (system_type) { + case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE; + + case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP; + + case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON; + + case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET; + + case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE; + + case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER; + + case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER; + + case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE; + + case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_RESERVED4: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT; + + case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_PARAFOIL: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; + + case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR; + + case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE; + + case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE; + + case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE; + + default: return MAV_ODID_UA_TYPE_OTHER; + } + } + + + bool send() override + { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + mavlink_open_drone_id_basic_id_t msg{}; + + msg.target_system = 0; // 0 for broadcast + msg.target_component = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + + // id_type: MAV_ODID_ID_TYPE + msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER; + + // ua_type: MAV_ODID_UA_TYPE + msg.ua_type = odidTypeForMavType(vehicle_status.system_type); + + // uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type + // TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format + board_get_px4_guid_formated((char *)(msg.uas_id), sizeof(msg.uas_id)); + + mavlink_msg_open_drone_id_basic_id_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // OPEN_DRONE_ID_BASIC_ID_HPP diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp new file mode 100644 index 0000000000..401ff852f9 --- /dev/null +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -0,0 +1,273 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef OPEN_DRONE_ID_LOCATION +#define OPEN_DRONE_ID_LOCATION + +#include +#include +#include +#include +#include +#include + +class MavlinkStreamOpenDroneIdLocation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdLocation(mavlink); } + + static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_LOCATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool send() override + { + mavlink_open_drone_id_location_t msg{}; + msg.target_component = 0; // 0 for broadcast + msg.target_system = 0; // 0 for broadcast + // msg.id_or_mac // Only used for drone ID data received from other UAs. + + // initialize all fields to unknown + msg.status = MAV_ODID_STATUS_UNDECLARED; + msg.direction = 36100; // If unknown: 36100 centi-degrees + msg.speed_horizontal = 25500; // If unknown: 25500 cm/s + msg.speed_vertical = 6300; // If unknown: 6300 cm/s + msg.latitude = 0; // If unknown: 0 + msg.longitude = 0; // If unknown: 0 + msg.altitude_geodetic = -1000; // If unknown: -1000 m + msg.altitude_geodetic = -1000; // If unknown: -1000 m + msg.height = -1000; // If unknown: -1000 m + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN; + msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN; + msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; + msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN; + msg.timestamp = 0xFFFF; // If unknown: 0xFFFF + msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN; + + bool updated = false; + + // status: MAV_ODID_STATUS_GROUND/MAV_ODID_STATUS_AIRBORNE + if (_vehicle_land_detected_sub.advertised()) { + vehicle_land_detected_s vehicle_land_detected{}; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected) + && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) { + if (vehicle_land_detected.landed) { + msg.status = MAV_ODID_STATUS_GROUND; + + } else { + msg.status = MAV_ODID_STATUS_AIRBORNE; + } + + updated = true; + } + } + + // status: MAV_ODID_STATUS_EMERGENCY + if (_vehicle_status_sub.advertised()) { + vehicle_status_s vehicle_status{}; + + if (_vehicle_status_sub.copy(&vehicle_status) && hrt_elapsed_time(&vehicle_status.timestamp) < 10_s) { + if (vehicle_status.failsafe && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + msg.status = MAV_ODID_STATUS_EMERGENCY; + updated = true; + } + } + } + + if (_vehicle_gps_position_sub.advertised()) { + sensor_gps_s vehicle_gps_position{}; + + if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) + && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) { + + if (vehicle_gps_position.vel_ned_valid) { + const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s}; + + // direction: calculate GPS course over ground angle + const float course = atan2f(vel_ned(1), vel_ned(0)); + const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course))); + msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees + + // speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s. + const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f; + msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425); + + // speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f); + msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200); + + // speed_accuracy + if (vehicle_gps_position.s_variance_m_s < 0.3f) { + msg.speed_accuracy = MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND; + + } else if (vehicle_gps_position.s_variance_m_s < 1.f) { + msg.speed_accuracy = MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND; + + } else if (vehicle_gps_position.s_variance_m_s < 3.f) { + msg.speed_accuracy = MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND; + + } else if (vehicle_gps_position.s_variance_m_s < 10.f) { + msg.speed_accuracy = MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND; + + } else { + msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN; + } + + updated = true; + } + + if (vehicle_gps_position.fix_type >= 2) { + msg.latitude = vehicle_gps_position.lat; + msg.longitude = vehicle_gps_position.lon; + + // altitude_geodetic + if (vehicle_gps_position.fix_type >= 3) { + msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f; + } + + // horizontal_accuracy + if (vehicle_gps_position.eph < 1.f) { + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER; + + } else if (vehicle_gps_position.eph < 3.f) { + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_3_METER; + + } else if (vehicle_gps_position.eph < 10.f) { + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_10_METER; + + } else if (vehicle_gps_position.eph < 30.f) { + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_30_METER; + + } else { + msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN; + } + + // vertical_accuracy + if (vehicle_gps_position.epv < 1.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_1_METER; + + } else if (vehicle_gps_position.epv < 3.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_3_METER; + + } else if (vehicle_gps_position.epv < 10.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_10_METER; + + } else if (vehicle_gps_position.epv < 25.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_25_METER; + + } else if (vehicle_gps_position.epv < 45.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_45_METER; + + } else if (vehicle_gps_position.epv < 150.f) { + msg.vertical_accuracy = MAV_ODID_VER_ACC_150_METER; + + } else { + msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN; + } + + updated = true; + } + + if (vehicle_gps_position.time_utc_usec != 0) { + // timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000 + uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000; + msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000; + + if (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s) { + msg.timestamp_accuracy = MAV_ODID_TIME_ACC_1_0_SECOND; // TODO + } + + updated = true; + } + } + } + + // altitude_barometric: The altitude calculated from the barometric pressue + if (_vehicle_air_data_sub.advertised()) { + vehicle_air_data_s vehicle_air_data{}; + + if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) { + msg.altitude_barometric = vehicle_air_data.baro_alt_meter; + msg.barometer_accuracy = MAV_ODID_VER_ACC_150_METER; // TODO + updated = true; + } + } + + // height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference + if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) { + home_position_s home_position{}; + vehicle_local_position_s vehicle_local_position{}; + + if (_home_position_sub.copy(&home_position) + && _vehicle_local_position_sub.copy(&vehicle_local_position) + && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s) + ) { + + if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) { + float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt); + + msg.height = altitude - home_position.alt; + msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF; + updated = true; + } + } + } + + if (updated) { + mavlink_msg_open_drone_id_location_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // OPEN_DRONE_ID_LOCATION