Merge branch 'offboard2' of github.com:elikos/Firmware into offboard2

3 files changed, 86 insertions, 7 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3b3423a4b..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),
@@ -365,30 +366,47 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 		case 1:
 			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
 			ml_armed = true;
+
 			break;
 
 		case 2:
 			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
 			ml_armed = true;
+
 			break;
 
 		case 3:
 			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
 			ml_armed = true;
+
 			break;
 
 		case 4:
 			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
 			ml_armed = true;
+
 			break;
 		default:
 			break;
 		}
 
-		offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-		offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-		offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-		offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
+		if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
+			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
+
+		} else if (m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || m1_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
+
+			/*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */ 
+			/* Converts INT16 centimeters to float meters */
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f;
+			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f;
+
+		}
 
 		if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
 			ml_armed = false;
@@ -420,7 +438,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 					loc_pos_sp.x = offboard_control_sp.p1;
 					loc_pos_sp.y = offboard_control_sp.p2;
 					loc_pos_sp.yaw = offboard_control_sp.p3;
-					loc_pos_sp.z = offboard_control_sp.p4;
+					loc_pos_sp.z = -offboard_control_sp.p4;
 
 					/* Close fds to allow position controller to use attitude controller */
 					if (_att_sp_pub > 0) {
@@ -433,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);
 
@@ -441,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 */
@@ -1021,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 6721d017d..7af148c7f 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) {