From b6ee38ebe50b2bb8d1d7b36fe91e7bde457b982f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 17:24:01 -0500 Subject: [PATCH 1/7] commander remove gps receiver checks --- msg/vehicle_status_flags.msg | 1 - src/modules/commander/PreflightCheck.cpp | 44 ------------ src/modules/commander/commander.cpp | 71 ++++--------------- .../commander/state_machine_helper.cpp | 35 +-------- 4 files changed, 14 insertions(+), 137 deletions(-) diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 7abfeece1232..d53d5e2ea60b 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -31,5 +31,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/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..3af49db8c524 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -136,8 +136,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)) @@ -1344,13 +1342,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); @@ -1457,10 +1448,6 @@ Commander::run() /* 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)); @@ -2339,54 +2326,22 @@ Commander::run() 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; + vehicle_gps_position_s gps_position = {}; + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; - //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; - } - } + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - // 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"); - } - } + /* 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) { - } 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"); - } + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3f, hrt_absolute_time()); } - } /* start mission result check */ @@ -2890,7 +2845,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 +2870,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; 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 { From 50b7a88d6177562981d399f18d137556ecd02991 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:09:16 -0500 Subject: [PATCH 2/7] mavlink EXTENDED_SYS_STATE initialize landed and vtol state --- src/modules/mavlink/mavlink_messages.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) 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; } } From 6b715854513b9287459b96b20136b4a87749b538 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:14:02 -0500 Subject: [PATCH 3/7] commander remove globallocalconverter --- src/modules/commander/commander.cpp | 37 ----------------------------- 1 file changed, 37 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3af49db8c524..2234c6f751c3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -116,7 +116,6 @@ #include #include #include -#include #include #include #include @@ -1440,15 +1439,6 @@ Commander::run() 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)); - /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -2318,32 +2308,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) { - - vehicle_gps_position_s gps_position = {}; - gps_position.eph = FLT_MAX; - gps_position.epv = FLT_MAX; - - 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(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3f, hrt_absolute_time()); - } - } - /* start mission result check */ const auto prev_mission_instance_count = _mission_result_sub.get().instance_count; if (_mission_result_sub.update()) { @@ -3116,7 +3080,6 @@ 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); From 227da9f1fd10957eb85c85ec11c9dd95b9ae82e9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:16:03 -0500 Subject: [PATCH 4/7] commander remove unused differential pressure check --- msg/vehicle_status_flags.msg | 1 - src/modules/commander/commander.cpp | 15 --------------- 2 files changed, 16 deletions(-) diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index d53d5e2ea60b..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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2234c6f751c3..b73e205adf1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -97,7 +97,6 @@ #include #include #include -#include #include #include #include @@ -1439,11 +1438,6 @@ Commander::run() int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); land_detector.landed = true; - /* 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)); @@ -1852,12 +1846,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) { @@ -1893,8 +1881,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); @@ -3083,7 +3069,6 @@ Commander::run() 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); From 01f331bfd4d29fbf4f6aa2550af40051162537d9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:16:53 -0500 Subject: [PATCH 5/7] commander remove unused include --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b73e205adf1d..a06bd0c84539 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -107,7 +107,6 @@ #include #include #include -#include #include #include #include From 0ed5884ed63c1bb54ba643acce93cab1333dab59 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:23:12 -0500 Subject: [PATCH 6/7] commander remove vehicle_attitude usage --- src/modules/commander/Commander.hpp | 5 ++--- src/modules/commander/commander.cpp | 35 +++++++++-------------------- 2 files changed, 12 insertions(+), 28 deletions(-) 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/commander.cpp b/src/modules/commander/commander.cpp index a06bd0c84539..55b02488c186 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -110,7 +110,6 @@ #include #include #include -#include #include #include #include @@ -673,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 */ @@ -883,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); } } } @@ -920,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 { @@ -1145,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 @@ -1170,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) { @@ -1429,10 +1427,6 @@ 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; @@ -2061,14 +2055,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); @@ -2776,8 +2762,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; } } @@ -2846,7 +2831,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) { @@ -2860,12 +2845,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); } } @@ -2873,7 +2858,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); } } From eb59d7d644aad5618015f9fac86fb04328eeb2eb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Feb 2018 18:35:43 -0500 Subject: [PATCH 7/7] commander only copy actuator_controls if engine failure enabled --- src/modules/commander/commander.cpp | 48 +++++++++++++++-------------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55b02488c186..d9bf5a392e4d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1453,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)); @@ -1549,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; @@ -1607,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; @@ -2682,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 {