From bb4decfa8b6cbf03ee4408ec6554f72565277de7 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 12 Feb 2016 13:55:16 +0100 Subject: [PATCH] implemented basic heading timeout for waypoint acceptance, added parameter for yaw error on waypoint heading acceptance, set yaw timeout for vtol default --- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 1 + .../SITL/init/rcS_gazebo_standard_vtol | 1 + src/modules/navigator/mission_block.cpp | 14 +++++++++++- src/modules/navigator/mission_block.h | 7 +++--- src/modules/navigator/mission_params.c | 22 +++++++++++++++++++ 5 files changed, 41 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 0e28efd3b5..8d076ff5cd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -47,6 +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 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 c93af9204f..2fcf01e12d 100644 --- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol +++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol @@ -32,6 +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 rgbledsim start tone_alarm start gyrosim start diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index cdfb2e96c8..1b33f4b438 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -68,9 +69,12 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _action_start(0), + _time_wp_reached(0), _actuators{}, _actuator_pub(nullptr), _cmd_pub(nullptr), + _param_yaw_timeout(this, "MIS_YAW_TMT", false), + _param_yaw_err(this, "MIS_YAW_ERR", false), _param_vtol_wv_land(this, "VT_WV_LND_EN", false), _param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false) { @@ -174,6 +178,11 @@ MissionBlock::is_mission_item_reached() _waypoint_position_reached = true; } } + + if (_waypoint_position_reached) { + // reached just now + _time_wp_reached = now; + } } /* Check if the waypoint and the requested yaw setpoint. */ @@ -186,7 +195,9 @@ 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); - if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) + || (_param_yaw_timeout.get() >= -FLT_EPSILON && + now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _waypoint_yaw_reached = true; } @@ -221,6 +232,7 @@ MissionBlock::reset_mission_item_reached() _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_first_inside_orbit = 0; + _time_wp_reached = 0; } void diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index dac3b9b712..2f69294d1f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -45,9 +45,6 @@ #include -#include -#include - #include #include #include @@ -128,10 +125,14 @@ protected: bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; hrt_abstime _action_start; + hrt_abstime _time_wp_reached; actuator_controls_s _actuators; orb_advert_t _actuator_pub; orb_advert_t _cmd_pub; + + control::BlockParamFloat _param_yaw_timeout; + control::BlockParamFloat _param_yaw_err; control::BlockParamInt _param_vtol_wv_land; control::BlockParamInt _param_vtol_wv_loiter; }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 1ef0c57e3e..fdc8cd46ff 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -107,3 +107,25 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1); * @group Mission */ PARAM_DEFINE_INT32(MIS_YAWMODE, 1); + +/** + * Time in seconds we wait on reaching target heading at a waypoint. + * + * 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. + * + * @min -1 + * @max 20 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); + +/** + * Max yaw error in degree needed for waypoint heading acceptance. + * + * @min 0 + * @max 90 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);