From 87a63e75be8d6c00a6787420d7e4a34df239203d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 9 May 2024 14:30:55 +1200 Subject: [PATCH] mavlink: extract OpenDroneID function to lib This extracts the function mapping from MAV_TYPE to MAV_ODID_UA_TYPE to the library, so that it can be re-used later by the remote ID implementation over DroneCAN. Signed-off-by: Julian Oes --- src/lib/CMakeLists.txt | 1 + src/lib/open_drone_id/CMakeLists.txt | 41 +++ .../open_drone_id_translations.cpp | 255 ++++++++++++++++++ .../open_drone_id_translations.hpp | 52 ++++ src/modules/mavlink/CMakeLists.txt | 1 + .../streams/OPEN_DRONE_ID_BASIC_ID.hpp | 95 +------ .../streams/OPEN_DRONE_ID_LOCATION.hpp | 64 +---- 7 files changed, 358 insertions(+), 151 deletions(-) create mode 100644 src/lib/open_drone_id/CMakeLists.txt create mode 100644 src/lib/open_drone_id/open_drone_id_translations.cpp create mode 100644 src/lib/open_drone_id/open_drone_id_translations.hpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7a7b5973fd..942c2dcad8 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -60,6 +60,7 @@ add_subdirectory(mathlib EXCLUDE_FROM_ALL) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) +add_subdirectory(open_drone_id EXCLUDE_FROM_ALL) add_subdirectory(perf EXCLUDE_FROM_ALL) add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) add_subdirectory(pid EXCLUDE_FROM_ALL) diff --git a/src/lib/open_drone_id/CMakeLists.txt b/src/lib/open_drone_id/CMakeLists.txt new file mode 100644 index 0000000000..2090d15925 --- /dev/null +++ b/src/lib/open_drone_id/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(open_drone_id + open_drone_id_translations.cpp + open_drone_id_translations.hpp +) + +add_dependencies(open_drone_id mavlink_c_generate) + +target_compile_options(open_drone_id PRIVATE -Wno-cast-align -Wno-address-of-packed-member) diff --git a/src/lib/open_drone_id/open_drone_id_translations.cpp b/src/lib/open_drone_id/open_drone_id_translations.cpp new file mode 100644 index 0000000000..854e762304 --- /dev/null +++ b/src/lib/open_drone_id/open_drone_id_translations.cpp @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "open_drone_id_translations.hpp" +#include +#include + +using namespace time_literals; + + +namespace open_drone_id_translations +{ + +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_TILTWING: 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; + } +} + +MAV_ODID_SPEED_ACC odidSpeedAccForVariance(float s_variance_m_s) +{ + // TODO: should this be stddev, so square root of variance? + // speed_accuracy + if (s_variance_m_s < 0.3f) { + return MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND; + + } else if (s_variance_m_s < 1.f) { + return MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND; + + } else if (s_variance_m_s < 3.f) { + return MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND; + + } else if (s_variance_m_s < 10.f) { + return MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND; + + } else { + return MAV_ODID_SPEED_ACC_UNKNOWN; + } +} + +MAV_ODID_HOR_ACC odidHorAccForEph(float eph) +{ + if (eph < 1.f) { + return MAV_ODID_HOR_ACC_1_METER; + + } else if (eph < 3.f) { + return MAV_ODID_HOR_ACC_3_METER; + + } else if (eph < 10.f) { + return MAV_ODID_HOR_ACC_10_METER; + + } else if (eph < 30.f) { + return MAV_ODID_HOR_ACC_30_METER; + + } else { + return MAV_ODID_HOR_ACC_UNKNOWN; + } +} + +MAV_ODID_VER_ACC odidVerAccForEpv(float epv) +{ + if (epv < 1.f) { + return MAV_ODID_VER_ACC_1_METER; + + } else if (epv < 3.f) { + return MAV_ODID_VER_ACC_3_METER; + + } else if (epv < 10.f) { + return MAV_ODID_VER_ACC_10_METER; + + } else if (epv < 25.f) { + return MAV_ODID_VER_ACC_25_METER; + + } else if (epv < 45.f) { + return MAV_ODID_VER_ACC_45_METER; + + } else if (epv < 150.f) { + return MAV_ODID_VER_ACC_150_METER; + + } else { + return MAV_ODID_VER_ACC_UNKNOWN; + } +} + + +MAV_ODID_TIME_ACC odidTimeForElapsed(uint64_t elapsed) +{ + if (elapsed < 100_ms) { + return MAV_ODID_TIME_ACC_0_1_SECOND; + + } else if (elapsed < 200_ms) { + return MAV_ODID_TIME_ACC_0_2_SECOND; + + } else if (elapsed < 300_ms) { + return MAV_ODID_TIME_ACC_0_3_SECOND; + + } else if (elapsed < 400_ms) { + return MAV_ODID_TIME_ACC_0_4_SECOND; + + } else if (elapsed < 500_ms) { + return MAV_ODID_TIME_ACC_0_5_SECOND; + + } else if (elapsed < 600_ms) { + return MAV_ODID_TIME_ACC_0_6_SECOND; + + } else if (elapsed < 700_ms) { + return MAV_ODID_TIME_ACC_0_7_SECOND; + + } else if (elapsed < 800_ms) { + return MAV_ODID_TIME_ACC_0_8_SECOND; + + } else if (elapsed < 900_ms) { + return MAV_ODID_TIME_ACC_0_9_SECOND; + + } else if (elapsed < 1000_ms) { + return MAV_ODID_TIME_ACC_1_0_SECOND; + + } else if (elapsed < 1100_ms) { + return MAV_ODID_TIME_ACC_1_1_SECOND; + + } else if (elapsed < 1200_ms) { + return MAV_ODID_TIME_ACC_1_2_SECOND; + + } else if (elapsed < 1300_ms) { + return MAV_ODID_TIME_ACC_1_3_SECOND; + + } else if (elapsed < 1400_ms) { + return MAV_ODID_TIME_ACC_1_4_SECOND; + + } else if (elapsed < 1500_ms) { + return MAV_ODID_TIME_ACC_1_5_SECOND; + + } else { + return MAV_ODID_TIME_ACC_UNKNOWN; + } +} + +} // open_drone_id_translations diff --git a/src/lib/open_drone_id/open_drone_id_translations.hpp b/src/lib/open_drone_id/open_drone_id_translations.hpp new file mode 100644 index 0000000000..4632ed1e14 --- /dev/null +++ b/src/lib/open_drone_id/open_drone_id_translations.hpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +namespace open_drone_id_translations +{ + +MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type); + +MAV_ODID_SPEED_ACC odidSpeedAccForVariance(float s_variance_m_s); + +MAV_ODID_HOR_ACC odidHorAccForEph(float eph); + +MAV_ODID_VER_ACC odidVerAccForEpv(float epv); + +MAV_ODID_TIME_ACC odidTimeForElapsed(uint64_t elapsed); + +} // open_drone_id_translations diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index f42dd6fa9b..4a29a6eb0b 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -135,6 +135,7 @@ px4_add_module( sensor_calibration geo mavlink_c + open_drone_id timesync tunes variable_length_ringbuffer diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp index d7869dcbb6..dcc77c660a 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -35,6 +35,7 @@ #define OPEN_DRONE_ID_BASIC_ID_HPP #include +#include class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream { @@ -57,98 +58,6 @@ private: 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_TILTWING: 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 @@ -167,7 +76,7 @@ private: msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER; // ua_type: MAV_ODID_UA_TYPE - msg.ua_type = odidTypeForMavType(vehicle_status.system_type); + msg.ua_type = open_drone_id_translations::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 diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index 3a519c5e90..1eba926a35 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -144,22 +144,7 @@ private: 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; - } + msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s); updated = true; } @@ -173,45 +158,9 @@ private: msg.altitude_geodetic = static_cast(round(vehicle_gps_position.altitude_msl_m)); // [m] } - // horizontal_accuracy - if (vehicle_gps_position.eph < 1.f) { - msg.horizontal_accuracy = MAV_ODID_HOR_ACC_1_METER; + msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph); - } 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; - } + msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv); updated = true; } @@ -221,9 +170,8 @@ private: 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 - } + msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time( + &vehicle_gps_position.timestamp)); updated = true; } @@ -236,7 +184,7 @@ private: 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 + msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration. updated = true; } }