fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: initial OPEN_DRONE_ID_BASIC_ID/OPEN_DRONE_ID_LOCATION support

Browse Source
This commit is contained in:
Thomas Debrunner 2022-08-09 21:02:51 +02:00 committed by Daniel Agar
parent b179427b4c
commit 0af87ec745
10 changed files with 518 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/telemetry_status.msg
View File

@ -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

5
msg/vehicle_status.msg
View File

@ -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

28
src/modules/commander/Commander.cpp
View File

@ -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

2
src/modules/commander/Commander.hpp
View File

@ -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};

12
src/modules/mavlink/mavlink_main.cpp
View File

@ -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);

10
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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<MavlinkStreamObstacleDistance>(),
#endif // OBSTACLE_DISTANCE_HPP
#if defined(OPEN_DRONE_ID_BASIC_ID_HPP)
create_stream_list_item<MavlinkStreamOpenDroneIdBasicId>(),
#endif // OPEN_DRONE_ID_BASIC_ID_HPP
#if defined(OPEN_DRONE_ID_LOCATION)
create_stream_list_item<MavlinkStreamOpenDroneIdLocation>(),
#endif // OPEN_DRONE_ID_LOCATION
#if defined(ESC_INFO_HPP)
create_stream_list_item<MavlinkStreamESCInfo>(),
#endif // ESC_INFO_HPP

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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);

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -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};

185
src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_status.h>
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

273
src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp Normal file
View File

@ -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 <uORB/topics/home_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
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