Merge pull request #2472 from tumbili/SITL_progress

Sitl progress

posix-configs/SITL/init/rcS

@@ -22,6 +22,9 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.4
+param set MPC_XY_VEL_P 0.2
+param set MPC_XY_VEL_D 0.005
 rgbled start
 tone_alarm start
 gyrosim start
@@ -32,8 +35,13 @@ gpssim start
 hil mode_pwm
 commander start
 sensors start
+navigator start
 attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556

src/modules/mavlink/mavlink_messages.cpp

@@ -1776,6 +1776,10 @@ protected:
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
 			msg.z = pos_sp.z;
+			msg.yaw = pos_sp.yaw;
+			msg.vx = pos_sp.vx;
+			msg.vy = pos_sp.vy;
+			msg.vz = pos_sp.vz;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
 		}