follow me working

msg/follow_target.msg

@@ -1,11 +1,4 @@
 uint64 timestamp		# timestamp, UNIX epoch (GPS synced)
-#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

src/modules/mavlink/mavlink_receiver.cpp

@@ -127,6 +127,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_manual_pub(nullptr),
 	_land_detector_pub(nullptr),
 	_time_offset_pub(nullptr),
+	_follow_me_pub(nullptr),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -1627,27 +1628,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 = {};
+    mavlink_follow_target_t follow_target_msg;
+    follow_target_s follow_target_topic = { };
 
-    //mavlink_msg_follow_target_decode(msg, &follow_target_msg);
+    mavlink_msg_follow_target_decode(msg, &follow_target_msg);
 
-    follow_target.timestamp = hrt_absolute_time();
+    follow_target_topic.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));
 
-//    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;
+    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;
 
     if (_follow_me_pub == nullptr) {
-        _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target);
+        _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.lat,
-              (double) follow_target.lon,
-              (double) follow_target.alt);
-        orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target);
+      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/navigator/follow_target.cpp

@@ -51,6 +51,7 @@
 
 #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"
 
@@ -59,7 +60,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),
@@ -92,6 +93,10 @@ 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());
@@ -121,11 +126,10 @@ FollowTarget::on_active() {
         _current_vel(1) = _navigator->get_global_position()->vel_e;
     }
 
-    orb_check(_navigator->_tracker_motion_position_sub, &updated);
+    orb_check(_tracker_motion_position_sub, &updated);
 
     if (updated) {
-        warnx("UPDASTD ");
-        if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) {
+        if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
 
             float dt = ((double)(current_time - _last_message_time) * 1e-6);
 
@@ -173,14 +177,8 @@ FollowTarget::on_active() {
                       sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10);
 
         }
-    }
-
-    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);
-        }
+    } else if (((double)(current_time - _last_message_time) * 1e-6) > 10) {
+        on_activation();
     }
 
     if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {

src/modules/navigator/follow_target.h

@@ -63,6 +63,7 @@ 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;

src/modules/navigator/loiter.cpp

@@ -79,6 +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->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;

src/modules/navigator/navigator.h

@@ -57,7 +57,6 @@
 #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"
@@ -75,7 +74,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 9
+#define NAVIGATOR_MODE_ARRAY_SIZE 10
 
 class Navigator : public control::SuperBlock
 {
@@ -183,7 +182,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 */
@@ -203,7 +202,6 @@ 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;
@@ -220,7 +218,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;
@@ -253,7 +251,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 */
 
@@ -301,7 +299,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,7 +157,6 @@ 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;
@@ -168,7 +167,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[6] = &_rcLoss;
 	_navigation_mode_array[7] = &_takeoff;
 	_navigation_mode_array[8] = &_land;
-	//_navigation_mode_array[9] = nullptr;//&_follow_target;
+	_navigation_mode_array[9] = &_follow_target;
 
 	updateParams();
 }
@@ -213,13 +212,7 @@ Navigator::gps_position_update()
 void
 Navigator::sensor_combined_update()
 {
-    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);
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 }
 
 void
@@ -303,7 +296,6 @@ 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();
@@ -394,12 +386,6 @@ 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;
@@ -517,7 +503,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;