mavlink: only save a offboard_control, don't publish to setpoint topics (let the navigator do this)

1 files changed, 11 insertions, 143 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index aaae0933e..c4955f1b3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -355,70 +355,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)
 {
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+	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);
 
-	if (mavlink_system.sysid < 4) {
+	/* Only accept system IDs from 1 to 4
+	 * TODO: check for own system ID */
+	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));
 
-		enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT;
-		bool ml_armed = false;
+		/* Convert values * 1000 back */
+		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;
 
-		switch (quad_motors_setpoint.mode) {
-		case 0:
-			break;
-
-		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;
-		}
-
-		if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_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 (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_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] / 1000.0f;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f;
-		}
-
-		if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-			ml_armed = false;
-		}
-
-		offboard_control_sp.armed = ml_armed;
-		offboard_control_sp.mode = ml_mode;
+		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -428,90 +380,6 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 		} else {
 			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
 		}
-
-		bool updated;
-		orb_check(_control_mode_sub, &updated);
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
-		}
-
-		if (_control_mode.flag_control_offboard_enabled) {
-			if (_control_mode.flag_control_position_enabled) {
-				// TODO Use something else then quad_swarm_roll_pitch_yaw_thrust
-				struct vehicle_local_position_setpoint_s loc_pos_sp;
-				memset(&loc_pos_sp, 0, sizeof(loc_pos_sp));
-
-				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;
-
-				if (_local_pos_sp_pub < 0) {
-					_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp);
-
-				} else {
-					orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp);
-				}
-
-			} else if (_control_mode.flag_control_velocity_enabled) {
-				/* 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 (_global_vel_sp_pub < 0) {
-					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
-
-				} else {
-					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp);
-				}
-
-			} else if (_control_mode.flag_control_attitude_enabled) {
-				/* attitude control */
-				struct vehicle_attitude_setpoint_s att_sp;
-				memset(&att_sp, 0, sizeof(att_sp));
-
-				att_sp.roll_body = offboard_control_sp.p1;
-				att_sp.pitch_body = offboard_control_sp.p2;
-				att_sp.yaw_body = offboard_control_sp.p3;
-				att_sp.thrust = offboard_control_sp.p4;
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				if (_att_sp_pub < 0) {
-					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-
-				} else {
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
-				}
-
-			} else if (_control_mode.flag_control_rates_enabled) {
-				/* rates control */
-				struct vehicle_rates_setpoint_s rates_sp;
-				memset(&rates_sp, 0, sizeof(rates_sp));
-
-				rates_sp.roll = offboard_control_sp.p1;
-				rates_sp.pitch = offboard_control_sp.p2;
-				rates_sp.yaw = offboard_control_sp.p3;
-				rates_sp.thrust = offboard_control_sp.p4;
-
-				rates_sp.timestamp = hrt_absolute_time();
-
-				if (_rates_sp_pub < 0) {
-					_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-
-				} else {
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
-				}
-
-			} else {
-				/* direct control */
-				// TODO
-			}
-		}
 	}
 }