mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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 <julian@oes.ch>
This commit is contained in:
Julian Oes
parent
4c63e9e4f9
commit
87a63e75be
@ -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)
|
||||
|
||||
41
src/lib/open_drone_id/CMakeLists.txt
Normal file
41
src/lib/open_drone_id/CMakeLists.txt
Normal file
@ -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)
|
||||
255
src/lib/open_drone_id/open_drone_id_translations.cpp
Normal file
255
src/lib/open_drone_id/open_drone_id_translations.cpp
Normal file
@ -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 <uORB/topics/vehicle_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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
|
||||
52
src/lib/open_drone_id/open_drone_id_translations.hpp
Normal file
52
src/lib/open_drone_id/open_drone_id_translations.hpp
Normal file
@ -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 <stdint.h>
|
||||
#include <mavlink/common/mavlink.h>
|
||||
|
||||
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
|
||||
@ -135,6 +135,7 @@ px4_add_module(
|
||||
sensor_calibration
|
||||
geo
|
||||
mavlink_c
|
||||
open_drone_id
|
||||
timesync
|
||||
tunes
|
||||
variable_length_ringbuffer
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
#define OPEN_DRONE_ID_BASIC_ID_HPP
|
||||
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <lib/open_drone_id/open_drone_id_translations.hpp>
|
||||
|
||||
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
|
||||
|
||||
@ -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<float>(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;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user