Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Conflicts:
	src/modules/mavlink/mavlink_main.cpp
	src/modules/mavlink/mavlink_main.h
	src/modules/mavlink/mavlink_receiver.cpp

1 files changed, 47 insertions, 83 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b7cddf523..dfc5ddc91 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -150,18 +150,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
-		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
+	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+		handle_message_set_position_target_local_ned(msg);
 		break;
 
-	case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL:
-		handle_message_local_ned_position_setpoint_external(msg);
+	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+		handle_message_set_attitude_target(msg);
 		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL:
-		handle_message_attitude_setpoint_external(msg);
-		break;
-
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -251,7 +246,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
 
@@ -372,53 +368,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
 {
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
-
-	/* Only accept system IDs from 1 to 4 */
-	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
-		struct offboard_control_setpoint_s offboard_control_sp;
-		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
-
-		/* Convert values * 1000 back */
-		//XXX: convert to quaternion
-		//offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
-
-		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
-
-		offboard_control_sp.timestamp = hrt_absolute_time();
-
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
-		}
-	}
-}
-
-void
-MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg)
-{
-	mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external;
-	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
 
 	struct offboard_control_setpoint_s offboard_control_sp;
 	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
-	if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system ||
-				local_ned_position_setpoint_external.target_system == 0) &&
-			(mavlink_system.compid == local_ned_position_setpoint_external.target_component ||
-			 local_ned_position_setpoint_external.target_component == 0)) {
+	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				set_position_target_local_ned.target_system == 0) &&
+			(mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 set_position_target_local_ned.target_component == 0)) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
-		switch (local_ned_position_setpoint_external.coordinate_frame) {
+		switch (set_position_target_local_ned.coordinate_frame) {
 			case MAV_FRAME_LOCAL_NED:
 				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
 				break;
@@ -435,16 +400,16 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 				/* invalid setpoint, avoid publishing */
 				return;
 		}
-		offboard_control_sp.position[0] = local_ned_position_setpoint_external.x;
-		offboard_control_sp.position[1] = local_ned_position_setpoint_external.y;
-		offboard_control_sp.position[2] = local_ned_position_setpoint_external.z;
-		offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx;
-		offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy;
-		offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz;
-		offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx;
-		offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
-		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
-		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
+		offboard_control_sp.position[0] = set_position_target_local_ned.x;
+		offboard_control_sp.position[1] = set_position_target_local_ned.y;
+		offboard_control_sp.position[2] = set_position_target_local_ned.z;
+		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
+		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
+		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
+		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
+		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
+		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
+		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
 
 		/* If we are in force control mode, for now set offboard mode to force control */
 		if (offboard_control_sp.isForceSetpoint) {
@@ -454,7 +419,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		/* set ignore flags */
 		for (int i = 0; i < 9; i++) {
 			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
 
 
@@ -540,32 +505,32 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 }
 
 void
-MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 {
-	mavlink_attitude_setpoint_external_t attitude_setpoint_external;
-	mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external);
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 
 	struct offboard_control_setpoint_s offboard_control_sp;
 	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
-	if ((mavlink_system.sysid == attitude_setpoint_external.target_system ||
-				attitude_setpoint_external.target_system == 0) &&
-			(mavlink_system.compid == attitude_setpoint_external.target_component ||
-			 attitude_setpoint_external.target_component == 0)) {
+	if ((mavlink_system.sysid == set_attitude_target.target_system ||
+				set_attitude_target.target_system == 0) &&
+			(mavlink_system.compid == set_attitude_target.target_component ||
+			 set_attitude_target.target_component == 0)) {
 		for (int i = 0; i < 4; i++) {
-			offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i];
+			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
 		}
-		offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate;
-		offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate;
-		offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate;
+		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
+		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
+		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
 
 		for (int i = 0; i < 3; i++) {
 			offboard_control_sp.ignore &=  ~(1 << (i + 9));
-			offboard_control_sp.ignore |=  (attitude_setpoint_external.type_mask & (1 << i)) << 9;
+			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << 9;
 		}
 		offboard_control_sp.ignore &= ~(1 << 10);
-		offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3);
 
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
@@ -590,14 +555,14 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 			if (_control_mode.flag_control_offboard_enabled) {
 
 				/* Publish attitude setpoint if ignore bit is not set */
-				if (!(attitude_setpoint_external.type_mask & (1 << 7))) {
+				if (!(set_attitude_target.type_mask & (1 << 7))) {
 					struct vehicle_attitude_setpoint_s att_sp;
 					att_sp.timestamp = hrt_absolute_time();
-					mavlink_quaternion_to_euler(attitude_setpoint_external.q,
+					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
-					att_sp.thrust = attitude_setpoint_external.thrust;
+					att_sp.thrust = set_attitude_target.thrust;
 					att_sp.q_d_valid = true;
-					memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d));
+					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
 					if (_att_sp_pub < 0) {
 						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					} else {
@@ -607,13 +572,13 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 
 				/* Publish attitude rate setpoint if ignore bit are not set */
 				///XXX add support for ignoring individual axes
-				if (!(attitude_setpoint_external.type_mask & (0b111))) {
+				if (!(set_attitude_target.type_mask & (0b111))) {
 					struct vehicle_rates_setpoint_s rates_sp;
 					rates_sp.timestamp = hrt_absolute_time();
-					rates_sp.roll = attitude_setpoint_external.body_roll_rate;
-					rates_sp.pitch = attitude_setpoint_external.body_pitch_rate;
-					rates_sp.yaw = attitude_setpoint_external.body_yaw_rate;
-					rates_sp.thrust = attitude_setpoint_external.thrust;
+					rates_sp.roll = set_attitude_target.body_roll_rate;
+					rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					rates_sp.thrust = set_attitude_target.thrust;
 
 					if (_att_sp_pub < 0) {
 						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
@@ -626,7 +591,6 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 		}
 	}
 }
-
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {