diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fcd3034935..2ecfbeaa0b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1528,6 +1528,7 @@ int commander_thread_main(int argc, char *argv[]) int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; uint64_t telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES]; uint64_t telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES]; + bool telemetry_preflight_checks_reported[ORB_MULTI_MAX_INSTANCES]; bool telemetry_lost[ORB_MULTI_MAX_INSTANCES]; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { @@ -1535,6 +1536,7 @@ int commander_thread_main(int argc, char *argv[]) telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; + telemetry_preflight_checks_reported[i] = false; } /* Subscribe to global position */ @@ -1888,8 +1890,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry); /* perform system checks when new telemetry link connected */ - if (/* we first connect a link or re-connect a link after loosing it */ - (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) && + if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */ + (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000) + || !telemetry_preflight_checks_reported[i]) && /* and this link has a communication partner */ (telemetry.heartbeat_time > 0) && /* and it is still connected */ @@ -1898,6 +1901,8 @@ int commander_thread_main(int argc, char *argv[]) !armed.armed) { hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* flag the checks as reported for this link when we actually report them */ + telemetry_preflight_checks_reported[i] = hotplug_timeout; /* provide RC and sensor status feedback to the user */ if (is_hil_setup(autostart_id)) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index dfdfd04a1c..1dd37f9ec7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1105,8 +1105,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps, hrt_abstime time_since_boot) { - /* - */ bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && status_flags->condition_system_hotplug_timeout);