code clean up complete

src/modules/mavlink/mavlink_receiver.cpp

@@ -1645,9 +1645,6 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
     if (_follow_me_pub == nullptr) {
         _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
     } else {
-      warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target_topic.lat,
-              (double) follow_target_topic.lon,
-              (double) follow_target_topic.alt);
         orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
     }
 }

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1369,20 +1369,23 @@ MulticopterPositionControl::task_main()
 			} else {
 				/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
 				if (_run_pos_control) {
-					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0) + _pos_sp_triplet.current.vx;
-					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1) + _pos_sp_triplet.current.vy;
+					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
+					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
 				}
 
+				// if there is a velocity sp available, use it
+
+                if(_pos_sp_triplet.current.velocity_valid) {
+                    _vel_sp(0) += _pos_sp_triplet.current.vx;
+                    _vel_sp(1) += _pos_sp_triplet.current.vy;
+                }
+
 				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));
-
-//				float sp_vel = sqrtf(_pos_sp_triplet.current.vx*_pos_sp_triplet.current.vx +
-//				                     _pos_sp_triplet.current.vy*_pos_sp_triplet.current.vy);
+				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) {
                     _vel_sp(0) = _pos_sp_triplet.current.vx;

src/modules/navigator/follow_target.cpp

@@ -55,26 +55,25 @@
 #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),
-	gps_valid(false),
-	_last_message_time(0),
-	_index(0)
+	_current_target_state(ACSEND),
+	_target_motion_position_sub(-1),
+	_previous_target_gps_pos_valid(false),
+	_radius_entered(false),
+    _radius_exited(false),
+    _last_publish_time(0),
+    _current_target_motion({0}),
+    _previous_target_motion({0}),
+    _dt(0)
 {
-    /* load initial params */
     updateParams();
-    follow_target_reached = false;
-    pos_pair[0].setZero();
-    pos_pair[1].setZero();
-    _current_vel.setZero();
-    _steps = 0.0f;
-    target_dist_x = target_dist_y = 0.0f;
-    target = {};
+    _current_vel.zero();
+    _step_vel.zero();
+    _target_vel.zero();
+    _target_distance.zero();
 }
 
 FollowTarget::~FollowTarget()
@@ -84,133 +83,197 @@ FollowTarget::~FollowTarget()
 void
 FollowTarget::on_inactive()
 {
-    gps_valid = false;
+    _previous_target_gps_pos_valid = false;
+    _current_target_state = ACSEND;
 }
 
 void
 FollowTarget::on_activation()
 {
-    Vector2f vel;
-    vel.setZero();
-
-	if(_tracker_motion_position_sub < 0) {
-		_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
+	if(_target_motion_position_sub < 0) {
+		_target_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
 	}
 
-	// inital set point is same as loiter sp
+	update_target_motion();
+
+	reset_mission_item_reached();
+}
+
+void
+FollowTarget::loiter() {
+    math::Vector<3> vel;
+
+    _current_vel(0) = 0;
+    _current_vel(1) = 0;
 
     set_loiter_item(&_mission_item, _param_min_alt.get());
 
     update_position_sp(vel);
 
-    target.lat = _navigator->get_global_position()->lat;
-    target.lon = _navigator->get_global_position()->lon;
+    _current_target_motion.lat = _navigator->get_global_position()->lat;
+    _current_target_motion.lon = _navigator->get_global_position()->lon;
 }
 
 void
 FollowTarget::on_active() {
-
-    bool updated;
     struct map_projection_reference_s target_ref;
-    uint64_t current_time = hrt_absolute_time();
+
+    update_target_motion();
 
     // 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);
+    map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
+
+    // are we within the target acceptance radius?
+    // give a buffer to exit/enter the radius to give the controller
+    // a chance to catch up
+
+    _radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
+    _radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M);
+
+    switch (_current_target_state) {
+        case TRACK_POSITION:
+        {
+            if (_radius_entered == true) {
+                _current_target_state = TRACK_VELOCITY;
+            } else {
+                track_position();
+            }
+            break;
+        }
+        case TRACK_VELOCITY:
+        {
+            if (_radius_exited == true) {
+                _current_target_state = TRACK_POSITION;
+            } else {
+                track_velocity();
+            }
+            break;
+        }
+        case ACSEND:
+        {
+            // ascend to the minimum altitude
 
-    follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
+            loiter();
 
-    if(follow_target_reached == false) {
-        _current_vel(0) = _navigator->get_global_position()->vel_n;
-        _current_vel(1) = _navigator->get_global_position()->vel_e;
+            _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+            if(is_mission_item_reached()) {
+                _current_target_state = TRACK_POSITION;
+            }
+            break;
+        }
+        case TARGET_TIMEOUT:
+        {
+            // Loiter until signal is regained
+
+            loiter();
+            break;
+        }
     }
 
-    orb_check(_tracker_motion_position_sub, &updated);
+    update_position_sp(_current_vel);
+}
 
-    if (updated) {
-        if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
+void
+FollowTarget::track_position() {
 
-            float dt = ((double)(current_time - _last_message_time) * 1e-6);
+    set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
+    _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 
-            if ((gps_valid == false) ) {
-                gps_pair(0) = target.lat;
-                gps_pair(1) = target.lon;
-                gps_valid = true;
+    // keep the current velocity updated
 
-                return;
-            }
+    _current_vel(0) = _navigator->get_global_position()->vel_n;
+    _current_vel(1) = _navigator->get_global_position()->vel_e;
+}
 
-            // get last gps reference
+void
+FollowTarget::track_velocity() {
 
-            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)));
+    uint64_t current_time = hrt_absolute_time();
 
-            target_vel = pos_pair[1]/(dt);
+    if (_previous_target_gps_pos_valid == false) {
+        return;
+    }
 
-            target_vel(0) += target_dist_x*.1f;
-            target_vel(1) += target_dist_y*.1f;
+    set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
 
-            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));
+    // publish a new velocity every 100 ms
 
