yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: telemetry status only log simple HEARTBEAT validity

Browse Source 
* delete telemetry_heartbeat msg
 * delete unused _telemetry_status_mutex
This commit is contained in:
Daniel Agar
2020-10-13 13:37:10 -04:00
committed by GitHub
parent d71ca37087
commit 8d1b99be31
10 changed files with 187 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3809,65 +3809,47 @@ void Commander::data_link_check()
}
}
for (const auto &hb : telemetry.heartbeats) {
// handle different remote types
switch (hb.type) {
case telemetry_heartbeat_s::TYPE_GCS:
if (telemetry.heartbeat_type_gcs) {
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
status.data_link_lost = false;
_status_changed = true;
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
_status_changed = true;
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
}
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
// Only keep the very last heartbeat timestamp, so we don't get confused
// by multiple mavlink instances publishing different timestamps.
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
_datalink_last_heartbeat_gcs = hb.timestamp;
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
break;
case telemetry_heartbeat_s::TYPE_ONBOARD_CONTROLLER:
if (_onboard_controller_lost) {
if (hb.timestamp > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
_status_changed = true;
}
}
_datalink_last_heartbeat_onboard_controller = hb.timestamp;
if (hb.component_id == telemetry_heartbeat_s::COMP_ID_OBSTACLE_AVOIDANCE) {
if (hb.timestamp > _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = (_datalink_last_status_avoidance_system != hb.state);
}
_datalink_last_heartbeat_avoidance_system = hb.timestamp;
_datalink_last_status_avoidance_system = hb.state;
if (_avoidance_system_lost) {
_status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
}
}
break;
}
_datalink_last_heartbeat_gcs = telemetry.timestamp;
}
if (telemetry.heartbeat_type_onboard_controller) {
if (_onboard_controller_lost) {
_onboard_controller_lost = false;
_status_changed = true;
if (_datalink_last_heartbeat_onboard_controller != 0) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
}
}
_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
}
if (telemetry.heartbeat_component_obstacle_avoidance) {
if (_avoidance_system_lost) {
_avoidance_system_lost = false;
_status_changed = true;
}
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
}
}
}
@@ -3899,40 +3881,15 @@ void Commander::data_link_check()
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
//if heartbeats stop
// if heartbeats stop
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
_avoidance_system_lost = true;
status_flags.avoidance_system_valid = false;
}
//if status changed
if (_avoidance_system_status_change) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_BOOT) {
status_flags.avoidance_system_valid = false;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_ACTIVE) {
status_flags.avoidance_system_valid = true;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_CRITICAL) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_FLIGHT_TERMINATION) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
_avoidance_system_status_change = false;
}
}
// high latency data link loss failsafe
if (_high_latency_datalink_heartbeat > 0
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {