From 59296e0a494eb89dcf9bf8f7322d3cde383afe49 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Sun, 15 May 2016 16:14:22 -0700 Subject: [PATCH] adding user parameters, simplifying dynamic gps filter, adding yaw smoothing --- .../mc_pos_control/mc_pos_control_main.cpp | 5 +- src/modules/navigator/follow_target.cpp | 103 ++++++++++-------- src/modules/navigator/follow_target.h | 22 ++-- src/modules/navigator/follow_target_params.c | 13 +++ 4 files changed, 87 insertions(+), 56 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d2431989fd..40f008fede 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1149,7 +1149,10 @@ void MulticopterPositionControl::control_auto(float dt) _pos_sp = pos_sp_s.edivide(scale); /* update yaw setpoint if needed */ - if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { + + if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 3fc64d3c15..8bff1033a3 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include "navigator.h" @@ -61,6 +62,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : _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), + _param_tracking_resp(this,"NAV_FT_RS", false), + _param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false), _follow_target_state(SET_WAIT_FOR_TARGET_POSITION), _follow_target_position(FOLLOW_FROM_BEHIND), _follow_target_sub(-1), @@ -70,10 +73,11 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : _last_update_time(0), _current_target_motion({}), _previous_target_motion({}), - _confidence(0.0F), - _confidence_ratio(0.0F), - _yaw_rate(0.0F) + _avg_cos_ratio(0.0F), + _yaw_rate(0.0F), + _responsiveness(0.0F) { + _avg_cos_ratio = 0.0F; _filtered_target_lat = _filtered_target_lon = 0.0F; updateParams(); _current_vel.zero(); @@ -99,6 +103,10 @@ void FollowTarget::on_activation() _follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get(); + _responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F); + + _yaw_auto_max = _param_yaw_auto_max.get(); + _follow_target_position = _param_tracking_side.get(); if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { @@ -187,52 +195,33 @@ void FollowTarget::on_active() float cos_ratio = (_target_position_delta * prev_position_delta)/(_target_position_delta.length() * prev_position_delta.length()); - if(_confidence >= RESPONSIVENESS) { - _confidence = 0.0F; // reset confidence level to 50/50 - } - - _confidence += cos_ratio; + _avg_cos_ratio = _responsiveness*_avg_cos_ratio + (1 - _responsiveness) * cos_ratio; - if (_confidence < -1.0F * RESPONSIVENESS) { - _confidence = -1.0F * RESPONSIVENESS; + if(_avg_cos_ratio < 0) { + _avg_cos_ratio = 0.0F; } - if (_confidence > RESPONSIVENESS) { - _confidence = RESPONSIVENESS; + if (_avg_cos_ratio > 0.0F) { + _filtered_target_position_delta = _target_position_delta*_avg_cos_ratio + _filtered_target_position_delta*(1.0F - _avg_cos_ratio); + } else { + _filtered_target_position_delta.zero(); } - _confidence_ratio = (_confidence + RESPONSIVENESS) / (RESPONSIVENESS * 2.0F); - - // track to the left, right, behind, or front - - _filtered_target_position_delta = (_target_position_delta*_confidence_ratio) + _filtered_target_position_delta*(1.0F - _confidence_ratio); - - // only track from a set side if we are 100% sure - // UAV is moving in a straight line - - if(_confidence_ratio >= 1.0F) { + if(_avg_cos_ratio >= .50F) { _target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset); } + } else { _filtered_target_position_delta.zero(); - _confidence_ratio = _confidence = 0.0F; + _avg_cos_ratio = 0.0F; } - 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_rate = (yaw_angle - _navigator->get_global_position()->yaw)/(dt_ms/1000.0F); - - _yaw_rate = _wrap_pi(_yaw_rate); - // update the average velocity of the target based on the position _est_target_vel = _filtered_target_position_delta / (dt_ms / 1000.0f); - _filtered_target_lat = (_current_target_motion.lat*(double)_confidence_ratio) + _filtered_target_lat*(double)(1 - _confidence_ratio); - _filtered_target_lon = (_current_target_motion.lon*(double)_confidence_ratio) + _filtered_target_lon*(double)(1 - _confidence_ratio); + _filtered_target_lat = (_current_target_motion.lat*(double)_avg_cos_ratio) + _filtered_target_lat*(double)(1 - _avg_cos_ratio); + _filtered_target_lon = (_current_target_motion.lon*(double)_avg_cos_ratio) + _filtered_target_lon*(double)(1 - _avg_cos_ratio); // are we within the target acceptance radius? // give a buffer to exit/enter the radius to give the velocity controller @@ -252,9 +241,35 @@ void FollowTarget::on_active() _step_vel = (_est_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); + + // if we are less than 3 meters from the target don't worry about trying to yaw + // just lock the yaw until we are a distance that makes sense + + if(_target_distance.length() > 3.0F) { + + // smooth yaw + // this really needs to control the yaw rate directly in the attitude pid controller + // but seems to work ok for now since that cannot be controlled directly in auto mode + // right now + + 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_rate = (yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); + + _yaw_rate = _wrap_pi(_yaw_rate); + + _yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max)*.50F; + + } else { + yaw_angle = _yaw_rate = NAN; + } } - warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f con = %3.6f", + + warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f yaw rate = %3.6f", (double) _step_vel(0), (double) _step_vel(1), (double) _current_vel(0), @@ -263,7 +278,7 @@ void FollowTarget::on_active() (double) _est_target_vel(1), (double) _target_distance.length(), _follow_target_state, - (double)_confidence_ratio, (double) _confidence); + (double)_avg_cos_ratio, (double) _yaw_rate); } // update state machine @@ -280,8 +295,7 @@ void FollowTarget::on_active() _current_vel = _est_target_vel; _filtered_target_lat = _current_target_motion.lat; _filtered_target_lon = _current_target_motion.lon; - // TODO: make max cruise speed less if very close to target - update_position_sp(true, true, _yaw_rate); + update_position_sp(true, true, NAN); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } @@ -294,13 +308,14 @@ void FollowTarget::on_active() if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle); if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { _current_vel += _step_vel; _last_update_time = current_time; } + set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle); + update_position_sp(true, false, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; @@ -361,15 +376,15 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa pos_sp_triplet->current.vx = _current_vel(0); pos_sp_triplet->current.vy = _current_vel(1); pos_sp_triplet->next.valid = false; - pos_sp_triplet->current.yawspeed_valid = true; + pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate); pos_sp_triplet->current.yawspeed = yaw_rate; _navigator->set_position_setpoint_triplet_updated(); } void FollowTarget::reset_target_validity() { - _confidence = -1*RESPONSIVENESS; - _confidence_ratio = 0.0F; + _yaw_rate = NAN; + _avg_cos_ratio = 0.0F; _previous_target_motion = {}; _current_target_motion = {}; _target_updates = 0; @@ -384,12 +399,12 @@ void FollowTarget::reset_target_validity() bool FollowTarget::target_velocity_valid() { - // need at least 5 continuous data points for velocity estimate + // need at least 2 continuous data points for velocity estimate return (_target_updates >= 2); } bool FollowTarget::target_position_valid() { - // need at least 2 continuous data points for position estimate + // need at least 1 continuous data points for position estimate return (_target_updates >= 1); } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 25f77b162d..b58e45b032 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -60,16 +60,12 @@ public: private: - static constexpr int TARGET_TIMEOUT_MS = 5000; + static constexpr int TARGET_TIMEOUT_MS = 2500; static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5; static constexpr int INTERPOLATION_PNTS = 20; - static constexpr float FF_K = .1F; + static constexpr float FF_K = .25F; static constexpr float OFFSET_M = 8; - // higher numbers slow down the time it takes to decide whether a target is moving - - static constexpr float RESPONSIVENESS = 8.0F; - enum FollowTargetState { TRACK_POSITION, TRACK_VELOCITY, @@ -103,9 +99,12 @@ private: Navigator *_navigator; - control::BlockParamFloat _param_min_alt; - control::BlockParamFloat _param_tracking_dist; - control::BlockParamInt _param_tracking_side; + control::BlockParamFloat _param_min_alt; + control::BlockParamFloat _param_tracking_dist; + control::BlockParamInt _param_tracking_side; + control::BlockParamFloat _param_tracking_resp; + control::BlockParamFloat _param_yaw_auto_max; + FollowTargetState _follow_target_state; int _follow_target_position; @@ -127,11 +126,12 @@ private: follow_target_s _current_target_motion; follow_target_s _previous_target_motion; - float _confidence; - float _confidence_ratio; + float _avg_cos_ratio; double _filtered_target_lat; double _filtered_target_lon; float _yaw_rate; + float _responsiveness; + float _yaw_auto_max; // Mavlink defined motion reporting capabilities diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c index 27ad5645e1..dd18461186 100644 --- a/src/modules/navigator/follow_target_params.c +++ b/src/modules/navigator/follow_target_params.c @@ -77,3 +77,16 @@ PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f); */ PARAM_DEFINE_INT32(NAV_FT_FS, 1); +/** + * Dynamic filtering algorithm responsiveness to target movement + * lower numbers increase the responsiveness to changing long lat + * but also ignore less noise + * + * @unit n/a + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.8f); +