fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 07:49:10 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/commander/Commander.cpp
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

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

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

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

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

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

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