|
|
|
@ -278,17 +278,20 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
@@ -278,17 +278,20 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
|
|
|
|
|
// Sensors need to be initialized for STANDBY state, except for HIL
|
|
|
|
|
} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && |
|
|
|
|
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && |
|
|
|
|
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && |
|
|
|
|
(!status_flags->condition_system_sensors_initialized)) { |
|
|
|
|
if ((!status_flags->condition_system_prearm_error_reported && |
|
|
|
|
status_flags->condition_system_hotplug_timeout) || |
|
|
|
|
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { |
|
|
|
|
|
|
|
|
|
mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly"); |
|
|
|
|
status_flags->condition_system_prearm_error_reported = true; |
|
|
|
|
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { |
|
|
|
|
|
|
|
|
|
if (!status_flags->condition_system_sensors_initialized) { |
|
|
|
|
|
|
|
|
|
if (status_flags->condition_system_hotplug_timeout) { |
|
|
|
|
if (!status_flags->condition_system_prearm_error_reported) { |
|
|
|
|
mavlink_and_console_log_critical(mavlink_log_pub, |
|
|
|
|
"Not ready to fly: Sensors not set up correctly"); |
|
|
|
|
status_flags->condition_system_prearm_error_reported = true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
feedback_provided = true; |
|
|
|
|
valid_transition = false; |
|
|
|
|
} |
|
|
|
|
feedback_provided = true; |
|
|
|
|
valid_transition = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Finish up the state transition
|
|
|
|
|