fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator remove unnecessary calls to updateParams

Browse Source 
- the SuperBlock already does this
This commit is contained in:
Daniel Agar
2018-01-14 12:53:53 +01:00
committed by Lorenz Meier
parent 6227139df1
commit 25c7a03a8b
24 changed files with 107 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+10 -12
View File
@@ -55,11 +55,7 @@
#include <uORB/topics/vtol_vehicle_status.h>
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
_param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false),
_param_reverse_delay(this, "VT_B_REV_DEL", false)
NavigatorMode(navigator, name)
{
}
@@ -293,9 +289,11 @@ MissionBlock::is_mission_item_reached()
float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx +
_navigator->get_local_position()->vy * _navigator->get_local_position()->vy);
if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) {
mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() *
velocity;
const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration();
const float reverse_delay = _navigator->get_vtol_reverse_delay();
if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) {
mission_acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity;
}
@@ -328,16 +326,16 @@ MissionBlock::is_mission_item_reached()
float yaw_err = _wrap_pi(_mission_item.yaw - cog);
/* 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())
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
if (fabsf(yaw_err) < math::radians(_navigator->get_yaw_threshold())
|| (_navigator->get_yaw_timeout() >= 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->get_yaw_timeout() >= FLT_EPSILON) &&
(now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) {
_navigator->set_mission_failure("unable to reach heading within timeout");
}