mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 03:20:35 +08:00
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:
+141
-164
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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_ */
|
||||
|
||||
Reference in New Issue
Block a user