From 43c529f294cbffd5d675d470de1ac423ed6f3ef7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Oct 2021 10:57:15 +0200 Subject: [PATCH] Add MAVLink parachute system heartbeat detection --- msg/telemetry_status.msg | 1 + src/modules/commander/Commander.cpp | 20 ++++++++++++++++++++ src/modules/commander/Commander.hpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 5 +++++ src/modules/mavlink/mavlink_receiver.h | 1 + 5 files changed, 29 insertions(+) diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index af767d4dcc..a3c202caf7 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -44,6 +44,7 @@ bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER 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 # Heartbeats per component bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ca216f6dd0..267583405b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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 diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6e62ad8938..3b8733d17f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -328,8 +328,10 @@ private: hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; + hrt_abstime _datalink_last_heartbeat_parachute_system{0}; bool _onboard_controller_lost{false}; bool _avoidance_system_lost{false}; + bool _parachute_system_lost{true}; hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33d2ad077e..a6108a5b82 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2150,6 +2150,10 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _camera_status_pub.publish(camera_status); break; + case MAV_TYPE_PARACHUTE: + _heartbeat_type_parachute = now; + break; + default: PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid, msg->compid); @@ -2950,6 +2954,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_type_gimbal = (t <= TIMEOUT + _heartbeat_type_gimbal); 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_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index af9bd267be..9a8390fefd 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -377,6 +377,7 @@ private: hrt_abstime _heartbeat_type_gimbal{0}; hrt_abstime _heartbeat_type_adsb{0}; hrt_abstime _heartbeat_type_camera{0}; + hrt_abstime _heartbeat_type_parachute{0}; hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0};