dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
msg/vehicle_status.msg
View File

@ -74,21 +74,7 @@ bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool in_transition_mode # True if VTOL is doing a transition
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_prearm_error_reported # true if errors have already been reported
bool condition_system_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_landed # true if vehicle is landed, always true if disarmed
bool condition_power_input_valid # set if input power is valid
float32 avionics_power_rail_voltage # voltage of the avionics power rail
bool usb_connected # status of the USB power supply
float32 avionics_power_rail_voltage # voltage of the avionics power rail
bool rc_signal_found_once
bool rc_signal_lost # true if RC reception lost

305
src/modules/commander/commander.cpp
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);

153
src/modules/commander/state_machine_helper.cpp
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;

39
src/modules/commander/state_machine_helper.h
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_ */

32
src/modules/ekf2/ekf2_main.cpp
View File

@ -75,11 +75,11 @@
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <ecl/EKF/ekf.h>
@ -139,6 +139,8 @@ private:
int _vehicle_status_sub = -1;
int _optical_flow_sub = -1;
int _range_finder_sub = -1;
int _actuator_armed_sub = -1;
int _vehicle_land_detected_sub = -1;
bool _prev_motors_armed = false; // motors armed status from the previous frame
@ -319,6 +321,8 @@ void Ekf2::task_main()
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
_actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
px4_pollfd_struct_t fds[2] = {};
fds[0].fd = _sensors_sub;
@ -338,6 +342,7 @@ void Ekf2::task_main()
vehicle_control_mode_s vehicle_control_mode = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
actuator_armed_s actuator_armed = {};
while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@ -371,6 +376,8 @@ void Ekf2::task_main()
bool vehicle_status_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
bool actuator_armed_updated = false;
bool vehicle_land_detected_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
@ -465,14 +472,19 @@ void Ekf2::task_main()
_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
}
// read vehicle status if available for 'landed' information
orb_check(_vehicle_status_sub, &vehicle_status_updated);
orb_check(_actuator_armed_sub, &actuator_armed_updated);
if (vehicle_status_updated) {
struct vehicle_status_s status = {};
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
_ekf->set_in_air_status(!status.condition_landed);
_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
if (actuator_armed_updated) {
orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
_ekf->set_arm_status(&actuator_armed.armed);
}
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
struct vehicle_land_detected_s vehicle_land_detected = {};
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
_ekf->set_in_air_status(!vehicle_land_detected.landed);
}
// run the EKF update and output
@ -703,7 +715,7 @@ void Ekf2::task_main()
}
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) {
float decl_deg;
_ekf->copy_mag_decl_deg(&decl_deg);
_mag_declination_deg->set(decl_deg);

16
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -50,9 +50,9 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
@ -147,7 +147,8 @@ private:
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _vehicle_status_sub;
int _vehicle_land_detected_sub;
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
@ -169,13 +170,13 @@ private:
struct mag_report _mag;
struct airspeed_s _airspeed; /**< airspeed */
struct baro_report _baro; /**< baro readings */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_status_s _vehicle_status;
struct vehicle_land_detected_s _vehicle_land_detected;
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
hrt_abstime _last_accel;
@ -299,10 +300,15 @@ private:
void control_update();
/**
* Check for changes in vehicle status.
* Check for changes in land detected.
*/
void vehicle_status_poll();
/**
* Check for changes in land detected.
*/
void vehicle_land_detected_poll();
/**
* Shim for calling task_main from task_create.
*/

56
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -132,12 +132,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_airspeed_sub(-1),
_baro_sub(-1),
_gps_sub(-1),
_vstatus_sub(-1),
_vehicle_status_sub(-1),
_vehicle_land_detected_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
_landDetectorSub(-1),
_armedSub(-1),
/* publications */
@ -155,13 +155,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_mag{},
_airspeed{},
_baro{},
_vstatus{},
_vehicle_status{},
_vehicle_land_detected{},
_global_pos{},
_local_pos{},
_gps{},
_wind{},
_distance{},
_landDetector{},
_armed{},
_last_accel(0),
@ -346,22 +346,31 @@ int AttitudePositionEstimatorEKF::parameters_update()
void AttitudePositionEstimatorEKF::vehicle_status_poll()
{
bool vstatus_updated;
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
orb_check(_vehicle_status_sub, &updated);
bool landed = _vstatus.condition_landed;
if (updated) {
if (vstatus_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
// Tell EKF that the vehicle is a fixed wing or multi-rotor
_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
_ekf->setIsFixedWing(!_vehicle_status.is_rotary_wing);
}
}
void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
{
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
// Save params on landed
if (!landed && _vstatus.condition_landed) {
if (!_vehicle_land_detected.landed) {
_mag_offset_x.set(_ekf->magBias.x);
_mag_offset_x.commit();
_mag_offset_y.set(_ekf->magBias.y);
@ -526,10 +535,10 @@ void AttitudePositionEstimatorEKF::task_main()
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -586,13 +595,14 @@ void AttitudePositionEstimatorEKF::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
bool prev_hil = (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
vehicle_land_detected_poll();
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
if (!prev_hil && (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
@ -690,7 +700,7 @@ void AttitudePositionEstimatorEKF::task_main()
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
_ekf->setOnGround(_vehicle_land_detected.landed);
// We're apparently initialized in this case now
// check (and reset the filter as needed)
@ -1305,7 +1315,7 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
(_vehicle_land_detected.landed) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
@ -1520,14 +1530,6 @@ void AttitudePositionEstimatorEKF::pollData()
//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
//Update Land Detector
bool newLandData;
orb_check(_landDetectorSub, &newLandData);
if (newLandData) {
orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
}
//Update AirSpeed
orb_check(_airspeed_sub, &_newAdsData);

28
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -72,6 +72,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@ -135,6 +136,7 @@ private:
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _vehicle_land_detected_sub; /**< vehicle land detected subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
@ -155,6 +157,7 @@ private:
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
@ -317,6 +320,11 @@ private:
*/
void vehicle_status_poll();
/**
* Check for vehicle land detected updates.
*/
void vehicle_land_detected_poll();
/**
* Shim for calling task_main from task_create.
*/
@ -355,6 +363,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_manual_sub(-1),
_global_pos_sub(-1),
_vehicle_status_sub(-1),
_vehicle_land_detected_sub(-1),
/* publications */
_rate_sp_pub(nullptr),
@ -387,6 +396,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators_airframe = {};
_global_pos = {};
_vehicle_status = {};
_vehicle_land_detected = {};
_parameter_handles.p_tc = param_find("FW_P_TC");
@ -652,6 +662,18 @@ FixedwingAttitudeControl::vehicle_status_poll()
}
}
void
FixedwingAttitudeControl::vehicle_land_detected_poll()
{
/* check if there is new status information */
bool vehicle_land_detected_updated;
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}
}
void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@ -672,6 +694,7 @@ FixedwingAttitudeControl::task_main()
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
parameters_update();
@ -681,6 +704,7 @@ FixedwingAttitudeControl::task_main()
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();
/* wakeup source */
px4_pollfd_struct_t fds[2];
@ -806,6 +830,8 @@ FixedwingAttitudeControl::task_main()
vehicle_status_poll();
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
// we need to make sure that this flag is reset
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
@ -1056,7 +1082,7 @@ FixedwingAttitudeControl::task_main()
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
if (_vehicle_land_detected.landed || _vehicle_status.is_rotary_wing) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();

38
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -82,6 +82,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/tecs_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -161,6 +162,7 @@ private:
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _vehicle_land_detected_sub; /**< vehicle land detected subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
@ -177,6 +179,7 @@ private:
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
@ -377,6 +380,11 @@ private:
*/
void vehicle_status_poll();
/**
* Check for changes in vehicle land detected.
*/
void vehicle_land_detected_poll();
/**
* Check for manual setpoint updates.
*/
@ -516,6 +524,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_vehicle_land_detected_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
@ -535,6 +544,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_manual(),
_control_mode(),
_vehicle_status(),
_vehicle_land_detected(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
@ -812,6 +822,18 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
void
FixedwingPositionControl::vehicle_land_detected_poll()
{
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@ -1204,7 +1226,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_earth = _R_nb * accel_body;
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_status.condition_landed &&
bool in_air_alt_control = (!_vehicle_land_detected.landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
@ -1225,14 +1247,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float throttle_max = 1.0f;
/* save time when airplane is in air */
if (!_was_in_air && !_vehicle_status.condition_landed) {
if (!_was_in_air && !_vehicle_land_detected.landed) {
_was_in_air = true;
_time_went_in_air = hrt_absolute_time();
_takeoff_ground_alt = _global_pos.alt;
}
/* reset flag when airplane landed */
if (_vehicle_status.condition_landed) {
if (_vehicle_land_detected.landed) {
_was_in_air = false;
}
@ -1993,7 +2015,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* Copy thrust and pitch values from tecs */
if (_vehicle_status.condition_landed) {
if (_vehicle_land_detected.landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust = math::min(_parameters.throttle_idle, throttle_max);
@ -2062,6 +2084,7 @@ FixedwingPositionControl::task_main()
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -2069,6 +2092,8 @@ FixedwingPositionControl::task_main()
orb_set_interval(_control_mode_sub, 200);
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vehicle_status_sub, 200);
/* rate limit vehicle land detected updates to 5Hz */
orb_set_interval(_vehicle_land_detected_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@ -2112,6 +2137,9 @@ FixedwingPositionControl::task_main()
/* check vehicle status for changes to publication state */
vehicle_status_poll();
/* check vehicle land detected for changes to publication state */
vehicle_land_detected_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
@ -2222,7 +2250,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_last_tecs_update = hrt_absolute_time();
// do not run TECS if we are not in air
run_tecs &= !_vehicle_status.condition_landed;
run_tecs &= !_vehicle_land_detected.landed;
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
// (it should also not run during VTOL blending because airspeed is too low still)

9
src/modules/mavlink/mavlink_messages.cpp
View File

@ -574,12 +574,13 @@ protected:
struct vehicle_status_s status;
struct battery_status_s battery_status;
bool updated = _status_sub->update(&status);
updated = (updated || _battery_status_sub->update(&battery_status));
const bool updated_status = _status_sub->update(&status);
const bool updated_battery = _battery_status_sub->update(&battery_status);
if (updated) {
if (updated_status || updated_battery) {
mavlink_sys_status_t msg;
msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
@ -595,6 +596,8 @@ protected:
msg.errors_count3 = 0;
msg.errors_count4 = 0;
PX4_INFO("send sys status: voltage: %d", msg.voltage_battery);
_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
/* battery status message with higher resolution */

20
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -77,6 +77,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
@ -126,6 +127,7 @@ private:
orb_advert_t _mavlink_log_pub; /**< mavlink log advert */
int _vehicle_status_sub; /**< vehicle status subscription */
int _vehicle_land_detected_sub; /**< vehicle land detected subscription */
int _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
@ -144,7 +146,8 @@ private:
orb_id_t _attitude_setpoint_id;
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@ -607,6 +610,12 @@ MulticopterPositionControl::poll_subscriptions()
}
}
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}
orb_check(_ctrl_state_sub, &updated);
if (updated) {
@ -1153,6 +1162,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@ -1341,7 +1351,7 @@ MulticopterPositionControl::task_main()
}
} else if (_control_mode.flag_control_manual_enabled
&& _vehicle_status.condition_landed) {
&& _vehicle_land_detected.landed) {
/* don't run controller when landed */
_reset_pos_sp = true;
_reset_alt_sp = true;
@ -1462,7 +1472,7 @@ MulticopterPositionControl::task_main()
// check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore
if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
if (!_takeoff_jumped && !_vehicle_land_detected.landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
_takeoff_jumped = true;
}
@ -1906,7 +1916,7 @@ MulticopterPositionControl::task_main()
}
/* do not move yaw while sitting on the ground */
else if (!_vehicle_status.condition_landed &&
else if (!_vehicle_land_detected.landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */
@ -1939,7 +1949,7 @@ MulticopterPositionControl::task_main()
_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
/* enforce minimum throttle if not landed */
if (!_vehicle_status.condition_landed) {
if (!_vehicle_land_detected.landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}

14
src/modules/navigator/mission.cpp
View File

@ -144,7 +144,7 @@ Mission::on_inactive()
check_mission_valid();
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
_need_takeoff = true;
}
}
@ -278,7 +278,7 @@ Mission::update_offboard_mission()
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
_navigator->get_vstatus()->condition_landed);
_navigator->get_land_detected()->landed);
_navigator->get_mission_result()->valid = !failed;
if (!failed) {
@ -413,7 +413,7 @@ Mission::set_mission_items()
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
if (_navigator->get_vstatus()->condition_landed) {
if (_navigator->get_land_detected()->landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
@ -585,7 +585,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_ALIGN
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
@ -658,7 +658,7 @@ Mission::do_need_takeoff()
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
/* force takeoff if landed (additional protection) */
if (_navigator->get_vstatus()->condition_landed) {
if (_navigator->get_land_detected()->landed) {
_need_takeoff = true;
/* if in-air and already above takeoff height, don't do takeoff */
@ -752,7 +752,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
if (_navigator->get_land_detected()->landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else {
@ -1118,7 +1118,7 @@ Mission::check_mission_valid()
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
_navigator->get_vstatus()->condition_landed);
_navigator->get_land_detected()->landed);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();

4
src/modules/navigator/mission_block.cpp
View File

@ -95,7 +95,7 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_LAND: /* fall through */
case NAV_CMD_VTOL_LAND:
return _navigator->get_vstatus()->condition_landed;
return _navigator->get_land_detected()->landed;
/* TODO: count turns */
/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
@ -413,7 +413,7 @@ MissionBlock::set_previous_pos_setpoint()
void
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
{
if (_navigator->get_vstatus()->condition_landed) {
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;

9
src/modules/navigator/navigator.h
View File

@ -57,6 +57,7 @@
#include <uORB/topics/mission_result.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include "navigator_mode.h"
#include "mission.h"
@ -133,6 +134,7 @@ public:
* Getters
*/
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_land_detected_s* get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
@ -200,6 +202,7 @@ private:
int _sensor_combined_sub; /**< sensor combined subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _land_detected_sub; /**< vehicle land detected subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
@ -215,6 +218,7 @@ private:
when pos control is deactivated */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_land_detected_s _land_detected; /**< vehicle land_detected */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
vehicle_gps_position_s _gps_pos; /**< gps position */
@ -300,6 +304,11 @@ private:
*/
void vehicle_status_update();
/**
* Retrieve vehicle land detected
*/
void vehicle_land_detected_update();
/**
* Retrieve vehicle control mode
*/

16
src/modules/navigator/navigator_main.cpp
View File

@ -107,6 +107,7 @@ Navigator::Navigator() :
_gps_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_land_detected_sub(-1),
_capabilities_sub(-1),
_control_mode_sub(-1),
_onboard_mission_sub(-1),
@ -118,6 +119,7 @@ Navigator::Navigator() :
_geofence_result_pub(nullptr),
_att_sp_pub(nullptr),
_vstatus{},
_land_detected{},
_control_mode{},
_global_pos{},
_gps_pos{},
@ -241,6 +243,12 @@ Navigator::vehicle_status_update()
}
}
void
Navigator::vehicle_land_detected_update()
{
orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
}
void
Navigator::vehicle_control_mode_update()
{
@ -290,6 +298,7 @@ Navigator::task_main()
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
@ -299,6 +308,7 @@ Navigator::task_main()
/* copy all topics first time */
vehicle_status_update();
vehicle_land_detected_update();
vehicle_control_mode_update();
global_position_update();
gps_position_update();
@ -374,6 +384,12 @@ Navigator::task_main()
vehicle_status_update();
}
/* vehicle land detected updated */
orb_check(_land_detected_sub, &updated);
if (updated) {
vehicle_land_detected_update();
}
/* navigation capabilities updated */
orb_check(_capabilities_sub, &updated);
if (updated) {

2
src/modules/navigator/rtl.cpp
View File

@ -94,7 +94,7 @@ RTL::on_activation()
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");

16
src/modules/sdlog2/sdlog2.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -40,6 +40,7 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
@ -112,6 +113,7 @@
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -1156,6 +1158,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct ekf2_innovations_s innovations;
struct camera_trigger_s camera_trigger;
struct ekf2_replay_s replay;
struct vehicle_land_detected_s land_detected;
} buf;
memset(&buf, 0, sizeof(buf));
@ -1213,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST6_s log_INO3;
struct log_RPL3_s log_RPL3;
struct log_RPL4_s log_RPL4;
struct log_LAND_s log_LAND;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -1260,6 +1264,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int innov_sub;
int cam_trig_sub;
int replay_sub;
int land_detected_sub;
} subs;
subs.cmd_sub = -1;
@ -1299,6 +1304,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.innov_sub = -1;
subs.cam_trig_sub = -1;
subs.replay_sub = -1;
subs.land_detected_sub = -1;
/* add new topics HERE */
@ -1447,7 +1453,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = buf_status.main_state;
log_msg.body.log_STAT.arming_state = buf_status.arming_state;
log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
log_msg.body.log_STAT.load = buf_status.load;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
@ -2135,6 +2140,13 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(CAMT);
}
/* --- LAND DETECTED --- */
if (copy_if_updated(ORB_ID(vehicle_land_detected), &subs.land_detected_sub, &buf.land_detected)) {
log_msg.msg_type = LOG_LAND_MSG;
log_msg.body.log_LAND.landed = buf.land_detected.landed;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
pthread_mutex_lock(&logbuffer_mutex);
/* signal the other thread new data, but not yet unlock */

9
src/modules/sdlog2/sdlog2_messages.h
View File

@ -180,7 +180,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t arming_state;
uint8_t failsafe;
uint8_t landed;
float load;
};
@ -582,6 +581,11 @@ struct log_CAMT_s {
uint32_t seq;
};
#define LOG_LAND_MSG 56
struct log_LAND_s {
uint8_t landed;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -623,7 +627,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBf", "MainState,ArmS,Failsafe,Landed,Load"),
LOG_FORMAT(STAT, "BBBf", "MainState,ArmS,Failsafe,Load"),
LOG_FORMAT(VTOL, "fBBB", "Arsp,RwMode,TransMode,Failsafe"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@ -666,6 +670,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
LOG_FORMAT(LAND, "B", "Landed"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
LOG_FORMAT(TIME, "Q", "StartTime"),