commander: internalize system status bools

Most condition bools in the commander are not used anywhere but in the
commander. It therefore makes sense to move them to a different internal
struct and remove them from the vehicle_status message.

Also, the land_detected should be used by all the modules instead of
getting it through the commander and system_status.
This commit is contained in:
Julian Oes
2016-02-25 17:00:39 +00:00
parent 705979e3c7
commit 1f44fb1efd
18 changed files with 450 additions and 332 deletions
+141 -164
View File
@@ -219,16 +219,11 @@ 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;
static bool calibration_enabled = false;
static uint8_t main_state_prev = 0;
static struct status_flags_s status_flags = {};
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -408,15 +403,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,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, &mavlink_log_pub, true, true,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
@@ -438,7 +427,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "takeoff")) {
/* see if we got a home position */
if (status.condition_home_position_valid) {
if (status_flags.condition_home_position_valid) {
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
@@ -567,7 +556,7 @@ void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "OK" : "NO",
(status.condition_power_input_valid) ? " OK" : "NO");
(status.condition_power_input_valid) ? " OK" : "NO");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw);
warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
@@ -645,10 +634,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
&armed,
true /* fRunPreArmChecks */,
mavlink_log_pub_local,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
&status_flags);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -706,34 +692,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
if (custom_sub_mode > 0) {
switch(custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags);
break;
case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET);
@@ -746,12 +732,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
} else {
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags);
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
/* RATTITUDE */
@@ -759,30 +745,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags);
} else {
/* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
}
}
}
@@ -822,7 +808,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
else {
// Refuse to arm if preflight checks have failed
if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
@@ -944,7 +930,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (use_current) {
/* use current position */
if (status_local->condition_global_position_valid) {
if (status_flags.condition_global_position_valid) {
home->lat = global_pos->lat;
home->lon = global_pos->lon;
home->alt = global_pos->alt;
@@ -980,7 +966,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
/* mark home position as set */
status_local->condition_home_position_valid = true;
status_flags.condition_home_position_valid = true;
}
}
break;
@@ -994,7 +980,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -1008,7 +994,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else {
/* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev);
res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags);
status_local->offboard_control_set_by_command = false;
}
}
@@ -1016,7 +1002,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags)) {
warnx("taking off!");
} else {
warnx("takeoff denied");
@@ -1027,7 +1013,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags)) {
warnx("landing!");
} else {
warnx("landing denied");
@@ -1081,7 +1067,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
const vehicle_attitude_s &attitude)
{
//Need global position fix to be able to set home
if (!status.condition_global_position_valid) {
if (!status_flags.condition_global_position_valid) {
return;
}
@@ -1113,12 +1099,12 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
}
//Play tune first time we initialize HOME
if (!status.condition_home_position_valid) {
if (!status_flags.condition_home_position_valid) {
tune_home_set(true);
}
/* mark home position as set */
status.condition_home_position_valid = true;
status_flags.condition_home_position_valid = true;
}
int commander_thread_main(int argc, char *argv[])
@@ -1133,6 +1119,9 @@ int commander_thread_main(int argc, char *argv[])
bool startup_in_hil = false;
// XXX for now just set sensors as initialized
status_flags.condition_system_sensors_initialized = true;
#ifdef __PX4_NUTTX
/* NuttX indicates 3 arguments when only 2 are present */
argc -= 1;
@@ -1231,7 +1220,7 @@ int commander_thread_main(int argc, char *argv[])
/* vehicle status topic */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
@@ -1256,23 +1245,20 @@ int commander_thread_main(int argc, char *argv[])
status.offboard_control_signal_lost = true;
status.data_link_lost = true;
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
status.condition_system_prearm_error_reported = false;
status.condition_system_hotplug_timeout = false;
status_flags.condition_system_prearm_error_reported = false;
status_flags.condition_system_hotplug_timeout = false;
status.timestamp = hrt_absolute_time();
status.condition_power_input_valid = true;
status_flags.condition_power_input_valid = true;
status.avionics_power_rail_voltage = -1.0f;
status.usb_connected = false;
status_flags.usb_connected = false;
// CIRCUIT BREAKERS
circuit_breaker_engaged_power_check = false;
circuit_breaker_engaged_airspd_check = false;
circuit_breaker_engaged_enginefailure_check = false;
circuit_breaker_engaged_gpsfailure_check = false;
status_flags.circuit_breaker_engaged_power_check = false;
status_flags.circuit_breaker_engaged_airspd_check = false;
status_flags.circuit_breaker_engaged_enginefailure_check = false;
status_flags.circuit_breaker_engaged_gpsfailure_check = false;
get_circuit_breaker_params();
/* publish initial state */
@@ -1480,7 +1466,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 (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
@@ -1492,12 +1478,13 @@ int commander_thread_main(int argc, char *argv[])
status.rc_input_mode = rc_in_off;
if (is_hil_setup(autostart_id)) {
// HIL configuration selected: real sensors will be disabled
status.condition_system_sensors_initialized = false;
status_flags.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} 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), !circuit_breaker_engaged_gpsfailure_check, false);
status_flags.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_flags.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
@@ -1680,7 +1667,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 (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
chAirspeed = true;
}
@@ -1692,7 +1679,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), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@@ -1750,17 +1737,17 @@ int commander_thread_main(int argc, char *argv[])
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status.condition_power_input_valid = false;
status_flags.condition_power_input_valid = false;
} else {
status.condition_power_input_valid = true;
status_flags.condition_power_input_valid = true;
}
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
/* if the USB hardware connection went away, reboot */
if (status.usb_connected && !system_power.usb_connected) {
if (status_flags.usb_connected && !system_power.usb_connected) {
/*
* apparently the USB cable went away but we are still powered,
* so lets reset to a classic non-usb state.
@@ -1771,11 +1758,11 @@ int commander_thread_main(int argc, char *argv[])
}
/* finally judge the USB connected state based on software detection */
status.usb_connected = _usb_telemetry_active;
status_flags.usb_connected = _usb_telemetry_active;
}
}
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status_flags.condition_airspeed_valid), &status_changed);
/* update safety topic */
orb_check(safety_sub, &updated);
@@ -1795,10 +1782,7 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb)) {
&status_flags)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1849,7 +1833,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX consolidate this with local position handling and timeouts after release
// but we want a low-risk change now.
if (status.condition_global_position_valid) {
if (status_flags.condition_global_position_valid) {
if (gpos.eph < eph_threshold * 2.5f) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
@@ -1880,16 +1864,16 @@ int commander_thread_main(int argc, char *argv[])
//Global positions are only published by the estimators if they are valid
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
//We have had no good fix for POSITION_TIMEOUT amount of time
if (status.condition_global_position_valid) {
if (status_flags.condition_global_position_valid) {
set_tune_override(TONE_GPS_WARNING_TUNE);
status_changed = true;
status.condition_global_position_valid = false;
status_flags.condition_global_position_valid = false;
}
} else if (global_position.timestamp != 0) {
// Got good global position estimate
if (!status.condition_global_position_valid) {
if (!status_flags.condition_global_position_valid) {
status_changed = true;
status.condition_global_position_valid = true;
status_flags.condition_global_position_valid = true;
}
}
@@ -1897,7 +1881,7 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_local_position_valid) {
if (status_flags.condition_local_position_valid) {
if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
@@ -1915,9 +1899,9 @@ int commander_thread_main(int argc, char *argv[])
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
&& local_eph_good, &(status.condition_local_position_valid), &status_changed);
&& local_eph_good, &(status_flags.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
&(status.condition_local_altitude_valid), &status_changed);
&(status_flags.condition_local_altitude_valid), &status_changed);
/* Update land detector */
static bool check_for_disarming = false;
@@ -1926,12 +1910,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
}
if ((updated && status.condition_local_altitude_valid) || check_for_disarming) {
if (status.condition_landed != land_detector.landed) {
status.condition_landed = land_detector.landed;
status_changed = true;
if (status.condition_landed) {
if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) {
if (was_landed != land_detector.landed) {
if (land_detector.landed) {
mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
} else {
@@ -2051,17 +2032,14 @@ int commander_thread_main(int argc, char *argv[])
}
/* If in INIT state, try to proceed to STANDBY state */
if (!calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
if (!status_flags.condition_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,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
&status_flags);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2099,7 +2077,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if GPS is ok */
if (!circuit_breaker_engaged_gpsfailure_check) {
if (!status_flags.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
@@ -2178,13 +2156,13 @@ int commander_thread_main(int argc, char *argv[])
break;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags)) {
geofence_loiter_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags)) {
geofence_rtl_on = true;
}
break;
@@ -2231,7 +2209,7 @@ int commander_thread_main(int argc, char *argv[])
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation, main_state_prev);
main_state_transition(&status, geofence_main_state_before_violation, main_state_prev, &status_flags);
}
}
}
@@ -2258,7 +2236,7 @@ int commander_thread_main(int argc, char *argv[])
* rejection. Back off 2 seconds to not overlay
* home tune.
*/
if (status.condition_home_position_valid &&
if (status_flags.condition_home_position_valid &&
(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
_last_mission_instance != mission_result.instance_count) {
if (!mission_result.valid) {
@@ -2304,7 +2282,7 @@ int commander_thread_main(int argc, char *argv[])
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
status.condition_landed) &&
land_detector.landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -2316,10 +2294,7 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
&status_flags);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2350,7 +2325,7 @@ int commander_thread_main(int argc, char *argv[])
&& (status.main_state != vehicle_status_s::MAIN_STATE_ALTCTL)) {
print_reject_arm("NOT ARMING: Switch to a manual mode first.");
} else if (!status.condition_home_position_valid &&
} else if (!status_flags.condition_home_position_valid &&
geofence_action == geofence_result_s::GF_ACTION_RTL) {
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
@@ -2361,10 +2336,7 @@ int commander_thread_main(int argc, char *argv[])
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
&status_flags);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2461,7 +2433,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* got link again or new */
status.condition_system_prearm_error_reported = false;
status_flags.condition_system_prearm_error_reported = false;
status_changed = true;
telemetry_lost[i] = false;
@@ -2513,7 +2485,7 @@ int commander_thread_main(int argc, char *argv[])
/* Check engine failure
* only for fixed wing for now
*/
if (!circuit_breaker_engaged_enginefailure_check &&
if (!status_flags.circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
@@ -2549,9 +2521,9 @@ int commander_thread_main(int argc, char *argv[])
if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.condition_landed)) {
&& land_detector.landed)) {
main_state_transition(&status, main_state_prev, main_state_prev);
main_state_transition(&status, main_state_prev, main_state_prev, &status_flags);
}
}
@@ -2627,17 +2599,17 @@ int commander_thread_main(int argc, char *argv[])
const hrt_abstime now = hrt_absolute_time();
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
if (!status_flags.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
else if (((!was_armed && armed.armed) || (was_landed && !land_detector.landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
was_landed = status.condition_landed;
was_landed = land_detector.landed;
was_armed = armed.armed;
/* print new state */
@@ -2649,7 +2621,9 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished,
mission_result.stay_in_failsafe);
mission_result.stay_in_failsafe,
&status_flags,
land_detector.landed);
if (status.failsafe != failsafe_old) {
status_changed = true;
@@ -2731,15 +2705,15 @@ int commander_thread_main(int argc, char *argv[])
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
status_changed = true;
}
/* update timeout flag */
if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
status.condition_system_hotplug_timeout = hotplug_timeout;
if(!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) {
status_flags.condition_system_hotplug_timeout = hotplug_timeout;
status_changed = true;
}
@@ -2797,16 +2771,11 @@ int commander_thread_main(int argc, char *argv[])
void
get_circuit_breaker_params()
{
circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
cb_usb =
circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status_flags.cb_usb = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
}
void
@@ -2834,7 +2803,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status_flags.condition_system_sensors_initialized && hotplug_timeout)) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
@@ -2842,7 +2811,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
} else if (!status_flags.condition_system_sensors_initialized && !hotplug_timeout) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2860,7 +2829,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
rgbled_set_color(RGBLED_COLOR_RED);
} else {
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
@@ -2915,7 +2884,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
// just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags);
}
/* manual setpoint has not updated, do not re-evaluate it */
@@ -2944,7 +2913,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -2959,13 +2928,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
warnx("RTL switch changed and ON!");
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO RTL");
/* fallback to LOITER if home position not set */
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags);
}
if (res != TRANSITION_DENIED) {
@@ -3078,11 +3047,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
*/
// XXX: put ACRO and STAB on separate switches
if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev, &status_flags);
} else if (!status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
}
}
@@ -3090,12 +3059,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev, &status_flags);
}
}else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
}
// TRANSITION_DENIED is not possible here
@@ -3103,7 +3072,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -3113,7 +3082,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
@@ -3124,13 +3093,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
// TRANSITION_DENIED is not possible here
break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -3139,7 +3108,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO PAUSE");
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -3148,7 +3117,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -3156,21 +3125,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to POSCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags);
// TRANSITION_DENIED is not possible here
break;
@@ -3300,6 +3269,20 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status_flags.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status_flags.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
@@ -3545,14 +3528,11 @@ void *commander_low_prio_loop(void *arg)
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb)) {
&status_flags)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
break;
} else {
calibration_enabled = true;
status_flags.condition_calibration_enabled = true;
}
if ((int)(cmd.param1) == 1) {
@@ -3613,7 +3593,7 @@ void *commander_low_prio_loop(void *arg)
calib_ret = OK;
}
calibration_enabled = false;
status_flags.condition_calibration_enabled = false;
if (calib_ret == OK) {
tune_positive(true);
@@ -3627,12 +3607,12 @@ 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 (!circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
if (!status_flags.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), !circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
status_flags.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_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status,
&safety,
@@ -3640,10 +3620,7 @@ void *commander_low_prio_loop(void *arg)
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
circuit_breaker_engaged_power_check,
cb_usb);
&status_flags);
} else {
tune_negative(true);
+79 -74
View File
@@ -109,17 +109,13 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
static int last_prearm_ret = 1; ///< initialize to fail
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
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)
transition_result_t arming_state_transition(struct vehicle_status_s *status,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags)
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -145,22 +141,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
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);
status_flags);
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
if (!status_flags->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& 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 */,
circuit_breaker_engaged_airspd_check,
circuit_breaker_engaged_gpsfailure_check,
cb_usb);
status->condition_system_sensors_initialized = !prearm_ret;
status_flags);
status_flags->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
} else {
@@ -179,7 +171,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
prearm_ret = OK;
status->condition_system_sensors_initialized = true;
status_flags->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
@@ -218,9 +210,9 @@ 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 (!circuit_breaker_engaged_power_check) {
if (!status_flags->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
if (!status_flags->condition_power_input_valid) {
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Connect power module.");
feedback_provided = true;
@@ -229,7 +221,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// 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)) {
if (status_flags->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < AVIONICS_ERROR_VOLTAGE) {
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
@@ -262,16 +254,26 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
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
} 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
feedback_provided = true;
} else {
// Silent ignore
@@ -282,12 +284,17 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
} 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 ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
(!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)) {
<<<<<<< 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;
@@ -305,7 +312,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
valid_transition) {
status->condition_system_prearm_error_reported = false;
status_flags->condition_system_prearm_error_reported = false;
}
/* end of atomic state update */
@@ -339,7 +346,8 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev)
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
status_flags_s *status_flags)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -356,23 +364,23 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
(status_flags->condition_local_altitude_valid ||
status_flags->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
if (status_flags->condition_local_position_valid ||
status_flags->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
if (status_flags->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
@@ -383,7 +391,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
case vehicle_status_s::MAIN_STATE_AUTO_LAND:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
@@ -589,7 +597,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -605,14 +613,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !landed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -686,11 +694,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -703,11 +711,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
(status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -727,11 +735,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -741,11 +749,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -767,13 +775,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -808,13 +816,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -829,11 +837,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_altitude_valid) {
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -852,9 +860,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status->condition_local_altitude_valid) {
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -869,28 +877,25 @@ 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,
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool cb_usb)
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags)
{
/*
*/
bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout);
bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
status_flags->condition_system_hotplug_timeout);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
if (!status_flags->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),
!circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
!status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!cb_usb && status->usb_connected && prearm) {
if (!status_flags->cb_usb && status_flags->usb_connected && prearm) {
preflight_ok = false;
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");
@@ -906,7 +911,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
/* report once, then set the flag */
if (reportFailures && !preflight_ok) {
status->condition_system_prearm_error_reported = true;
status_flags->condition_system_prearm_error_reported = true;
}
return !preflight_ok;
+30 -9
View File
@@ -54,6 +54,29 @@ typedef enum {
} transition_result_t;
// This is a struct used by the commander internally.
struct status_flags_s {
bool condition_calibration_enabled;
bool condition_system_sensors_initialized;
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;
bool condition_global_position_valid; // set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch)
bool condition_local_position_valid;
bool condition_local_altitude_valid;
bool condition_airspeed_valid; // set to true by the commander app if there is a valid airspeed measurement available
bool condition_power_input_valid; // set if input power is valid
bool usb_connected; // status of the USB power supply
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;
};
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,
@@ -62,20 +85,18 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta
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);
status_flags_s *status_flags);
transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev);
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
status_flags_s *status_flags);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
bool set_nav_state(struct vehicle_status_s *status, 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,
bool circuit_breaker_engaged_airspd_check,
bool circuit_breaker_engaged_gpsfailure_check,
bool cb_usb);
status_flags_s *status_flags);
#endif /* STATE_MACHINE_HELPER_H_ */