introduce offboard control mode topic

Replace offboard_control_setpoint with offboard_control_mode
Remove all setpoint data from the topic as it's  not used anymore
(setpoint data is directly routed into position/attitude setpoint topics
for some time now)
Remove mode enum and replace with ignore booleans which map better to
the mavlink message
Mavlink: Rework parsing of offboard setpoints
Commander: in offboard mode set control flags based on ignore flags
instead of enum

3 files changed, 57 insertions, 121 deletions

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 9711d8fc3..7d6b60e22 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -51,7 +51,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#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_vicon_position.h>
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 90c3cb47f..3a5ae47ab 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -108,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_range_pub(-1),
-	_offboard_control_sp_pub(-1),
+	_offboard_control_mode_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
@@ -517,8 +517,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	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
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
 	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
@@ -527,64 +527,24 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			 set_position_target_local_ned.target_component == 0)) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
-		switch (set_position_target_local_ned.coordinate_frame) {
-			case MAV_FRAME_LOCAL_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
-				break;
-			case MAV_FRAME_LOCAL_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
-				break;
-			case MAV_FRAME_BODY_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
-				break;
-			case MAV_FRAME_BODY_OFFSET_NED:
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
-				break;
-			default:
-				/* invalid setpoint, avoid publishing */
-				return;
-		}
-		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.yaw = set_position_target_local_ned.yaw;
-		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
-		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) {
-			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
-		}
-
-		/* set ignore flags */
-		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
-		}
+		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+		/* yaw ignore flag mapps to ignore_attitude */
+		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+		/* yawrate ignore flag mapps to ignore_bodyrate */
+		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
 
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-		if (set_position_target_local_ned.type_mask & (1 << 10)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
-		}
 
-		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-		if (set_position_target_local_ned.type_mask & (1 << 11)) {
-			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
-		}
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_mode.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -596,15 +556,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (offboard_control_sp.isForceSetpoint &&
-						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
-						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+				if (is_force_sp && offboard_control_mode.ignore_position &&
+						offboard_control_mode.ignore_velocity) {
 					/* The offboard setpoint is a force setpoint only, directly writing to the force
 					 * setpoint topic and not publishing the setpoint triplet topic */
 					struct vehicle_force_setpoint_s	force_sp;
-					force_sp.x = offboard_control_sp.acceleration[0];
-					force_sp.y = offboard_control_sp.acceleration[1];
-					force_sp.z = offboard_control_sp.acceleration[2];
+					force_sp.x = set_position_target_local_ned.afx;
+					force_sp.y = set_position_target_local_ned.afy;
+					force_sp.z = set_position_target_local_ned.afz;
 					//XXX: yaw
 					if (_force_sp_pub < 0) {
 						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
@@ -619,62 +578,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.current.valid = true;
 					pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
 
-					/* set the local pos values if the setpoint type is 'local pos' and none
-					 * of the local pos fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
+					/* set the local pos values */
+					if (!offboard_control_mode.ignore_position) {
 					pos_sp_triplet.current.position_valid = true;
-					pos_sp_triplet.current.x = offboard_control_sp.position[0];
-					pos_sp_triplet.current.y = offboard_control_sp.position[1];
-					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					pos_sp_triplet.current.x = set_position_target_local_ned.x;
+					pos_sp_triplet.current.y = set_position_target_local_ned.y;
+					pos_sp_triplet.current.z = set_position_target_local_ned.z;
 					} else {
 						pos_sp_triplet.current.position_valid = false;
 					}
 
-					/* set the local vel values if the setpoint type is 'local pos' and none
-					 * of the local vel fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
+					/* set the local vel values */
+					if (!offboard_control_mode.ignore_velocity) {
 						pos_sp_triplet.current.velocity_valid = true;
-						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
-						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
-						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
 					} else {
 						pos_sp_triplet.current.velocity_valid = false;
 					}
 
 					/* set the local acceleration values if the setpoint type is 'local pos' and none
 					 * of the accelerations fields is set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
+					if (!offboard_control_mode.ignore_acceleration_force) {
 						pos_sp_triplet.current.acceleration_valid = true;
-						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
-						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
-						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+						pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+						pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
 						pos_sp_triplet.current.acceleration_is_force =
-						offboard_control_sp.isForceSetpoint;
+						is_force_sp;
 
 					} else {
 						pos_sp_triplet.current.acceleration_valid = false;
 					}
 
-					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+					/* set the yaw sp value */
+					if (!offboard_control_mode.ignore_attitude) {
 						pos_sp_triplet.current.yaw_valid = true;
-						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
 					} else {
 						pos_sp_triplet.current.yaw_valid = false;
 					}
 
-					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
-					 * field is not set to 'ignore' */
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+					/* set the yawrate sp value */
+					if (!offboard_control_mode.ignore_bodyrate) {
 						pos_sp_triplet.current.yawspeed_valid = true;
-						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
 					} else {
 						pos_sp_triplet.current.yawspeed_valid = false;
@@ -743,42 +693,29 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 	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
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
 
 	/* Only accept messages which are intended for this system */
 	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] = set_attitude_target.q[i];
-		}
-		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;
 
 		/* set correct ignore flags for body rate fields: copy from mavlink message */
-		for (int i = 0; i < 3; i++) {
-			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
-			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
-		}
+		offboard_control_mode.ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
 		/* set correct ignore flags for thrust field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
+		offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
 		/* set correct ignore flags for attitude field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
-
+		offboard_control_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
-		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+		offboard_control_mode.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+		if (_offboard_control_mode_pub < 0) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
 
 		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+			orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
 		}
 
 		/* If we are in offboard control mode and offboard control loop through is enabled
@@ -793,8 +730,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 			if (_control_mode.flag_control_offboard_enabled) {
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
-				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+				if (!(offboard_control_mode.ignore_attitude ||
+							offboard_control_mode.ignore_thrust)) {
 					struct vehicle_attitude_setpoint_s att_sp;
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
@@ -814,8 +751,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
 				///XXX add support for ignoring individual axes
-				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+				if (!(offboard_control_mode.ignore_bodyrate ||
+							offboard_control_mode.ignore_thrust)) {
 					struct vehicle_rates_setpoint_s rates_sp;
 					rates_sp.timestamp = hrt_absolute_time();
 					rates_sp.roll = set_attitude_target.body_roll_rate;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 699996860..15943753d 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -53,7 +53,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -142,7 +142,7 @@ private:
 	/**
 	* Exponential moving average filter to smooth time offset
 	*/
-	void smooth_time_offset(uint64_t offset_ns);	
+	void smooth_time_offset(uint64_t offset_ns);
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
@@ -162,7 +162,7 @@ private:
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
-	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _offboard_control_mode_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;