fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 02:30:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+28
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
src/modules/commander/Commander.hpp
+2
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};