yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

updated timeout logic to work only on waypoints with forced headings, updated param docs accordingly

Browse Source
This commit is contained in:
Andreas Antener
2016-02-14 11:57:16 +01:00
parent eb5b8a32ee
commit 570fb97163
6 changed files with 39 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@@ -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

2
posix-configs/SITL/init/rcS_gazebo_standard_vtol
View File

@@ -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

27
src/modules/navigator/mission.cpp
View File

@@ -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()
{

5
src/modules/navigator/mission.h
View File

@@ -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
*/

17
src/modules/navigator/mission_block.cpp
View File

@@ -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)) {
/* 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 {
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
_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 {

8
src/modules/navigator/mission_params.c
View File

@@ -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