Add body/NED frame for offboard velocity control

This adds the possibility to use offboard velocity control in the body
frame and not just the NED (world) frame.

The frame is set in the set_position_target_local_ned message and passed
on to mc_pos_control in the position_setpoint topic.

msg/position_setpoint.msg

@@ -9,6 +9,9 @@ uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed
 uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
 uint8 SETPOINT_TYPE_FOLLOW_TARGET=7  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
 
+uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
+uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
+
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller
 float32 x			# local position setpoint in m in NED
@@ -19,6 +22,8 @@ float32 vx			# local velocity setpoint in m/s in NED
 float32 vy			# local velocity setpoint in m/s in NED
 float32 vz			# local velocity setpoint in m/s in NED
 bool velocity_valid		# true if local velocity setpoint valid
+uint8 velocity_frame		# to set velocity setpoints in NED or body
+
 bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg

src/modules/mavlink/mavlink_receiver.cpp

@@ -868,6 +868,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
 						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
 
+						pos_sp_triplet.current.velocity_frame =
+							set_position_target_local_ned.coordinate_frame;
 					} else {
 						pos_sp_triplet.current.velocity_valid = false;
 					}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1222,12 +1222,24 @@ MulticopterPositionControl::control_offboard(float dt)
 					_hold_offboard_xy = true;
 				}
 
-				_run_pos_control = false;
+				_run_pos_control = true;
 
 			} else {
-				/* set position setpoint move rate */
-				_vel_sp(0) = _pos_sp_triplet.current.vx;
-				_vel_sp(1) = _pos_sp_triplet.current.vy;
+
+				if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
+					/* set position setpoint move rate */
+					_vel_sp(0) = _pos_sp_triplet.current.vx;
+					_vel_sp(1) = _pos_sp_triplet.current.vy;
+
+				} else if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
+					// Transform velocity command from body frame to NED frame
+					_vel_sp(0) = cosf(_yaw) * _pos_sp_triplet.current.vx - sinf(_yaw) * _pos_sp_triplet.current.vy;
+					_vel_sp(1) = sinf(_yaw) * _pos_sp_triplet.current.vx + cosf(_yaw) * _pos_sp_triplet.current.vy;
+
+				} else {
+					PX4_WARN("Unknown velocity offboard coordinate frame");
+				}
+
 				_run_pos_control = false;
 
 				_hold_offboard_xy = false;