From eb5b8a32ee66aeff5edf78c4374b3837bff354a2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 13 Feb 2016 01:08:07 +0100 Subject: [PATCH] transition alignment will force heading now and go to RTL if it cannot reach it in time, handle mission failure correctly, reset after mission update, issue message with actual problem --- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 2 +- msg/mission_result.msg | 3 ++- msg/vehicle_status.msg | 2 +- src/modules/commander/commander.cpp | 9 +++++++++ src/modules/commander/state_machine_helper.cpp | 2 ++ src/modules/navigator/mission.cpp | 7 +++++++ src/modules/navigator/mission_block.cpp | 10 +++++++++- src/modules/navigator/mission_params.c | 3 ++- src/modules/navigator/navigation.h | 1 + src/modules/navigator/navigator.h | 2 ++ src/modules/navigator/navigator_main.cpp | 10 ++++++++++ 11 files changed, 46 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 8d076ff5cd..177d995f64 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -47,7 +47,7 @@ then param set MPC_LAND_SPEED 0.7 param set MPC_Z_VEL_MAX 1.5 param set MPC_XY_VEL_MAX 4.0 - param set MIS_YAW_TMT 10 + param set MIS_YAW_TMT 5 fi set PWM_DISARMED 900 diff --git a/msg/mission_result.msg b/msg/mission_result.msg index ac4d32f559..769015d4c0 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -6,7 +6,8 @@ bool warning # true if mission is valid, but has potentially problematic items bool reached # true if mission has been reached bool finished # true if mission has been completed bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode -bool flight_termination # true if the navigator demands a flight termination from the commander app +bool flight_termination # true if the navigator demands a flight termination from the commander app bool item_do_jump_changed # true if the number of do jumps remaining has changed uint32 item_changed_index # indicate which item has changed uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item +bool mission_failure # true if the mission cannot continue or be completed for some reason diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e7ddca892a..f9b0bdbd69 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -154,7 +154,7 @@ bool vtol_transition_failure # Set to true if vtol transition failed bool vtol_transition_failure_cmd # Set to true if vtol transition failure mode is commanded bool gps_failure # Set to true if a gps failure is detected bool gps_failure_cmd # Set to true if a gps failure mode is commanded - +bool mission_failure # Set to true if mission could not continue/finish bool barometer_failure # Set to true if a barometer failure is detected bool offboard_control_signal_found_once diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2be0d1d2cb..157762ec7c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2043,6 +2043,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (status.mission_failure != mission_result.mission_failure) { + status.mission_failure = mission_result.mission_failure; + status_changed = true; + + if (status.mission_failure) { + mavlink_log_critical(mavlink_fd, "mission cannot be completed"); + } + } } /* start geofence result check */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 966dc3b189..742776140c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -659,6 +659,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->vtol_transition_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } else if (status->mission_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d77c3be5cf..451538b066 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -219,6 +219,8 @@ Mission::update_onboard_mission() // XXX check validity here as well _navigator->get_mission_result()->valid = true; + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->mission_failure = false; _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -263,6 +265,10 @@ Mission::update_offboard_mission() _navigator->get_vstatus()->condition_landed); _navigator->get_mission_result()->valid = !failed; + if (!failed) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->mission_failure = false; + } _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -510,6 +516,7 @@ Mission::set_mission_items() _navigator->get_global_position()->lon, mission_item_next_position.lat, mission_item_next_position.lon); + _mission_item.force_heading = true; } /* yaw is aligned now */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1b33f4b438..25370649b7 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -196,9 +196,17 @@ MissionBlock::is_mission_item_reached() float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) + /* check if we should accept heading after a timeout, 0 means accept instantly */ || (_param_yaw_timeout.get() >= -FLT_EPSILON && now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { - _waypoint_yaw_reached = true; + + /* if heading needs to be reached but we got here because of the timeout, abort mission */ + if (_mission_item.force_heading && !(fabsf(yaw_err) < math::radians(_param_yaw_err.get()))) { + _navigator->set_mission_failure("unable to reach heading within timeout"); + + } else { + _waypoint_yaw_reached = true; + } } } else { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index fdc8cd46ff..422a72e933 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -113,7 +113,8 @@ PARAM_DEFINE_INT32(MIS_YAWMODE, 1); * * Prevents missions getting blocked at waypoints because it cannot reach target yaw. * Mainly useful for VTOLs that have less yaw authority and might not reach target - * yaw in wind. Disabled by default. Can be set to 0 if it should not care for yaw on waypoints. + * yaw in wind. Disabled by default. If set to 0 it will not wait for yaw to align, + * however VTOL transitions will abort if heading is not reached. * * @min -1 * @max 20 diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 4f58256832..873e66ead0 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -105,6 +105,7 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ int8_t frame; /**< mission frame ***/ + bool force_heading; /**< heading needs to be reached ***/ }; #pragma pack(pop) #include diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 514705a68a..36fda30c62 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -168,6 +168,8 @@ public: void increment_mission_instance_count() { _mission_instance_count++; } + void set_mission_failure(const char *reason); + private: bool _task_should_exit; /**< if true, sensor task should exit */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 68e24ebd20..f3e69d4711 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -750,3 +750,13 @@ Navigator::publish_att_sp() _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } + +void +Navigator::set_mission_failure(const char* reason) +{ + if (!_mission_result.mission_failure) { + _mission_result.mission_failure = true; + set_mission_result_updated(); + mavlink_log_critical(_mavlink_fd, "%s", reason); + } +}