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

commander: fix status init & some status update changes

Browse Source
This commit is contained in:
Beat Küng
2019-03-12 10:28:56 +01:00
parent 697566177c
commit 6672284871
2 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+8 -6
View File
@@ -3769,9 +3769,6 @@ Commander *Commander::instantiate(int argc, char *argv[])
{ {
Commander *instance = new Commander(); Commander *instance = new Commander();
// XXX remove this once this is a class member
status = {};
if (instance) { if (instance) {
if (argc >= 2 && !strcmp(argv[1], "--hil")) { if (argc >= 2 && !strcmp(argv[1], "--hil")) {
instance->enable_hil(); instance->enable_hil();
@@ -3885,7 +3882,10 @@ void Commander::data_link_check(bool &status_changed)
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) { if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false; status.data_link_lost = false;
status_changed = true; 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) { if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained"); mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false; _onboard_controller_lost = false;
status_changed = true;
} }
} }
@@ -3915,9 +3916,9 @@ void Commander::data_link_check(bool &status_changed)
if (_avoidance_system_lost) { if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained"); mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
status_changed = true;
_avoidance_system_lost = false;
} }
_avoidance_system_lost = false;
} }
break; break;
@@ -3947,6 +3948,7 @@ void Commander::data_link_check(bool &status_changed)
mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost"); mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost");
_onboard_controller_lost = true; _onboard_controller_lost = true;
status_changed = true;
} }
// AVOIDANCE SYSTEM state check (only if it is enabled) // AVOIDANCE SYSTEM state check (only if it is enabled)
src/modules/commander/Commander.hpp
+5 -4
View File
@@ -167,10 +167,15 @@ private:
void mission_init(); 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. * Checks the status of all available data links and handles switching between different system telemetry states.
*/ */
void data_link_check(bool &status_changed); void data_link_check(bool &status_changed);
int _telemetry_status_sub{-1}; int _telemetry_status_sub{-1};
hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_gcs{0};
@@ -190,14 +195,10 @@ private:
hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0}; hrt_abstime _high_latency_datalink_lost{0};
void estimator_check(bool *status_changed);
int _battery_sub{-1}; int _battery_sub{-1};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
float _battery_current{0.0f}; float _battery_current{0.0f};
void battery_status_check();
systemlib::Hysteresis _auto_disarm_landed{false}; systemlib::Hysteresis _auto_disarm_landed{false};
systemlib::Hysteresis _auto_disarm_killed{false}; systemlib::Hysteresis _auto_disarm_killed{false};