From 55f023b77111d440c61c096026077ff3d1685ca8 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Mon, 11 Apr 2016 23:52:22 -0700 Subject: [PATCH] adding params for offset, and side to track, fixing velocity tracking bug --- src/modules/navigator/follow_target.cpp | 39 +++++++++++++------- src/modules/navigator/follow_target.h | 4 ++ src/modules/navigator/follow_target_params.c | 24 ++++++++++++ 3 files changed, 54 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 0e259a683c..e3e7a4fed8 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -47,7 +47,6 @@ #include #include -#include #include #include @@ -61,13 +60,16 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), _param_min_alt(this, "NAV_MIN_FT_HT", false), + _param_tracking_dist(this,"NAV_FT_DST", false), + _param_tracking_side(this,"NAV_FT_FS", false), _follow_target_state(WAIT_FOR_TARGET_POSITION), _follow_target_position(FOLLOW_FROM_BEHIND), _follow_target_sub(-1), _step_time_in_ms(0.0f), + _follow_offset(OFFSET_M), _target_updates(0), _last_update_time(0), - _current_target_motion( {}), + _current_target_motion({}), _previous_target_motion({}) { updateParams(); @@ -91,6 +93,18 @@ void FollowTarget::on_inactive() void FollowTarget::on_activation() { + updateParams(); + + _follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get(); + + _follow_target_position = (FollowTargetPosition) _param_tracking_side.get(); + + if((_follow_target_position > 3) || (_follow_target_position < 0)) { + _follow_target_position = FOLLOW_FROM_BEHIND; + } + + _rot_matrix = (_follow_position_matricies[_follow_target_position]); + if (_follow_target_sub < 0) { _follow_target_sub = orb_subscribe(ORB_ID(follow_target)); } @@ -120,8 +134,7 @@ void FollowTarget::on_active() orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion); - } else if (((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS - && target_velocity_valid()) { + } else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } @@ -157,9 +170,9 @@ void FollowTarget::on_active() dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); - // protect from to small of a dt + // ignore a small dt - if (dt_ms > 10) { + if (dt_ms > 10.0F) { // get last gps known reference for target @@ -169,14 +182,14 @@ void FollowTarget::on_active() map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), &(target_position(1))); - // only calculate new velocity if target has moved + // only calculate offset rotation if target has moved if(target_position.length() > 0.0F) { // track to the left, right, behind, or front - _target_position_offset = _rot_matrix * (target_position.normalized() * OFFSET_M); - + _target_position_offset = _rot_matrix * (target_position.normalized() * _follow_offset); + } // update the average velocity of the target based on the position _target_vel = target_position / (dt_ms / 1000.0f); @@ -192,7 +205,6 @@ void FollowTarget::on_active() _step_vel = (_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); _step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS; - } } } @@ -200,8 +212,10 @@ void FollowTarget::on_active() if (target_position_valid() && updated) { - yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _current_target_motion.lat, _current_target_motion.lon); + yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _current_target_motion.lat, + _current_target_motion.lon); // warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d", // target_motion.lat, @@ -258,7 +272,6 @@ void FollowTarget::on_active() } case WAIT_FOR_TARGET_POSITION: { - // Climb to the minimum altitude // and wait until a position is received diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 019c93c293..b00136b5ce 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -99,11 +99,15 @@ private: Navigator *_navigator; control::BlockParamFloat _param_min_alt; + control::BlockParamFloat _param_tracking_dist; + control::BlockParamInt _param_tracking_side; + FollowTargetState _follow_target_state; FollowTargetPosition _follow_target_position; int _follow_target_sub; float _step_time_in_ms; + float _follow_offset; uint64_t _target_updates; diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c index 3796f638e4..27ad5645e1 100644 --- a/src/modules/navigator/follow_target_params.c +++ b/src/modules/navigator/follow_target_params.c @@ -53,3 +53,27 @@ * @group Follow target */ PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f); + +/** + * Distance to follow target from + * + * The distance in meters to follow the target at + * + * @unit meters + * @min 1.0 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f); + +/** + * Side to follow target from + * + * The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3) + * + * @unit n/a + * @min 0 + * @max 3 + * @group Follow target + */ +PARAM_DEFINE_INT32(NAV_FT_FS, 1); +