adding user parameters, simplifying dynamic gps filter, adding yaw smoothing

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;
 		}
 

src/modules/navigator/follow_target.cpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/follow_target.h>
 #include <lib/geo/geo.h>
+#include <lib/mathlib/math/Limits.hpp>
 
 #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),
+	_avg_cos_ratio(0.0F),
-	_confidence_ratio(0.0F),
+	_yaw_rate(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) {
+				_avg_cos_ratio = _responsiveness*_avg_cos_ratio + (1 - _responsiveness) * cos_ratio;
-					_confidence = 0.0F; // reset confidence level to 50/50
-				}
-
-				_confidence += cos_ratio;
 
-				if (_confidence < -1.0F * RESPONSIVENESS) {
+				if(_avg_cos_ratio < 0) {
-					_confidence = -1.0F * RESPONSIVENESS;
+					_avg_cos_ratio = 0.0F;
 				}
 
-				if (_confidence > RESPONSIVENESS) {
+				if (_avg_cos_ratio > 0.0F) {
-					_confidence = RESPONSIVENESS;
+					_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);
+				if(_avg_cos_ratio  >= .50F) {
-
-				// 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) {
 					_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_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)_confidence_ratio) + _filtered_target_lon*(double)(1 - _confidence_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, NAN);
-				update_position_sp(true, true, _yaw_rate);
 			} 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;
+	_yaw_rate = NAN;
-	_confidence_ratio = 0.0F;
+	_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);
 }

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_min_alt;
-	control::BlockParamFloat _param_tracking_dist;
+	control::BlockParamFloat 	_param_tracking_dist;
-	control::BlockParamInt _param_tracking_side;
+	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 _avg_cos_ratio;
-	float _confidence_ratio;
 	double _filtered_target_lat;
 	double _filtered_target_lon;
 	float _yaw_rate;
+	float _responsiveness;
+	float _yaw_auto_max;
 
 	// Mavlink defined motion reporting capabilities
 

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);
+