Tests

msg/follow_target.msg

@@ -1,11 +1,11 @@
 uint64 timestamp		# timestamp, UNIX epoch (GPS synced)
-uint8 est_capabilities		# Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES)
+#uint8 est_capabilities		# Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES)
 float64 lat			# target position (deg * 1e7)
 float64 lon			# target position (deg * 1e7) 
 float32 alt			# target position 
-float32[3] velocity		# target velocity (AMSL, meters) # target position (0 0 0 for unknown)
-float32[3] accel		# target acceleration (linear) in earth frame. 
-float32[4] attitude_q		# where the target is facing. 
-float32[3] rates		# (0 0 0 for unknown)
-float32[3] pos_cov		# uncertainly in earth frame for X, Y and Z. We will need to agree on the exact format here -1 for unknown
-uint64 custom_state		# A custom vector, can be used to transmit e.g. button states or switches of a tracker device
+#float32[3] velocity		# target velocity (AMSL, meters) # target position (0 0 0 for unknown)
+#float32[3] accel		# target acceleration (linear) in earth frame. 
+#float32[4] attitude_q		# where the target is facing. 
+#float32[3] rates		# (0 0 0 for unknown)
+#float32[3] pos_cov		# uncertainly in earth frame for X, Y and Z. We will need to agree on the exact format here -1 for unknown
+#uint64 custom_state		# A custom vector, can be used to transmit e.g. button states or switches of a tracker device

src/modules/mavlink/mavlink_receiver.cpp

@@ -1627,27 +1627,27 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
 {
-    mavlink_follow_target_t follow_target_msg;
-    follow_target_s follow_target_topic = { };
+//    mavlink_follow_target_t follow_target_msg;
+    follow_target_s follow_target = {};
 
-    mavlink_msg_follow_target_decode(msg, &follow_target_msg);
+    //mavlink_msg_follow_target_decode(msg, &follow_target_msg);
 
-    follow_target_topic.timestamp = hrt_absolute_time();
+    follow_target.timestamp = hrt_absolute_time();
 
-    memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel));
-    memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity));
+   // memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel));
+    //memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity));
 
-    follow_target_topic.lat = follow_target_msg.lat*1e-7;
-    follow_target_topic.lon = follow_target_msg.lon*1e-7;
-    follow_target_topic.alt = follow_target_msg.alt;
+//    follow_target.lat = follow_target_msg.lat*1e-7;
+//    follow_target.lon = follow_target_msg.lon*1e-7;
+//    follow_target.alt = follow_target_msg.alt;
 
     if (_follow_me_pub == nullptr) {
-        _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
+        _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target);
     } 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);
+      warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target.lat,
+              (double) follow_target.lon,
+              (double) follow_target.alt);
+        orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target);
     }
 }
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1369,8 +1369,8 @@ 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);
-					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
+					_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;
 				}
 
 				if (_run_alt_control) {

src/modules/navigator/follow_target.cpp

@@ -51,7 +51,6 @@
 
 #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"
 
@@ -60,7 +59,7 @@ using namespace matrix;
 FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
     MissionBlock(navigator, name),
 	_navigator(navigator),
-	_tracker_motion_position_sub(-1),
+	//_tracker_motion_position_sub(-1),
 	_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
 	gps_valid(false),
 	_last_message_time(0),
@@ -73,6 +72,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
     pos_pair[1].setZero();
     _current_vel.setZero();
     _steps = 0.0f;
+    target_dist_x = target_dist_y = 0.0f;
+    target = {};
 }
 
 FollowTarget::~FollowTarget()
@@ -91,31 +92,42 @@ FollowTarget::on_activation()
     Vector2f vel;
     vel.setZero();
 
-	if(_tracker_motion_position_sub < 0) {
-		_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
-	}
-
 	// inital set point is same as loiter sp
 
     set_loiter_item(&_mission_item, _param_min_alt.get());
 
-    convert_mission_item_to_sp(vel);
+    update_position_sp(vel);
+
+    target.lat = _navigator->get_global_position()->lat;
+    target.lon = _navigator->get_global_position()->lon;
 }
 
 void
 FollowTarget::on_active() {
-    follow_target_s target;
+
     bool updated;
-   struct map_projection_reference_s target_ref;
-    float target_dist_x,target_dist_y;
+    struct map_projection_reference_s target_ref;
     uint64_t current_time = hrt_absolute_time();
 
+    // 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);
 
-    orb_check(_tracker_motion_position_sub, &updated);
+    follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
+
+    if(follow_target_reached == false) {
+        _current_vel(0) = _navigator->get_global_position()->vel_n;
+        _current_vel(1) = _navigator->get_global_position()->vel_e;
+    }
+
+    orb_check(_navigator->_tracker_motion_position_sub, &updated);
 
     if (updated) {
-        if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
+        warnx("UPDASTD ");
+        if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) {
+
+            float dt = ((double)(current_time - _last_message_time) * 1e-6);
 
             if ((gps_valid == false) ) {
                 gps_pair(0) = target.lat;
@@ -125,34 +137,15 @@ FollowTarget::on_active() {
                 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);
+            target_vel = pos_pair[1]/(dt);
 
+            target_vel(0) += target_dist_x*.1f;
+            target_vel(1) += target_dist_y*.1f;
 
             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),
@@ -169,44 +162,63 @@ FollowTarget::on_active() {
 
             _last_message_time = current_time;
 
+            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;
+             }
+
             _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];
+                      sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10);
+
         }
     }
