diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 177d995f64..8d076ff5cd 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 5 + param set MIS_YAW_TMT 10 fi set PWM_DISARMED 900 diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol index 2fcf01e12d..e135c48f3d 100644 --- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol +++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol @@ -32,7 +32,7 @@ param set SENS_BOARD_ROT 8 param set COM_RC_IN_MODE 1 param set NAV_ACC_RAD 3.0 param set MPC_TKO_SPEED 1.0 -param set MIS_YAW_TMT 5 +param set MIS_YAW_TMT 10 rgbledsim start tone_alarm start gyrosim start diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 451538b066..929e08ae02 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -505,10 +505,7 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_ALIGN; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; - _mission_item.altitude_is_relative = false; + copy_positon_if_valid(_mission_item, pos_sp_triplet->current); _mission_item.autocontinue = true; _mission_item.time_inside = 0; _mission_item.yaw = get_bearing_to_next_waypoint( @@ -541,10 +538,7 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = pos_sp_triplet->current.lat; - _mission_item.lon = pos_sp_triplet->current.lon; - _mission_item.altitude = pos_sp_triplet->current.alt; - _mission_item.altitude_is_relative = false; + copy_positon_if_valid(_mission_item, pos_sp_triplet->current); _mission_item.autocontinue = true; _mission_item.time_inside = 0; } @@ -603,6 +597,23 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint) +{ + if (setpoint.valid) { + _mission_item.lat = setpoint.lat; + _mission_item.lon = setpoint.lon; + _mission_item.altitude = setpoint.alt; + + } else { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + + _mission_item.altitude_is_relative = false; +} + bool Mission::do_need_takeoff() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index dff8d32540..5d491df1c7 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -119,6 +119,11 @@ private: */ void set_mission_items(); + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint); + /** * Returns true if we need to do a takeoff at the current state */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 25370649b7..a6d76e5c25 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -195,18 +195,17 @@ MissionBlock::is_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ 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)) { + || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { + _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; - } + /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ + if (!_waypoint_yaw_reached && _mission_item.force_heading && + _param_yaw_timeout.get() >= FLT_EPSILON && + now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f) { + _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 422a72e933..5de180bf5c 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -109,12 +109,12 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1); PARAM_DEFINE_INT32(MIS_YAWMODE, 1); /** - * Time in seconds we wait on reaching target heading at a waypoint. + * Time in seconds we wait on reaching target heading at a waypoint if it is forced. * - * Prevents missions getting blocked at waypoints because it cannot reach target yaw. + * If set > 0 it will ignore the target heading for normal waypoint acceptance. If the + * waypoint forces the heading the timeout will matter. For example on VTOL forwards transiton. * Mainly useful for VTOLs that have less yaw authority and might not reach target - * 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. + * yaw in wind. Disabled by default. * * @min -1 * @max 20