diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d25aa94aaf44b50a0438844dc0f6aa3219104b58..054a8749bedbfa1bfcca507b363c43f6fa7a1e5b 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 4741b755cae92d996c5e2a9fce12c7bb7ef354eb..82c36483859881d9b5c827f9629b4a08fcd634b2 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};