-        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_time - _last_message_time) / (1000*1000) > 5) {
+    //    on_activation();
+        static int gg = 0;
+        if(!(gg++%100)) {
+            warnx("timed out loitering %llu %d", (current_time - _last_message_time) / (1000*1000), _navigator->_tracker_motion_position_sub);
+        }
+    }
 
-            if(_current_vel(0) >= target_vel(0)) {
-                _current_vel(0) -= _steps;
-            }
+    if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {
 
-            if(_current_vel(1) <= target_vel(1)) {
-               _current_vel(1) += _steps;
-            }
+        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);
+        }
+
+        update_position_sp(_current_vel);
+
+        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));
 
-            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(Vector2f & vel) {
+FollowTarget::update_position_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);

src/modules/navigator/follow_target.h

@@ -63,7 +63,6 @@ public:
 
 private:
     Navigator *_navigator;
-    int     _tracker_motion_position_sub;   /**< tracker motion subscription */
     control::BlockParamFloat _param_min_alt;
     matrix::Vector2f pos_pair[2];
     matrix::Vector2f gps_pair;
@@ -83,5 +82,9 @@ private:
     int wp_cnt;
     matrix::Vector2f _current_vel;
     matrix::Vector2f target_vel;
-    void convert_mission_item_to_sp(matrix::Vector2f & vel);
+    void update_position_sp(matrix::Vector2f & vel);
+    void loiter();
+    float target_dist_x;
+    float target_dist_y;
+    follow_target_s target;
 };

src/modules/navigator/mission_block.cpp

@@ -453,7 +453,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
         item->lon = target.lon;
         item->altitude = target.alt;
 
-        if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+        if (((min_clearance > 0.0f) && (item->altitude < _navigator->get_home_position()->alt + min_clearance)) || PX4_ISFINITE(target.alt)) {
             item->altitude = _navigator->get_home_position()->alt + min_clearance;
         }
     }

src/modules/navigator/navigator.h

@@ -57,6 +57,7 @@
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/follow_target.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -74,7 +75,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 10
+#define NAVIGATOR_MODE_ARRAY_SIZE 9
 
 class Navigator : public control::SuperBlock
 {
@@ -182,7 +183,7 @@ public:
 	void		increment_mission_instance_count() { _mission_instance_count++; }
 
 	void 		set_mission_failure(const char *reason);
-
+	int     _tracker_motion_position_sub;   /**< tracker motion subscription */
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -202,6 +203,7 @@ private:
 	int		_param_update_sub;		/**< param update subscription */
 	int		_vehicle_command_sub;		/**< vehicle commands (onboard and offboard) */
 
+
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
 	orb_advert_t	_geofence_result_pub;
@@ -218,7 +220,7 @@ private:
 	mission_item_s 					_mission_item;		/**< current mission item */
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
-
+	follow_target_s                     _follow_target_pk;
 	mission_result_s				_mission_result;
 	geofence_result_s				_geofence_result;
 	vehicle_attitude_setpoint_s			_att_sp;
@@ -251,7 +253,7 @@ private:
 							  (FW only!) */
 	GpsFailure	_gpsFailure;			/**< class that handles the OBC gpsfailure loss mode */
 
-	FollowTarget _follow_target;
+//	FollowTarget _follow_target;
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
@@ -299,7 +301,7 @@ private:
 	 * Retrieve vehicle control mode
 	 */
 	void		vehicle_control_mode_update();
-
+	void follow_target_update();
 	/**
 	 * Update parameters
 	 */

src/modules/navigator/navigator_main.cpp

@@ -148,7 +148,7 @@ Navigator::Navigator() :
 	_dataLinkLoss(this, "DLL"),
 	_engineFailure(this, "EF"),
 	_gpsFailure(this, "GPSF"),
-	_follow_target(this, "TAR"),
+//	_follow_target(this, "TAR"),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -157,6 +157,7 @@ Navigator::Navigator() :
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
 	_mission_cruising_speed(-1.0f)
 {
+	_tracker_motion_position_sub = -1;
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
@@ -167,7 +168,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[6] = &_rcLoss;
 	_navigation_mode_array[7] = &_takeoff;
 	_navigation_mode_array[8] = &_land;
-	_navigation_mode_array[9] = &_follow_target;
+	//_navigation_mode_array[9] = nullptr;//&_follow_target;
 
 	updateParams();
 }
@@ -212,7 +213,13 @@ Navigator::gps_position_update()
 void
 Navigator::sensor_combined_update()
 {
-	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+    orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+}
+
+void
+Navigator::follow_target_update()
+{
+    orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &_follow_target_pk);
 }
 
 void
@@ -296,6 +303,7 @@ Navigator::task_main()
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -386,6 +394,12 @@ Navigator::task_main()
 			home_position_update();
 		}
 
+		orb_check(_tracker_motion_position_sub, &updated);
+		if(updated) {
+		    follow_target_update();
+		    warnx("updated in nav");
+		}
+
 		orb_check(_vehicle_command_sub, &updated);
 		if (updated) {
 			vehicle_command_s cmd;
@@ -503,7 +517,7 @@ Navigator::task_main()
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 				_pos_sp_triplet_published_invalid_once = false;
-				_navigation_mode = &_follow_target;
+//				_navigation_mode = &_follow_target;
 				break;
 			default:
 				_navigation_mode = nullptr;