velocity smoothing

msg/position_setpoint.msg

@@ -7,6 +7,7 @@ uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
 uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
+uint8 SETPOINT_TYPE_FOLLOW_TARGET=7  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
 
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -720,6 +720,7 @@ MulticopterPositionControl::reset_pos_sp()
 		// we have logic in the main function which chooses the velocity setpoint such that the attitude setpoint is
 		// continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting
 		// position in a special way. In position control mode the position will be reset anyway until the vehicle has reduced speed.
+
 		_pos_sp(0) = _pos(0);
 		_pos_sp(1) = _pos(1);
 	}
@@ -1023,7 +1024,7 @@ 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 && 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) {
@@ -1380,7 +1381,13 @@ MulticopterPositionControl::task_main()
 				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)) {
+//				float sp_vel = sqrtf(_pos_sp_triplet.current.vx*_pos_sp_triplet.current.vx +
+//				                     _pos_sp_triplet.current.vy*_pos_sp_triplet.current.vy);
+
+                if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
+                    _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;

src/modules/navigator/follow_target.cpp

@@ -52,16 +52,27 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/follow_target.h>
+#include <lib/geo/geo.h>
 #include "navigator.h"
 
+using namespace matrix;
+
 FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
     MissionBlock(navigator, name),
 	_navigator(navigator),
 	_tracker_motion_position_sub(-1),
-	_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
+	_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
+	gps_valid(false),
+	_last_message_time(0),
+	_index(0)
 {
     /* load initial params */
     updateParams();
+    follow_target_reached = false;
+    pos_pair[0].setZero();
+    pos_pair[1].setZero();
+    _current_vel.setZero();
+    _steps = 0.0f;
 }
 
 FollowTarget::~FollowTarget()
@@ -71,11 +82,15 @@ FollowTarget::~FollowTarget()
 void
 FollowTarget::on_inactive()
 {
+    gps_valid = false;
 }
 
 void
 FollowTarget::on_activation()
 {
+    Vector2f vel;
+    vel.setZero();
+
 	if(_tracker_motion_position_sub < 0) {
 		_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
 	}
@@ -84,35 +99,118 @@ FollowTarget::on_activation()
 
     set_loiter_item(&_mission_item, _param_min_alt.get());
 
-    convert_mission_item_to_sp();
+    convert_mission_item_to_sp(vel);
 }
 
 void
 FollowTarget::on_active() {
     follow_target_s target;
     bool updated;
+   struct map_projection_reference_s target_ref;
+    float target_dist_x,target_dist_y;
+    uint64_t current_time = hrt_absolute_time();
+
+
 
     orb_check(_tracker_motion_position_sub, &updated);
 
     if (updated) {
         if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
+
+            if ((gps_valid == false) ) {
+                gps_pair(0) = target.lat;
+                gps_pair(1) = target.lon;
+                gps_valid = true;
+
+                return;
+            }
+
+            // get distance to target
+
+            map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+            map_projection_project(&target_ref, target.lat, target.lon, &target_dist_x, &target_dist_y);
+
+            follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
+
+            if(follow_target_reached == false && gps_valid == true) {
+                Vector2f go_to_vel;
+
+                warnx("WP not reached lat %f (%f) lon %f (%f), %f",  target.lat,  (double)_navigator->get_global_position()->lat,  (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
+
                 set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
-            convert_mission_item_to_sp();
+                _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+                convert_mission_item_to_sp(go_to_vel);
+                _navigator->get_position_setpoint_triplet()->previous.valid = false;
+                _navigator->set_position_setpoint_triplet_updated();
+                //target_vel
+                return;
+            }
+
+            // get last gps reference
+
+            map_projection_init(&target_ref, gps_pair(0), gps_pair(1));
+            map_projection_project(&target_ref, target.lat, target.lon, &(pos_pair[1](0)), &(pos_pair[1](1)));
+
+            target_vel = pos_pair[1]/((double)(current_time - _last_message_time) * 1e-6);
+
+
+            warnx("tracker x %f y %f m, x %f m/s, y %f m/s gs = %f m/s dis = %f m",
+                    (double) pos_pair[1](0),
+                    (double) pos_pair[1](1),
+                    (double) target_vel(0),
+                    (double) target_vel(1),
+                    (double) sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1)),
+                    (double) sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
+
+            set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
+
+            gps_pair(0) = target.lat;
+            gps_pair(1) = target.lon;
+
+            _last_message_time = current_time;
+
+            _steps =  fabs((double)((sqrtf(_current_vel(0)*_current_vel(0) + _current_vel(1)*_current_vel(1)) -
+                      sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double)10.0f;
+            //pos_pair[0] = pos_pair[1];
         }
     }
+        if ((((double) (current_time - _last_publish_time) * 1e-3) > 100) && (follow_target_reached == true)) {
+
+            if(_current_vel(0) <= target_vel(0)) {
+               _current_vel(0) += _steps;
+            }
+
+            if(_current_vel(0) >= target_vel(0)) {
+                _current_vel(0) -= _steps;
+            }
+
+            if(_current_vel(1) <= target_vel(1)) {
+               _current_vel(1) += _steps;
+            }
+
+            if(_current_vel(1) >= target_vel(1)) {
+                _current_vel(1) -= _steps;
+            }
+            convert_mission_item_to_sp(_current_vel);
+        _navigator->set_position_setpoint_triplet_updated();
+        warnx("updating x %f y %f %f", (double)_current_vel(0), (double)_current_vel(1), (double) _steps);
+        _last_publish_time = current_time;
+    }
+
 }
 
 void
-FollowTarget::convert_mission_item_to_sp() {
+FollowTarget::convert_mission_item_to_sp(Vector2f & vel) {
 
     /* convert mission item to current setpoint */
     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
     // activate line following in pos control
-
+    pos_sp_triplet->previous.valid = false;
-    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);
+    pos_sp_triplet->current.vx = vel(0);
+    pos_sp_triplet->current.vy = vel(1);
     pos_sp_triplet->next.valid = false;
 
     _navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/follow_target.h

@@ -45,11 +45,10 @@
 
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include <lib/matrix/matrix/Vector2.hpp>
 
 class FollowTarget : public MissionBlock
 {
-	Navigator *_navigator;
-	int		_tracker_motion_position_sub;	/**< tracker motion subscription */
 
 public:
     FollowTarget(Navigator *navigator, const char *name);
@@ -63,6 +62,26 @@ public:
     virtual void on_active();
 
 private:
+    Navigator *_navigator;
+    int     _tracker_motion_position_sub;   /**< tracker motion subscription */
     control::BlockParamFloat _param_min_alt;
-    void convert_mission_item_to_sp();
+    matrix::Vector2f pos_pair[2];
+    matrix::Vector2f gps_pair;
+    bool gps_valid;
+    uint64_t _last_message_time;
+    uint64_t _last_publish_time;
+    float _steps;
+    bool follow_target_reached;
+    int _index;
+
+    struct pos_history_s{
+       struct position_setpoint_triplet_s pos_history[6];
+       uint64_t wp_time;
+    };
+
+    pos_history_s wp_history[6];
+    int wp_cnt;
+    matrix::Vector2f _current_vel;
+    matrix::Vector2f target_vel;
+    void convert_mission_item_to_sp(matrix::Vector2f & vel);
 };

src/modules/navigator/mission_block.cpp

@@ -228,7 +228,7 @@ MissionBlock::is_mission_item_reached()
 
 	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
-
+	    return true;
 		if (_time_first_inside_orbit == 0) {
 			_time_first_inside_orbit = now;
 
@@ -239,7 +239,7 @@ MissionBlock::is_mission_item_reached()
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
-		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+		if ((now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f)) {
 			return true;
 		}
 	}
@@ -376,6 +376,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 			sp->disable_mc_yaw_control = true;
 		}
 		break;
+	case NAV_CMD_FOLLOW_TARGET:
+	    sp->type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
+	    break;
 
 	default:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;

src/modules/navigator/navigation.h

@@ -62,6 +62,7 @@ enum NAV_CMD {
 	NAV_CMD_ROI = 80,
 	NAV_CMD_PATHPLANNING = 81,
 	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
+	NAV_CMD_GOTO_TAREGT = 195,
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,