follow target mode working

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "mavlink/include/mavlink/v1.0"]
 	path = mavlink/include/mavlink/v1.0
-	url = git://github.com/mavlink/c_library.git
+	url = https://github.com/catch-twenty-two/mavlink.git
 [submodule "NuttX"]
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git

msg/follow_target.msg

@@ -1,7 +1,7 @@
 uint64 timestamp		# timestamp, UNIX epoch (GPS synced)
 uint8 est_capabilities		# Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES)
-uint32 lat			# target position (deg * 1e7)
-uint32 lon			# target position (deg * 1e7) 
+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. 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1625,28 +1625,28 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	}
 }
 
-void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) {
-    mavlink_follow_target_t follow_me_msg;
-    mavlink_msg_follow_target_decode(msg, &follow_me_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_msg_follow_target_decode(msg, &follow_target_msg);
+
     follow_target_topic.timestamp = hrt_absolute_time();
 
-    memcpy(follow_target_topic.accel, follow_me_msg.acc, sizeof(follow_target_topic.accel));
-    memcpy(follow_target_topic.velocity, follow_me_msg.acc, sizeof(follow_target_topic.velocity));
-    //memcpy(follow_target_topic.attitude_q,  follow_me_msg.attitude quaternion, sizeof(follow_target_topic.attitude_q));
-    follow_target_topic.lat = follow_me_msg.lat;
-    follow_target_topic.lon = follow_me_msg.lon;
+    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;
 
     if (_follow_me_pub == nullptr) {
         _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
     } else {
-      warnx("publishing follow");
+      warnx("publishing follow %f %f %f", (double) follow_target_topic.velocity[0], (double) follow_target_topic.velocity[1], (double) follow_target_topic.velocity[2]);
         orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
     }
-
-    warnx("got message follow");
 }
 
 void

src/modules/navigator/follow_target.cpp

@@ -57,7 +57,8 @@
 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)
 {
     /* load initial params */
     updateParams();
@@ -90,17 +91,15 @@ FollowTarget::on_active() {
   if (updated) {
     if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
 
-      /* predict  target location*/
+          set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
 
-      if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
-        _navigator->get_mission_result()->finished = true;
-        _navigator->set_mission_result_updated();
-        set_idle_item(&_mission_item);
+          /* 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;
 
-        struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-        mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-        _navigator->set_position_setpoint_triplet_updated();
-      }
+          _navigator->set_position_setpoint_triplet_updated();
     }
   }
 }

src/modules/navigator/follow_target.h

@@ -62,4 +62,6 @@ public:
 
     virtual void on_active();
 
+private:
+    control::BlockParamFloat _param_min_alt;
 };

src/modules/navigator/mission_block.cpp

@@ -434,6 +434,39 @@ 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)
+{
+    if (_navigator->get_vstatus()->condition_landed) {
+        /* landed, don't takeoff, but switch to IDLE mode */
+        item->nav_cmd = NAV_CMD_IDLE;
+
+    } else {
+        item->nav_cmd = NAV_CMD_FOLLOW_TARGET;
+
+        /* use current target position */
+
+        item->lat = target.lat;
+        item->lon = target.lon;
+        item->altitude = target.alt;
+
+        if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+            item->altitude = _navigator->get_home_position()->alt + min_clearance;
+        }
+    }
+
+    item->altitude_is_relative = false;
+    item->yaw = NAN;
+    item->loiter_radius = _navigator->get_loiter_radius();
+    item->loiter_direction = 1;
+    item->acceptance_radius = _navigator->get_acceptance_radius();
+    item->time_inside = 0.0f;
+    item->pitch_min = 0.0f;
+    item->autocontinue = false;
+    item->origin = ORIGIN_ONBOARD;
+}
+
+
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
 {

src/modules/navigator/mission_block.h

@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/follow_target.h>
 
 #include "navigator_mode.h"
 
@@ -116,10 +117,15 @@ protected:
 	 */
 	void set_idle_item(struct mission_item_s *item);
 
-	/**
-	 * Convert a mission item to a command
-	 */
-	void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
+    /**
+     * Convert a mission item to a command
+     */
+    void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
+
+    /**
+     * set follow_target item
+     */
+    void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target);
 
 	void issue_command(const struct mission_item_s *item);
 

src/modules/navigator/navigation.h

@@ -61,6 +61,7 @@ enum NAV_CMD {
 	NAV_CMD_TAKEOFF = 22,
 	NAV_CMD_ROI = 80,
 	NAV_CMD_PATHPLANNING = 81,
+	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,