Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

Fixed some issues for datalink loss & regain (GCS HEARTBEAT).

Browse Source 
Major changes:
- Prevent  "HIGH LATENCY DATA LINK LOST" message to appear if iridium telemetry is not used.
- Prevent "DATA LINK LOSS" mavlink messages flooding when QGC is open and then closed.
- Changed "DATA LINK REGAINED" condition (use _datalink_last_heartbeat_gcs insthead of _datalink_lost)



Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-02-20 17:01:26 +01:00
committed by Beat Küng
parent d8bdc1b367
commit 1fc055a51f
3 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+9 -8
View File
@@ -3858,7 +3858,7 @@ void Commander::data_link_check(bool &status_changed)
case (telemetry_status_s::LINK_TYPE_IRIDIUM): case (telemetry_status_s::LINK_TYPE_IRIDIUM):
// lazily subscribe // lazily subscribe
if (orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) { if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
_iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status)); _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
} }
@@ -3892,10 +3892,12 @@ void Commander::data_link_check(bool &status_changed)
case (telemetry_status_s::MAV_TYPE_GCS): case (telemetry_status_s::MAV_TYPE_GCS):
_datalink_last_heartbeat_gcs = telemetry.heartbeat_time; _datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
// Recover from data link lost
if (status.data_link_lost) { if (status.data_link_lost) {
if (hrt_elapsed_time(&_datalink_lost) > (_datalink_regain_threshold.get() * 1_s)) { if (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) < (_datalink_regain_threshold.get() * 1_s)) {
status.data_link_lost = false; status.data_link_lost = false;
status_changed = true; status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "DATA LINK REGAINED");
} }
} }
@@ -3932,12 +3934,10 @@ void Commander::data_link_check(bool &status_changed)
} }
// GCS data link loss failsafe // GCS data link loss failsafe
if ((_datalink_last_heartbeat_gcs > 0) && if (!status.data_link_lost) {
(hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) { if (_datalink_last_heartbeat_gcs != 0
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s)) {
_datalink_lost = hrt_absolute_time();
if (!status.data_link_lost) {
status.data_link_lost = true; status.data_link_lost = true;
status.data_link_lost_counter++; status.data_link_lost_counter++;
@@ -3998,7 +3998,8 @@ void Commander::data_link_check(bool &status_changed)
// high latency data link loss failsafe // high latency data link loss failsafe
if (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) { if (_high_latency_datalink_heartbeat > 0
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) {
_high_latency_datalink_lost = hrt_absolute_time(); _high_latency_datalink_lost = hrt_absolute_time();
if (!status.high_latency_data_link_lost) { if (!status.high_latency_data_link_lost) {
src/modules/commander/Commander.hpp
-1
View File
@@ -175,7 +175,6 @@ private:
int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {}; int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
uint64_t _datalink_last_heartbeat_gcs{0}; uint64_t _datalink_last_heartbeat_gcs{0};
uint64_t _datalink_lost{0};
uint64_t _datalink_last_heartbeat_onboard_controller{0}; uint64_t _datalink_last_heartbeat_onboard_controller{0};
uint64_t _onboard_controller_lost{0}; uint64_t _onboard_controller_lost{0};
src/modules/commander/commander_params.c
+1 -1
View File
@@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
* @decimal 1 * @decimal 1
* @increment 0.5 * @increment 0.5
*/ */
PARAM_DEFINE_INT32(COM_DL_REG_T, 0); PARAM_DEFINE_INT32(COM_DL_REG_T, 2);
/** /**
* High Latency Datalink loss time threshold * High Latency Datalink loss time threshold