making first setpoint on follow target activation loiter in place

src/modules/mavlink/mavlink_receiver.cpp

@@ -1644,7 +1644,9 @@ 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 %f %f %f", (double) follow_target_topic.velocity[0], (double) follow_target_topic.velocity[1], (double) follow_target_topic.velocity[2]);
+      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

@@ -79,27 +79,41 @@ FollowTarget::on_activation()
 	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();
 }
 
 void
 FollowTarget::on_active() {
-  follow_target_s target;
-  bool updated;
+    follow_target_s target;
+    bool updated;
 
-  orb_check(_tracker_motion_position_sub, &updated);
+    orb_check(_tracker_motion_position_sub, &updated);
 
-  if (updated) {
-    if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
-
-          set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
-
-          /* convert mission item to current setpoint */
-          struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-          pos_sp_triplet->previous.valid = false;
-          mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-          pos_sp_triplet->next.valid = false;
-
-          _navigator->set_position_setpoint_triplet_updated();
+    if (updated) {
+        if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
+            set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
+            convert_mission_item_to_sp();
+        }
     }
-  }
+}
+
+void
+FollowTarget::convert_mission_item_to_sp() {
+
+    /* 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 = pos_sp_triplet->current;
+    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+    pos_sp_triplet->next.valid = false;
+
+    _navigator->set_position_setpoint_triplet_updated();
 }

src/modules/navigator/follow_target.h

@@ -64,4 +64,5 @@ public:
 
 private:
     control::BlockParamFloat _param_min_alt;
+    void convert_mission_item_to_sp();
 };