velocity smoothing

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);
+                _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);
-            convert_mission_item_to_sp();
+
+            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 = true;
+    pos_sp_triplet->previous.valid = false;
     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();