diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1c1d2e7a18..9c7067b274 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9593ffb313..59e1bc5c62 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4304ef18d4..5d8f5b49ca 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ca5a826e19..551e8bac77 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -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_ */ diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index cd42d089c7..b48d984737 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -75,11 +75,11 @@ #include #include #include -#include -#include +#include #include #include #include +#include #include @@ -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); diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 650f054993..62e7f3c9bf 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -50,9 +50,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -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. */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d34bdb369c..df5df703fe 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b5e49d67c8..e7cf9cc851 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -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(); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 880b847aa1..04ed7f0856 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include #include #include @@ -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> ¤t_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> ¤t_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> ¤t_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) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5fcc24690a..69cabf91d9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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 */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a34b5d5cb1..82e494c99e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -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()); } } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 990e46bc7d..ac53b0ea23 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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(); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index eca686a9b3..432122c2d8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 067aa7b556..b4f0832429 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,6 +57,7 @@ #include #include #include +#include #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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 871e54c656..4e10c848aa 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7b2c46a6c3..5905047434 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -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"); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c89cab2fd4..c971fdf505 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 * @author Anton Babushkin * @author Ban Siesta + * @author Julian Oes */ #include @@ -112,6 +113,7 @@ #include #include #include +#include #include #include @@ -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 */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5505998400..95dfa2d7e2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"),