Added support for velocity setpoint in mavlink_receiver and mc_pos_control

3 files changed, 64 insertions, 2 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;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index db9e06461..a20cbc7e9 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -55,6 +55,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -138,6 +139,7 @@ private:
 	orb_advert_t _flow_pub;
 	orb_advert_t _offboard_control_sp_pub;
 	orb_advert_t _local_pos_sp_pub;
+	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
 	orb_advert_t _vicon_position_pub;
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 9d9d2a7c0..53394b41c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -119,6 +119,7 @@ private:
 	int		_local_pos_sub;			/**< vehicle local position */
 	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 	int 		_local_pos_sp_sub;		/**< offboard local position setpoint */
+	int 		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
@@ -257,6 +258,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_manual_sub(-1),
 	_arming_sub(-1),
 	_local_pos_sub(-1),
+	_global_vel_sp_sub(-1),
 	_pos_sp_triplet_sub(-1),
 
 /* publications */
@@ -784,6 +786,24 @@ MulticopterPositionControl::task_main()
 					_vel_sp(2) = _params.land_speed;
 				}
 
+				/* Offboard velocity control mode */
+				if (_control_mode.flag_control_offboard_enabled) {
+					bool updated;
+					orb_check(_global_vel_sp_sub, &updated);
+
+					if (updated) {
+						orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp);
+					}
+
+					if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) {
+						_vel_sp(2) = _global_vel_sp.vz;
+					}
+
+					if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
+						_vel_sp(0) = _global_vel_sp.vx;
+						_vel_sp(1) = _global_vel_sp.vy;
+				}
+
 				if (!_control_mode.flag_control_manual_enabled) {
 					/* limit 3D speed only in non-manual modes */
 					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
@@ -801,10 +821,16 @@ MulticopterPositionControl::task_main()
 				if (_global_vel_sp_pub > 0) {
 					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
 
-				} else {
+				} else if (!_control_mode.flag_control_offboard_enabled) {
 					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
 				}
 
+				/* Close fd to let offboard vel sp be advertised in mavlink receiver */
+				if (_control_mode.flag_control_offboard_enabled && _global_vel_sp_pub > 0) {
+					close (_global_vel_sp);
+					_global_vel_sp_pub = -1;
+				}
+
 				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
 					/* reset integrals if needed */
 					if (_control_mode.flag_control_climb_rate_enabled) {