Added support for velocity setpoint in mavlink_receiver and mc_pos_control

1 files changed, 35 insertions, 1 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index de9a6e7ad..8a00509a4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -103,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_flow_pub(-1),
 	_offboard_control_sp_pub(-1),
 	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
 	_vicon_position_pub(-1),
@@ -450,6 +451,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 						_rates_sp_pub = -1;
 					}
 
+					if (_global_vel_sp_pub > 0) {
+						close(_global_vel_sp_pub);
+						_global_vel_sp_pub = -1;
+					}
+
 					if (_local_pos_sp_pub < 0) {
 						_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
 
@@ -458,7 +464,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 					}
 
 				} else if (_control_mode.flag_control_velocity_enabled) {
-					// TODO
+					/* velocity control */
+					struct vehicle_global_velocity_setpoint_s global_vel_sp;
+					memset(&global_vel_sp, 0, sizeof(&global_vel_sp));
+
+					global_vel_sp.vx = offboard_control_sp.p1;
+					global_vel_sp.vy = offboard_control_sp.p2;
+					global_vel_sp.vz = offboard_control_sp.p3;
+
+					if (_att_sp_pub > 0) {
+						close(_att_sp_pub);
+						_att_sp_pub = -1;
+					}
+
+					if (_rates_sp_pub > 0) {
+						close(_rates_sp_pub);
+						_rates_sp_pub = -1;
+					}
+
+					if (_global_vel_sp_pub < 0) {
+						_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint_s), &_global_vel_sp_pub);
+
+					} else {
+						orb_publish(ORB_ID(vehicle_global_velocity_setpoint_s), _global_vel_sp_pub, &global_vel_sp);
+					}
 
 				} else if (_control_mode.flag_control_attitude_enabled) {
 					/* attitude control */
@@ -1038,6 +1067,11 @@ MavlinkReceiver::receive_thread(void *arg)
 					_local_pos_sp_pub = -1;
 				}
 
+				if (_global_vel_sp_pub > 0) {
+					close(_global_vel_sp_pub);
+					_global_vel_sp_pub = -1;
+				}
+
 				if (_att_sp_pub > 0) {
 					close(_att_sp_pub);
 					_att_sp_pub = -1;