diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39e4e624f..44ad0d98f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -175,12 +175,6 @@ enum MAV_MODE_FLAG { }; -enum class vehicle_battery_warning { - NONE = 0, // no battery low voltage warning active - LOW = 1, // warning of low voltage - CRITICAL = 2, // alerting of critical voltage -}; - /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = 0; @@ -205,6 +199,7 @@ static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; static struct vehicle_status_s status; +static struct battery_status_s battery; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; @@ -407,9 +402,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -635,7 +630,9 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co // Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, + arming_res = arming_state_transition(&status, + &battery, + &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, @@ -1432,7 +1429,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); /* Subscribe to subsystem info topic */ @@ -1786,6 +1782,7 @@ int commander_thread_main(int argc, char *argv[]) vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, + &battery, &safety, new_arming_state, &armed, @@ -2044,6 +2041,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, + &battery, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, @@ -2300,7 +2298,9 @@ int commander_thread_main(int argc, char *argv[]) /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, + arming_ret = arming_state_transition(&status, + &battery, + &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, @@ -2343,6 +2343,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, + &battery, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, @@ -3546,6 +3547,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, + &battery, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, @@ -3639,6 +3641,7 @@ void *commander_low_prio_loop(void *arg) !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, + &battery, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6d314c9bf2..9534fc7f42 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -110,6 +110,7 @@ static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked static int last_prearm_ret = 1; ///< initialize to fail transition_result_t arming_state_transition(struct vehicle_status_s *status, + struct battery_status_s *battery, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed, @@ -142,7 +143,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags); + status_flags, battery); } /* re-run the pre-flight check as long as sensors are failing */ if (!status_flags->condition_system_sensors_initialized @@ -152,7 +153,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, - status_flags); + status_flags, battery); status_flags->condition_system_sensors_initialized = !prearm_ret; last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -255,26 +256,16 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { -<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169 - if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming"); -======= if (status_flags->condition_system_sensors_initialized) { - // mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming"); ->>>>>>> commander: internalize system status bools + mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm"); } feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && -<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169 - status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete"); -======= status_flags->condition_system_sensors_initialized) { - // mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete"); ->>>>>>> commander: internalize system status bools + mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { // Silent ignore @@ -289,13 +280,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if ((!status_flags->condition_system_prearm_error_reported && status_flags->condition_system_hotplug_timeout) || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { -<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169 mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly"); - status->condition_system_prearm_error_reported = true; -======= - mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not initialized"); status_flags->condition_system_prearm_error_reported = true; ->>>>>>> commander: internalize system status bools } feedback_provided = true; valid_transition = false; @@ -879,7 +865,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in return status->nav_state != nav_state_old; } -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags) +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) { /* */ @@ -904,7 +890,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } } - if (status->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { preflight_ok = false; if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: VERY LOW BATTERY"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 72cc961487..c085f924e5 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -94,11 +95,12 @@ struct status_flags_s { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *status, + struct battery_status_s *battery, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, + orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage); @@ -112,7 +114,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe, status_flags_s *status_flags, bool landed); -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, - status_flags_s *status_flags); +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, status_flags_s *status_flags, battery_status_s *battery); #endif /* STATE_MACHINE_HELPER_H_ */