fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 01:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: changed "datalink regain" logic.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-03-07 14:45:34 +01:00
committed by Beat Küng
parent ca53217175
commit a53594135b
3 changed files with 8 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+6 -4
View File
@@ -3890,30 +3890,32 @@ void Commander::data_link_check(bool &status_changed)
// handle different remote types
switch (telemetry.remote_type) {
case telemetry_status_s::MAV_TYPE_GCS:
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
// Recover from data link lost
if (status.data_link_lost) {
if (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) < (_datalink_regain_threshold.get() * 1_s)) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
status_changed = true;
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
}
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
break;
case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
if (_onboard_controller_lost) {
if (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) < _onboard_regain_threshold.get() * 1_s) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
}
}
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;