From 6672284871d4cd4163e99d68e75831d0495f58c8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 12 Mar 2019 10:28:56 +0100
Subject: [PATCH] commander: fix status init & some status update changes
---
src/modules/commander/Commander.cpp | 14 ++++++++------
src/modules/commander/Commander.hpp | 9 +++++----
2 files changed, 13 insertions(+), 10 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index d25aa94aaf..054a8749be 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -3769,9 +3769,6 @@ Commander *Commander::instantiate(int argc, char *argv[])
{
Commander *instance = new Commander();
- // XXX remove this once this is a class member
- status = {};
-
if (instance) {
if (argc >= 2 && !strcmp(argv[1], "--hil")) {
instance->enable_hil();
@@ -3885,7 +3882,10 @@ void Commander::data_link_check(bool &status_changed)
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");
+
+ if (_datalink_last_heartbeat_gcs != 0) {
+ mavlink_log_info(&mavlink_log_pub, "Data link regained");
+ }
}
}
@@ -3899,6 +3899,7 @@ void Commander::data_link_check(bool &status_changed)
if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
+ status_changed = true;
}
}
@@ -3915,9 +3916,9 @@ void Commander::data_link_check(bool &status_changed)
if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
+ status_changed = true;
+ _avoidance_system_lost = false;
}
-
- _avoidance_system_lost = false;
}
break;
@@ -3947,6 +3948,7 @@ void Commander::data_link_check(bool &status_changed)
mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost");
_onboard_controller_lost = true;
+ status_changed = true;
}
// AVOIDANCE SYSTEM state check (only if it is enabled)
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 4741b755ca..82c3648385 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -167,10 +167,15 @@ private:
void mission_init();
+ void estimator_check(bool *status_changed);
+
+ void battery_status_check();
+
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void data_link_check(bool &status_changed);
+
int _telemetry_status_sub{-1};
hrt_abstime _datalink_last_heartbeat_gcs{0};
@@ -190,14 +195,10 @@ private:
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
- void estimator_check(bool *status_changed);
-
int _battery_sub{-1};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
float _battery_current{0.0f};
- void battery_status_check();
-
systemlib::Hysteresis _auto_disarm_landed{false};
systemlib::Hysteresis _auto_disarm_killed{false};