-            set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
+    if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100)) {
+        _current_vel += _step_vel;
+        _last_publish_time = current_time;
+    }
+}
 
-            gps_pair(0) = target.lat;
-            gps_pair(1) = target.lon;
+void
+FollowTarget::update_target_motion() {
+    bool updated;
 
-            _last_message_time = current_time;
+    orb_check(_target_motion_position_sub, &updated);
 
-            if(follow_target_reached == false) {
-                 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));
-                 _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-                 update_position_sp(target_vel);
-                 return;
-             }
+    if (updated) {
+
+        // save last known motion topic
+
+        _previous_target_motion = _current_target_motion;
 
-            _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) (dt*10);
+        orb_copy(ORB_ID(follow_target), _target_motion_position_sub, &_current_target_motion);
 
+        update_target_velocity();
+
+        if (_previous_target_gps_pos_valid == false) {
+            _previous_target_motion = _current_target_motion;
+            _previous_target_gps_pos_valid = true;
         }
-    } else if (((double)(current_time - _last_message_time) * 1e-6) > 10) {
-        on_activation();
+
+            warnx(" lat %f (%f) lon %f (%f), dist = %f",
+                    _current_target_motion.lat,
+                    (double)_navigator->get_global_position()->lat,
+                    _current_target_motion.lon,
+                    (double)_navigator->get_global_position()->lon,
+                    (double) _target_distance.length());
     }
 
-    if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {
+    if ((((double)(hrt_absolute_time() - _previous_target_motion.timestamp) * 1e-6) > 10)) {
+        _current_target_state = TARGET_TIMEOUT;
+    } else if(_current_target_state == TARGET_TIMEOUT) {
+        _current_target_state = TRACK_POSITION;
+    }
+}
 
-        if (_current_vel(0) <= target_vel(0)) {
-            _current_vel(0) += (_steps);
-        }
+void
+FollowTarget::update_target_velocity() {
+    struct map_projection_reference_s target_ref;
+    math::Vector<3> target_position(0,0,0);
 
-        if (_current_vel(0) >= target_vel(0)) {
-            _current_vel(0) -= (_steps);
-        }
+    _dt = ((double) (_current_target_motion.timestamp - _previous_target_motion.timestamp) * 1e-6);
 
-        if (_current_vel(1) <= target_vel(1)) {
-            _current_vel(1) += (_steps);
-        }
+    // get last gps reference for target
 
-        if (_current_vel(1) >= target_vel(1)) {
-            _current_vel(1) -= (_steps);
-        }
+    map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
 
-        update_position_sp(_current_vel);
+    // calculate distance the target has moved
 
-        warnx("updating x %f m y %f m x %f m/s y %f m/s %f ( uav gs %f)", (double )target_dist_x, (double )target_dist_y,
-                (double )_current_vel(0), (double )_current_vel(1), (double ) _steps,
-                (double ) sqrtf(_navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e + _navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n));
+    map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), &(target_position(1)));
 
