diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 7abfeece1232..b6c96e633412 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -12,7 +12,6 @@ bool condition_home_position_valid # indicates a valid home position (a valid bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation 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 circuit_breaker_engaged_power_check @@ -31,5 +30,4 @@ bool offboard_control_loss_timeout # true if offboard is lost for bool rc_signal_found_once bool rc_input_blocked # set if RC input should be ignored temporarily bool vtol_transition_failure # Set to true if vtol transition failed -bool gps_failure # Set to true if a gps failure is detected bool usb_connected # status of the USB power supply diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ed7d7387e409..a14870f0d73c 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -95,12 +94,12 @@ class Commander : public control::SuperBlock, public ModuleBase bool handle_command(vehicle_status_s *status, const safety_s *safety, vehicle_command_s *cmd, actuator_armed_s *armed, home_position_s *home, vehicle_global_position_s *global_pos, - vehicle_local_position_s *local_pos, vehicle_attitude_s *attitude, orb_advert_t *home_pub, + vehicle_local_position_s *local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed); bool set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref); + bool set_alt_only_to_lpos_ref); void mission_init(); diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index e6879fd79ee3..2830a2afb089 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -448,42 +448,6 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep return success; } -static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool &lock_detected) -{ - bool success = true; - lock_detected = false; - int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - - //Wait up to 2000ms to allow the driver to detect a GNSS receiver module - px4_pollfd_struct_t fds[1]; - fds[0].fd = gpsSub; - fds[0].events = POLLIN; - - if (px4_poll(fds, 1, 2000) <= 0) { - success = false; - - } else { - struct vehicle_gps_position_s gps; - - if ((OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || - (hrt_elapsed_time(&gps.timestamp) > 1000000)) { - success = false; - } else if (gps.fix_type >= 3) { - lock_detected = true; - } - } - - //Report failure to detect module - if (!success) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); - } - } - - orb_unsubscribe(gpsSub); - return success; -} - static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required) { bool success = true; // start with a pass and change to a fail if any test fails @@ -779,14 +743,6 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check } } - /* ---- Global Navigation Satellite System receiver ---- */ - if (checkGNSS) { - bool lock_detected = false; - if (!gnssCheck(mavlink_log_pub, reportFailures, lock_detected)) { - failed = true; - } - } - /* ---- Navigation EKF ---- */ // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled int32_t estimator_type; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 90db3fd2da4a..d9bf5a392e4d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -97,7 +97,6 @@ #include #include #include -#include #include #include #include @@ -108,15 +107,12 @@ #include #include #include -#include #include #include #include -#include #include #include #include -#include #include #include #include @@ -136,8 +132,6 @@ typedef enum VEHICLE_MODE_FLAG VEHICLE_MODE_FLAG_ENUM_END=129, /* | */ } VEHICLE_MODE_FLAG; -static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ - /* Decouple update interval and hysteresis counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 10000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -678,7 +672,7 @@ bool Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local, vehicle_command_s *cmd, actuator_armed_s *armed_local, home_position_s *home, vehicle_global_position_s *global_pos, - vehicle_local_position_s *local_pos, vehicle_attitude_s *attitude, orb_advert_t *home_pub, + vehicle_local_position_s *local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ @@ -888,7 +882,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !home->manual_home) { - set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false); + set_home_position(*home_pub, *home, *local_pos, *global_pos, false); } } } @@ -925,7 +919,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety if (use_current) { /* use current position */ - if (set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) { + if (set_home_position(*home_pub, *home, *local_pos, *global_pos, false)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1150,7 +1144,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety bool Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref) + bool set_alt_only_to_lpos_ref) { if (!set_alt_only_to_lpos_ref) { //Need global and local position fix to be able to set home @@ -1175,8 +1169,7 @@ Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, home.y = localPosition.y; home.z = localPosition.z; - matrix::Eulerf euler = matrix::Quatf(attitude.q); - home.yaw = euler.psi(); + home.yaw = localPosition.yaw; //Play tune first time we initialize HOME if (!status_flags.condition_home_position_valid) { @@ -1344,13 +1337,6 @@ Commander::run() status_flags.condition_local_velocity_valid = false; status_flags.condition_local_altitude_valid = false; - // initialize gps failure to false if circuit breaker enabled - if (status_flags.circuit_breaker_engaged_gpsfailure_check) { - status_flags.gps_failure = false; - } else { - status_flags.gps_failure = true; - } - /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -1441,32 +1427,10 @@ Commander::run() int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position = {}; - /* Subscribe to attitude data */ - int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - struct vehicle_attitude_s attitude = {}; - /* Subscribe to land detector */ int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); land_detector.landed = true; - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - - /* Subscribe to GPS topic */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - gps_position.eph = FLT_MAX; - gps_position.epv = FLT_MAX; - - /* Subscribe to differential pressure topic */ - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - memset(&diff_pres, 0, sizeof(diff_pres)); - /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1489,8 +1453,6 @@ Commander::run() /* Subscribe to actuator controls (outputs) */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); /* Subscribe to vtol vehicle status topic */ int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); @@ -1585,8 +1547,6 @@ Commander::run() param_get(_param_rc_arm_hyst, &rc_arm_hyst); rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; - transition_result_t arming_ret; - int32_t datalink_loss_act = 0; int32_t rc_loss_act = 0; int32_t datalink_loss_timeout = 10; @@ -1643,7 +1603,7 @@ Commander::run() while (!should_exit()) { - arming_ret = TRANSITION_NOT_CHANGED; + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; /* update parameters */ bool params_updated = false; @@ -1875,12 +1835,6 @@ Commander::run() } } - orb_check(diff_pres_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - } - orb_check(system_power_sub, &updated); if (updated) { @@ -1916,8 +1870,6 @@ Commander::run() } } - check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status_flags.condition_airspeed_valid), &status_changed); - /* update safety topic */ orb_check(safety_sub, &updated); @@ -2099,14 +2051,6 @@ Commander::run() } } - /* update attitude estimate */ - orb_check(attitude_sub, &updated); - - if (updated) { - /* attitude changed */ - orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude); - } - /* update condition_local_altitude_valid */ check_valid(local_position.timestamp, posctl_nav_loss_delay, local_position.z_valid, &(status_flags.condition_local_altitude_valid), &status_changed); @@ -2331,64 +2275,6 @@ Commander::run() } } - /* - * Check GPS fix quality. Note that this check augments the position validity - * checks and adds an additional level of protection. - */ - - orb_check(gps_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - } - - /* Initialize map projection if gps is valid */ - if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) { - /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); - } - - /* check if GPS is ok */ - 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 - if (!armed.armed && gpsIsNoisy) { - if (!status_flags.gps_failure) { - mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy"); - set_tune_override(TONE_GPS_WARNING_TUNE); - - //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight - status_flags.gps_failure = true; - status_changed = true; - } - } - - // Check fix type and data freshness - if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status_flags.gps_failure && !gpsIsNoisy) { - status_flags.gps_failure = false; - status_changed = true; - if (status_flags.condition_home_position_valid) { - mavlink_log_critical(&mavlink_log_pub, "GPS fix regained"); - } - } - - } else if (!status_flags.gps_failure) { - status_flags.gps_failure = true; - status_changed = true; - if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - mavlink_log_critical(&mavlink_log_pub, "GPS fix lost"); - } - } - - } - /* start mission result check */ const auto prev_mission_instance_count = _mission_result_sub.get().instance_count; if (_mission_result_sub.update()) { @@ -2792,31 +2678,37 @@ Commander::run() } } - /* handle commands last, as the system needs to be updated to handle them */ + // engine failure detection + // TODO: move out of commander orb_check(actuator_controls_sub, &updated); if (updated) { - /* got command */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - /* Check engine failure * only for fixed wing for now */ if (!status_flags.circuit_breaker_engaged_enginefailure_check && - status.is_rotary_wing == false && - armed.armed && - ((actuator_controls.control[3] > ef_throttle_thres && - battery.current_a / actuator_controls.control[3] < - ef_current2throttle_thres) || - (status.engine_failure))) { - /* potential failure, measure time */ - if (timestamp_engine_healthy > 0 && - hrt_elapsed_time(×tamp_engine_healthy) > - ef_time_thres * 1e6f && - !status.engine_failure) { - status.engine_failure = true; - status_changed = true; - mavlink_log_critical(&mavlink_log_pub, "Engine Failure"); + !status.is_rotary_wing && !status.is_vtol && armed.armed) { + + actuator_controls_s actuator_controls = {}; + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); + + const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + const float current2throttle = battery.current_a / throttle; + + if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres)) + || status.engine_failure) { + + const float elapsed = hrt_elapsed_time(×tamp_engine_healthy) / 1e6f; + + /* potential failure, measure time */ + if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres) + && !status.engine_failure) { + + status.engine_failure = true; + status_changed = true; + + PX4_ERR("Engine Failure"); + } } } else { @@ -2872,8 +2764,7 @@ Commander::run() orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, - &attitude, &home_pub, &command_ack_pub, &status_changed)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub, &command_ack_pub, &status_changed)) { status_changed = true; } } @@ -2890,7 +2781,7 @@ Commander::run() internal_state.main_state != commander_state_s::MAIN_STATE_STAB && internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL && internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL && - ((status.data_link_lost && status_flags.gps_failure))) { + status.data_link_lost) { armed.force_failsafe = true; status_changed = true; @@ -2915,7 +2806,7 @@ Commander::run() internal_state.main_state == commander_state_s::MAIN_STATE_STAB || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status_flags.gps_failure))) { + status.rc_signal_lost) { armed.force_failsafe = true; status_changed = true; @@ -2942,7 +2833,7 @@ Commander::run() (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, false); } } else { if (status_flags.condition_home_position_valid) { @@ -2956,12 +2847,12 @@ Commander::run() if (home_dist_xy > local_position.epv * 2 || home_dist_z > local_position.eph * 2) { /* update when disarmed, landed and moved away from current home position */ - set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, false); } } } else { /* First time home position update - but only if disarmed */ - set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, false); } } @@ -2969,7 +2860,7 @@ Commander::run() * This allows home atitude to be used in the calculation of height above takeoff location when GPS * use has commenced after takeoff. */ if (!_home.valid_alt && local_position.z_global) { - set_home_position(home_pub, _home, local_position, global_position, attitude, true); + set_home_position(home_pub, _home, local_position, global_position, true); } } @@ -3161,11 +3052,9 @@ Commander::run() px4_close(offboard_control_mode_sub); px4_close(local_position_sub); px4_close(global_position_sub); - px4_close(gps_sub); px4_close(safety_sub); px4_close(cmd_sub); px4_close(subsys_sub); - px4_close(diff_pres_sub); px4_close(param_changed_sub); px4_close(battery_sub); px4_close(land_detector_sub); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3274893c1c30..50d9f72477e0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -71,7 +71,6 @@ using namespace DriverFramework; static const char reason_no_rc[] = "no RC"; static const char reason_no_offboard[] = "no offboard"; static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; -static const char reason_no_gps[] = "no gps"; static const char reason_no_local_position[] = "no local position"; static const char reason_no_global_position[] = "no global position"; static const char reason_no_datalink[] = "no datalink"; @@ -696,18 +695,7 @@ bool set_nav_state(struct vehicle_status_s *status, * - if we have vtol transition failure * - depending on datalink, RC and if the mission is finished */ - if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; @@ -747,16 +735,6 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else if (status->data_link_lost && data_link_loss_act_configured && !landed) { @@ -791,17 +769,6 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure) { - if (status->is_rotary_wing) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { // nothing to do - everything done in check_invalid_pos_nav_state } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 67594d76240e..19a961370036 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3720,7 +3720,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream _msg() { _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; - _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; } bool send(const hrt_abstime t) @@ -3770,9 +3770,6 @@ class MavlinkStreamExtendedSysState : public MavlinkStream } } } - - } else { - _msg.landed_state = MAV_LANDED_STATE_UNDEFINED; } }