commander: remove circuit breakers from status msg

Since the circuit breaker bools are not actually used anywhere else than
in the commander, it is safe to remove them and replace them with local
bools.
This commit is contained in:
Julian Oes 2016-02-24 08:00:33 +00:00
parent ebacd4c1de
commit 5f3a23a253
4 changed files with 137 additions and 50 deletions

View File

@ -127,7 +127,7 @@ bool in_transition_mode # True if VTOL is doing a transition
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_prearm_error_reported # true if errors have already been reported
bool condition_system_prearm_error_reported # true if errors have already been reported
bool condition_system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_system_returned_to_home
bool condition_auto_mission_available
@ -186,8 +186,3 @@ uint16 errors_count2
uint16 errors_count3
uint16 errors_count4
bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_engaged_gpsfailure_check
bool cb_usb

View File

@ -210,6 +210,12 @@ static manual_control_setpoint_s _last_sp_man = {};
static struct vtol_vehicle_status_s vtol_status = {};
static bool circuit_breaker_engaged_power_check;
static bool circuit_breaker_engaged_airspd_check;
static bool circuit_breaker_engaged_enginefailure_check;
static bool circuit_breaker_engaged_gpsfailure_check;
static bool cb_usb;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@ -388,9 +394,15 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) {
int checkres = 0;
checkres = preflight_check(&status, &mavlink_log_pub, false, true);
checkres = preflight_check(&status, &mavlink_log_pub, false, true,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, &mavlink_log_pub, true, true);
checkres = preflight_check(&status, &mavlink_log_pub, true, true,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
@ -612,9 +624,17 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
return TRANSITION_DENIED;
}
// Transition the armed state.
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_log_pub_local);
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
mavlink_log_pub_local,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@ -1244,10 +1264,10 @@ int commander_thread_main(int argc, char *argv[])
status.usb_connected = false;
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
status.circuit_breaker_engaged_airspd_check = false;
status.circuit_breaker_engaged_enginefailure_check = false;
status.circuit_breaker_engaged_gpsfailure_check = false;
circuit_breaker_engaged_power_check = false;
circuit_breaker_engaged_airspd_check = false;
circuit_breaker_engaged_enginefailure_check = false;
circuit_breaker_engaged_gpsfailure_check = false;
get_circuit_breaker_params();
/* publish initial state */
@ -1455,7 +1475,7 @@ int commander_thread_main(int argc, char *argv[])
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
@ -1472,7 +1492,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
@ -1655,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[])
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing
*/
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
chAirspeed = true;
}
@ -1667,7 +1687,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* check sensors also */
(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@ -1764,9 +1784,17 @@ int commander_thread_main(int argc, char *argv[])
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);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, &mavlink_log_pub)) {
mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch");
if (TRANSITION_CHANGED == arming_state_transition(&status,
&safety,
new_arming_state,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
}
@ -2030,8 +2058,16 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
&mavlink_log_pub);
arming_ret = arming_state_transition(&status,
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -2069,7 +2105,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if GPS is ok */
if (!status.circuit_breaker_engaged_gpsfailure_check) {
if (!circuit_breaker_engaged_gpsfailure_check) {
bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE;
//Check if GPS receiver is too noisy while we are disarmed
@ -2281,8 +2317,15 @@ 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, new_arming_state, &armed, true /* fRunPreArmChecks */,
&mavlink_log_pub);
arming_ret = arming_state_transition(&status, &safety,
new_arming_state,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -2318,8 +2361,16 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
&mavlink_log_pub);
arming_ret = arming_state_transition(&status,
&safety,
vehicle_status_s::ARMING_STATE_ARMED,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -2461,7 +2512,7 @@ int commander_thread_main(int argc, char *argv[])
/* Check engine failure
* only for fixed wing for now
*/
if (!status.circuit_breaker_engaged_enginefailure_check &&
if (!circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
@ -2744,15 +2795,15 @@ int commander_thread_main(int argc, char *argv[])
void
get_circuit_breaker_params()
{
status.circuit_breaker_engaged_power_check =
circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.cb_usb =
cb_usb =
circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
}
@ -3486,8 +3537,16 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
false /* fRunPreArmChecks */, &mavlink_log_pub)) {
if (TRANSITION_DENIED == arming_state_transition(&status,
&safety,
vehicle_status_s::ARMING_STATE_INIT,
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
break;
} else {
@ -3566,14 +3625,23 @@ void *commander_low_prio_loop(void *arg)
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub);
arming_state_transition(&status,
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
} else {
tune_negative(true);

View File

@ -116,6 +116,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
orb_advert_t *mavlink_log_pub) ///< uORB handle for mavlink log
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool circuit_breaker_engaged_power_check,
bool cb_usb)
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@ -140,7 +144,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */ );
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
@ -149,7 +156,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */);
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
status->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@ -208,7 +218,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
if (!circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
@ -859,7 +869,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
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)
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool cb_usb)
{
/*
*/
@ -869,15 +882,15 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
if (!circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
checkAirspeed = true;
}
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
!circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!status->cb_usb && status->usb_connected && prearm) {
if (!cb_usb && status->usb_connected && prearm) {
preflight_ok = false;
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");

View File

@ -56,8 +56,16 @@ typedef enum {
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 *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub,
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool circuit_breaker_engaged_power_check,
bool cb_usb);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
@ -65,6 +73,9 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report=false);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool cb_usb);
#endif /* STATE_MACHINE_HELPER_H_ */