transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings arming_state_t new_arming_state, ///< arming state requested struct actuator_armed_s *armed, ///< current armed status bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ int prearm_ret = OK; /* only perform the check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = prearm_check(status, mavlink_fd); } /* * Perform an atomic state update */ #ifdef __PX4_NUTTX irqstate_t flags = irqsave(); #endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; prearm_ret = OK; status->condition_system_sensors_initialized = true; /* recover from a prearm fail */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } else { armed->lockdown = false; } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } // Perform power checks only if circuit breaker is not // engaged for these checks if (!status->circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } } } } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { // Silent ignore feedback_provided = true; } // 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->condition_system_sensors_initialized)) { if (!fRunPreArmChecks) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); } feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* end of atomic state update */ #ifdef __PX4_NUTTX irqrestore(flags); #endif } if (ret == TRANSITION_DENIED) { #define WARNSTR "INVAL: %s - %s" /* only print to console here by default as this is too technical to be useful during operation */ warnx(WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); /* print to MAVLink if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_log_critical(mavlink_fd, WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); } #undef WARNSTR } return ret; }
transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot) { // Double check that our static arrays are still valid static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; bool feedback_provided = false; const bool hil_enabled = (status->hil_state == vehicle_status_s::HIL_STATE_ON); /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ bool preflight_check_ret = true; bool prearm_check_ret = true; const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot); if (preflight_check_ret) { status_flags->condition_system_sensors_initialized = true; } feedback_provided = true; } /* re-run the pre-flight check as long as sensors are failing */ if (!status_flags->condition_system_sensors_initialized && fRunPreArmChecks && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) && !hil_enabled) { if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot); last_preflight_check = hrt_absolute_time(); } } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && !hil_enabled) { if (preflight_check_ret) { // only bother running prearm if preflight was successful prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, safety, arm_requirements); } if (!(preflight_check_ret && prearm_check_ret)) { // the prearm and preflight checks already print the rejection reason feedback_provided = true; valid_transition = false; } } } } if (hil_enabled) { /* enforce lockdown in HIL */ armed->lockdown = true; status_flags->condition_system_sensors_initialized = true; /* recover from a prearm fail */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } // HIL can always go to standby if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } } if (!hil_enabled && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { // Sensors need to be initialized for STANDBY state, except for HIL if (!status_flags->condition_system_sensors_initialized) { feedback_provided = true; valid_transition = false; } } // Finish up the state transition if (valid_transition) { armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { armed->armed_time_ms = hrt_absolute_time() / 1000; } else { armed->armed_time_ms = 0; } } } if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s to %s", arming_state_names[status->arming_state], arming_state_names[new_arming_state]); } } return ret; }
transition_result_t arming_state_transition(struct vehicle_status_s *status, /// current vehicle status const struct safety_s *safety, /// current safety settings arming_state_t new_arming_state, /// arming state requested struct actuator_armed_s *armed, /// current armed status const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ int prearm_ret = OK; /* only perform the check if we have to */ if (new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } /* * Perform an atomic state update */ irqstate_t flags = irqsave(); /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; } else { armed->lockdown = false; } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if HIL_STATE_ON if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); valid_transition = false; } // Perform power checks only if circuit breaker is not // engaged for these checks if (!status->circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && (status->avionics_power_rail_voltage < 4.9f)) { mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { valid_transition = false; } // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* end of atomic state update */ irqrestore(flags); } if (ret == TRANSITION_DENIED) { static const char *errMsg = "INVAL: %s - %s"; mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } return ret; }