-        _last_publish_time = current_time;
-    }
+    // update the average velocity of the target
+
+    _target_vel = target_position / _dt;
+
+    // to keep the velocity increase/decrease smooth
+    // calculate how many velocity increments/decrements
+    // it will take to reach the targets velocity
+    // in 10 steps also add a feed forward input that increases
+    // velocity as the position gap increases
+
+    _step_vel = (_target_vel - _current_vel) + _target_distance * .15f;
+    _step_vel /= (_dt * 10);
 }
 
 void
-FollowTarget::update_position_sp(Vector2f & vel) {
+FollowTarget::update_position_sp(math::Vector<3> & vel) {
 
     /* convert mission item to current setpoint */
     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -221,6 +284,7 @@ FollowTarget::update_position_sp(Vector2f & vel) {
     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->current.velocity_valid = true;
     pos_sp_triplet->next.valid = false;
 
     _navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/follow_target.h

@@ -45,7 +45,8 @@
 
 #include "navigator_mode.h"
 #include "mission_block.h"
-#include <lib/matrix/matrix/Vector2.hpp>
+#include <lib/mathlib/math/Vector.hpp>
+#include <lib/matrix/matrix/Matrix.hpp>
 
 class FollowTarget : public MissionBlock
 {
@@ -61,31 +62,41 @@ public:
 
     virtual void on_active();
 
+    enum {
+        ACSEND,
+        TRACK_POSITION,
+        TRACK_VELOCITY,
+        TARGET_TIMEOUT
+    };
+
 private:
+    static constexpr int TARGET_TIMEOUT_INT_S = 10;
+    static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 10;
+
     Navigator *_navigator;
-    int     _tracker_motion_position_sub;   /**< tracker motion subscription */
     control::BlockParamFloat _param_min_alt;
-    matrix::Vector2f pos_pair[2];
-    matrix::Vector2f gps_pair;
-    bool gps_valid;
-    uint64_t _last_message_time;
+    int _current_target_state;
+    int _target_motion_position_sub;
+
+    bool _previous_target_gps_pos_valid;
+    bool _radius_entered;
+    bool _radius_exited;
+
     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;
-    };
+    math::Vector<3> _current_vel;
+    math::Vector<3> _step_vel;
+    math::Vector<3> _target_vel;
+    math::Vector<3> _target_distance;
+
+    follow_target_s _current_target_motion;
+    follow_target_s _previous_target_motion;
+    float _dt;
 
-    pos_history_s wp_history[6];
-    int wp_cnt;
-    matrix::Vector2f _current_vel;
-    matrix::Vector2f target_vel;
-    void update_position_sp(matrix::Vector2f & vel);
+    void track_position();
+    void track_velocity();
     void loiter();
-    float target_dist_x;
-    float target_dist_y;
-    follow_target_s target;
+    void update_position_sp(math::Vector<3> & vel);
+    void update_target_motion();
+    void update_target_velocity();
 };

src/modules/navigator/loiter.cpp

@@ -79,7 +79,7 @@ Loiter::on_activation()
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-    pos_sp_triplet->current.vx = pos_sp_triplet->current.vy = pos_sp_triplet->current.vz = 0;
+    pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;

src/modules/navigator/mission_block.cpp

@@ -438,7 +438,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 }
 
 void
-MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target)
+MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw)
 {
     if (_navigator->get_vstatus()->condition_landed) {
         /* landed, don't takeoff, but switch to IDLE mode */
@@ -459,7 +459,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
     }
 
     item->altitude_is_relative = false;
-    item->yaw = NAN;
+    item->yaw = yaw;
     item->loiter_radius = _navigator->get_loiter_radius();
     item->loiter_direction = 1;
     item->acceptance_radius = _navigator->get_acceptance_radius();

src/modules/navigator/mission_block.h

@@ -125,7 +125,7 @@ protected:
     /**
      * set follow_target item
      */
-    void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target);
+    void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw);
 
 	void issue_command(const struct mission_item_s *item);