Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

ROMFS/px4fmu_common/init.d/rc.usb

@@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
 usleep 100000
+mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
 exit

src/modules/mavlink/mavlink_messages.cpp

@@ -959,14 +959,14 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sp_triplet_sub->update(t)) {
-			mavlink_msg_global_position_setpoint_int_send(_channel,
-					MAV_FRAME_GLOBAL,
-					(int32_t)(pos_sp_triplet->current.lat * 1e7),
-					(int32_t)(pos_sp_triplet->current.lon * 1e7),
-					(int32_t)(pos_sp_triplet->current.alt * 1000),
-					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
-		}
+		/* always send this message, even if it has not been updated */
+		pos_sp_triplet_sub->update(t);
+		mavlink_msg_global_position_setpoint_int_send(_channel,
+			MAV_FRAME_GLOBAL,
+			(int32_t)(pos_sp_triplet->current.lat * 1e7),
+			(int32_t)(pos_sp_triplet->current.lon * 1e7),
+			(int32_t)(pos_sp_triplet->current.alt * 1000),
+			(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
 	}
 };