Merge pull request #1199 from PX4/navlogfix

Nav log fix

src/modules/navigator/navigator_main.cpp

@@ -458,7 +458,7 @@ void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	/* TODO: set nav_state */
+	_pos_sp_triplet.nav_state = _vstatus.nav_state;
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {

src/modules/sdlog2/sdlog2.c

@@ -1432,8 +1432,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
+
+			if (buf.triplet.current.valid) {
 				log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
 				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
 				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
 				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
@@ -1444,6 +1446,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
 				LOGBUFFER_WRITE_AND_COUNT(GPSP);
 			}
+		}
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {

src/modules/uORB/topics/position_setpoint_triplet.h

@@ -95,6 +95,8 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
+
+	unsigned nav_state;				/**< report the navigation state */
 };
 
 /**