fixed position tracking overshoot

src/modules/mavlink/mavlink_receiver.cpp

@@ -1647,8 +1647,6 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
     } else {
         orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic);
     }
-
-    warnx("new msg recieved");
 }
 
 void

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1023,7 +1023,10 @@ void MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION  || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) && previous_setpoint_valid) {
+		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION  ||
+		     _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) &&
+		      previous_setpoint_valid) {
+
 			/* follow "previous - current" line */
 
 			if ((curr_sp - prev_sp).length() > MIN_DIST) {
@@ -1372,30 +1375,52 @@ MulticopterPositionControl::task_main()
 					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
 				}
 
-				// if there is a velocity sp available, use it
+				// do not go slower than the follow target velocity when position tracking is active
 
-                if(_pos_sp_triplet.current.velocity_valid) {
+                if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
-                    _vel_sp(0) += _pos_sp_triplet.current.vx;
+                   _pos_sp_triplet.current.velocity_valid) {
-                    _vel_sp(1) += _pos_sp_triplet.current.vy;
+
+                    math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
+
+                    float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
+
+                    // only override velocity set points when uav is traveling in same direction as target and vector component
+                    // is greater than calculated position set point velocity component
+
+                    if(cos_ratio > 0) {
+                        ft_vel *= (cos_ratio);
+                        // min speed a little faster than target vel
+                        ft_vel += ft_vel.normalized()*1.5f;
+                    } else {
+                        ft_vel.zero();
+                    }
+
+                    _vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
+                    _vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
+                }
+
+                // Ignore position control calculated vel set points
+
+                if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY &&
+                   _pos_sp_triplet.current.velocity_valid) {
+                    _vel_sp(0) = _pos_sp_triplet.current.vx;
+                    _vel_sp(1) = _pos_sp_triplet.current.vy;
+                }
+
+                /* make sure velocity setpoint is saturated in xy*/
+
+                float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +  _vel_sp(1) * _vel_sp(1));
+
+                if (vel_norm_xy > _params.vel_max(0)) {
+                    /* note assumes vel_max(0) == vel_max(1) */
+                    _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
+                    _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
                 }
 
 				if (_run_alt_control) {
 					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
 				}
 
-				/* make sure velocity setpoint is saturated in xy*/
-				float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +  _vel_sp(1) * _vel_sp(1));
-
-                if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
-                    _pos_sp_triplet.current.velocity_valid) {
-                    _vel_sp(0) = _pos_sp_triplet.current.vx;
-                    _vel_sp(1) = _pos_sp_triplet.current.vy;
-                } else if (vel_norm_xy > _params.vel_max(0)) {
-                    /* note assumes vel_max(0) == vel_max(1) */
-                    _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
-                    _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
-                }
-
 				/* make sure velocity setpoint is saturated in z*/
 				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
 

src/modules/navigator/follow_target.cpp

@@ -101,8 +101,7 @@ void FollowTarget::pause()
 {
     math::Vector<3> vel(0, 0, 0);
 
-    _current_vel(0) = 0;
+    _current_vel.zero();
-    _current_vel(1) = 0;
 
     set_loiter_item(&_mission_item, _param_min_alt.get());
 
@@ -172,14 +171,11 @@ void FollowTarget::on_active()
 
 void FollowTarget::track_target_position()
 {
-
     set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
-    _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 
     // keep the current velocity updated
 
-    _current_vel(0) = _navigator->get_global_position()->vel_n;
+    _current_vel =  _target_vel;
-    _current_vel(1) = _navigator->get_global_position()->vel_e;
 }
 
 void FollowTarget::track_target_velocity()
@@ -249,8 +245,7 @@ void FollowTarget::update_target_velocity()
 
     // calculate distance the target has moved
 
-    map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),
+    map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), &(target_position(1)));
-            &(target_position(1)));
 
     // update the average velocity of the target based on the position
 
@@ -279,6 +274,18 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
     pos_sp_triplet->previous.valid = true;
     pos_sp_triplet->previous = pos_sp_triplet->current;
     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+    switch(_follow_target_state) {
+    case TRACK_POSITION:
+        pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
+        break;
+    case TRACK_VELOCITY:
+        pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
+        break;
+    default:
+        break;
+    }
+
     pos_sp_triplet->current.vx = vel(0);
     pos_sp_triplet->current.vy = vel(1);
     pos_sp_triplet->current.velocity_valid = true;

src/modules/navigator/follow_target.h

@@ -60,7 +60,7 @@ public:
 
 private:
 
-    static constexpr int TARGET_TIMEOUT_S = 10;
+    static constexpr int TARGET_TIMEOUT_S = 5;
     static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
     static constexpr int INTERPOLATION_PNTS = 20;
     static constexpr float FF_K = .15f;

src/modules/navigator/mission_block.cpp

@@ -374,9 +374,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
 			sp->disable_mc_yaw_control = true;
 		}
-		break;
+		break;// get rid of this
 	case NAV_CMD_FOLLOW_TARGET:
-	    sp->type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
+	    sp->type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
 	    break;
 
 	default: