Skip to content

Commit

Permalink
fix commander: run preflight checks on GCS connection
Browse files Browse the repository at this point in the history
Regression from 6dec451, leading to
preflight failures not being reported at all. Only after a failed arming
attempt the messages would be sent. And for GPS check failures, in case
they are set to optional (default), arming would be possible, but switching
to position would be rejected w/o error.

We need to run the preflight checks periodically, but this at least restores
the previous behavior.
  • Loading branch information
bkueng authored and LorenzMeier committed Jan 25, 2020
1 parent 2a67d22 commit db49e5a
Showing 1 changed file with 7 additions and 1 deletion.
8 changes: 7 additions & 1 deletion src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3558,12 +3558,18 @@ void Commander::data_link_check()
switch (telemetry.remote_type) {
case telemetry_status_s::MAV_TYPE_GCS:

// Recover from data link lost
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
_status_changed = true;

if (!armed.armed) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}

if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
Expand Down

0 comments on commit db49e5a

Please sign in to comment.