fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 09:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add MAVLink parachute system heartbeat detection

Browse Source
This commit is contained in:
Matthias Grob
2021-10-27 10:57:15 +02:00
parent fb54324f56
commit 43c529f294
5 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+20
View File
@@ -3424,6 +3424,19 @@ void Commander::data_link_check()
_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
}
if (telemetry.heartbeat_type_parachute) {
if (_parachute_system_lost) {
_parachute_system_lost = false;
if (_datalink_last_heartbeat_parachute_system != 0) {
mavlink_log_info(&_mavlink_log_pub, "Parachute system regained\t");
events::send(events::ID("commander_parachute_regained"), events::Log::Info, "Parachute system regained");
}
}
_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
}
if (telemetry.heartbeat_component_obstacle_avoidance) {
if (_avoidance_system_lost) {
_avoidance_system_lost = false;
@@ -3464,6 +3477,13 @@ void Commander::data_link_check()
_status_changed = true;
}
// Parachute system
if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
&& !_parachute_system_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost");
_parachute_system_lost = true;
}
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (_status_flags.avoidance_system_required && !_onboard_controller_lost) {
// if heartbeats stop