fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3858,7 +3858,7 @@ void Commander::data_link_check(bool &status_changed)
case (telemetry_status_s::LINK_TYPE_IRIDIUM):
// 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));
}
@ -3892,10 +3892,12 @@ void Commander::data_link_check(bool &status_changed)
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_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_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
if ((_datalink_last_heartbeat_gcs > 0) &&
(hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) {
if (!status.data_link_lost) {
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_counter++;
@ -3998,7 +3998,8 @@ void Commander::data_link_check(bool &status_changed)
// 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();
if (!status.high_latency_data_link_lost) {

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

@ -175,7 +175,6 @@ private:
int _telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
uint64_t _datalink_last_heartbeat_gcs{0};
uint64_t _datalink_lost{0};
uint64_t _datalink_last_heartbeat_onboard_controller{0};
uint64_t _onboard_controller_lost{0};

2
src/modules/commander/commander_params.c
View File

@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
* @decimal 1
* @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