Merge pull request #1786 from PX4/offboardmode

Replace offboard control setpoint topic with simple offboard control mode topic, remove setpoint data from topic

src/modules/commander/commander.cpp

@@ -65,7 +65,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -180,7 +180,7 @@ static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
-static struct offboard_control_setpoint_s sp_offboard;
+static struct offboard_control_mode_s offboard_control_mode;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&sp_man, 0, sizeof(sp_man));
 
 	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
 
 	/* Subscribe to telemetry status topics */
 	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
@@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
 		}
 
-		orb_check(sp_offboard_sub, &updated);
+		orb_check(offboard_control_mode_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+			orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
 		}
 
-		if (sp_offboard.timestamp != 0 &&
+		if (offboard_control_mode.timestamp != 0 &&
-		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+		    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
 			if (status.offboard_control_signal_lost) {
 				status.offboard_control_signal_lost = false;
 				status_changed = true;
@@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[])
 	led_deinit();
 	buzzer_deinit();
 	close(sp_man_sub);
-	close(sp_offboard_sub);
+	close(offboard_control_mode_sub);
 	close(local_position_sub);
 	close(global_position_sub);
 	close(gps_sub);
@@ -2426,56 +2426,30 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_offboard_enabled = true;
 
-		switch (sp_offboard.mode) {
+		/*
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+		 * The control flags depend on what is ignored according to the offboard control mode topic
-			control_mode.flag_control_rates_enabled = true;
+		 * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
-			control_mode.flag_control_attitude_enabled = false;
+		 */
-			control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
-			control_mode.flag_control_climb_rate_enabled = false;
+			!offboard_control_mode.ignore_attitude ||
-			control_mode.flag_control_position_enabled = false;
+			!offboard_control_mode.ignore_position ||
-			control_mode.flag_control_velocity_enabled = false;
+			!offboard_control_mode.ignore_velocity ||
-			break;
+			!offboard_control_mode.ignore_acceleration_force;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+		control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
-			control_mode.flag_control_rates_enabled = true;
+			!offboard_control_mode.ignore_position ||
-			control_mode.flag_control_attitude_enabled = true;
+			!offboard_control_mode.ignore_velocity ||
-			control_mode.flag_control_altitude_enabled = false;
+			!offboard_control_mode.ignore_acceleration_force;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+		control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity ||
-			control_mode.flag_control_rates_enabled = true;
+			!offboard_control_mode.ignore_position;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
 
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+			!offboard_control_mode.ignore_position;
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
 
-		default:
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
-			control_mode.flag_control_rates_enabled = false;
+
-			control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
 
 		break;
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1077,6 +1077,10 @@ FixedwingAttitudeControl::task_main()
 			_actuators_airframe.timestamp = hrt_absolute_time();
 			_actuators_airframe.timestamp_sample = _att.timestamp;
 
+			/* Only publish if any of the proper modes are enabled */
+			if(_vcontrol_mode.flag_control_rates_enabled ||
+			   _vcontrol_mode.flag_control_attitude_enabled)
+			{
 				/* publish the actuator controls */
 				if (_actuators_0_pub > 0) {
 					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
@@ -1093,6 +1097,7 @@ FixedwingAttitudeControl::task_main()
 					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
 				}
 			}
+		}
 
 		loop_counter++;
 		perf_end(_loop_perf);

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>

src/modules/mavlink/mavlink_receiver.cpp

@@ -108,7 +108,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_range_pub(-1),
-	_offboard_control_sp_pub(-1),
+	_offboard_control_mode_pub(-1),
+	_actuator_controls_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
@@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_attitude_target(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
+		handle_message_set_actuator_control_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -517,8 +522,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;
+	struct offboard_control_mode_s offboard_control_mode;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
+	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 +532,22 @@ 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) {
+		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
-			case MAV_FRAME_LOCAL_NED:
+		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
+		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
-				break;
+		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
-			case MAV_FRAME_LOCAL_OFFSET_NED:
+		/* yaw ignore flag mapps to ignore_attitude */
-				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
+		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
-				break;
+		/* yawrate ignore flag mapps to ignore_bodyrate */
-			case MAV_FRAME_BODY_NED:
+		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
-				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 */
+		offboard_control_mode.timestamp = hrt_absolute_time();
-		if (offboard_control_sp.isForceSetpoint) {
-			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
-		}
 
-		/* set ignore flags */
+		if (_offboard_control_mode_pub < 0) {
-		for (int i = 0; i < 9; i++) {
+			_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
-			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
-		}
-
-		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();
-
-		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);
+			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 +559,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 &&
+				if (is_force_sp && offboard_control_mode.ignore_position &&
-						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
+						offboard_control_mode.ignore_velocity) {
-						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
 					/* 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.x = set_position_target_local_ned.afx;
-					force_sp.y = offboard_control_sp.acceleration[1];
+					force_sp.y = set_position_target_local_ned.afy;
-					force_sp.z = offboard_control_sp.acceleration[2];
+					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 +581,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
+					/* set the local pos values */
-					 * of the local pos fields is set to 'ignore' */
+					if (!offboard_control_mode.ignore_position) {
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
 						pos_sp_triplet.current.position_valid = true;
-					pos_sp_triplet.current.x = offboard_control_sp.position[0];
+						pos_sp_triplet.current.x = set_position_target_local_ned.x;
-					pos_sp_triplet.current.y = offboard_control_sp.position[1];
+						pos_sp_triplet.current.y = set_position_target_local_ned.y;
-					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+						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
+					/* set the local vel values */
-					 * of the local vel fields is set to 'ignore' */
+					if (!offboard_control_mode.ignore_velocity) {
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
 						pos_sp_triplet.current.velocity_valid = true;
-						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
+						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
-						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
+						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
-						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+						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 &&
+					if (!offboard_control_mode.ignore_acceleration_force) {
-							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
 						pos_sp_triplet.current.acceleration_valid = true;
-						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
+						pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
-						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
+						pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
-						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						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
+					/* set the yaw sp value */
-					 * field is not set to 'ignore' */
+					if (!offboard_control_mode.ignore_attitude) {
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
 						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
+					/* set the yawrate sp value */
-					 * field is not set to 'ignore' */
+					if (!offboard_control_mode.ignore_bodyrate) {
-					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
 						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;
@@ -698,6 +651,66 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	}
 }
 
+void
+MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
+{
+	mavlink_set_actuator_control_target_t set_actuator_control_target;
+	mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
+
+	struct offboard_control_mode_s offboard_control_mode;
+	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+	
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
+	
+	if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
+		    set_actuator_control_target.target_system == 0) &&
+		(mavlink_system.compid == set_actuator_control_target.target_component ||
+		    set_actuator_control_target.target_component == 0)) {
+
+		/* ignore all since we are setting raw actuators here */
+		offboard_control_mode.ignore_thrust             = true;
+		offboard_control_mode.ignore_attitude           = true;
+		offboard_control_mode.ignore_bodyrate           = true;
+		offboard_control_mode.ignore_position           = true;
+		offboard_control_mode.ignore_velocity           = true;
+		offboard_control_mode.ignore_acceleration_force = true;
+
+		offboard_control_mode.timestamp = hrt_absolute_time();
+
+		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_mode), _offboard_control_mode_pub, &offboard_control_mode);
+		}
+
+
+		/* If we are in offboard control mode, publish the actuator controls */
+		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) {
+
+			actuator_controls.timestamp = hrt_absolute_time();
+
+			/* Set duty cycles for the servos in actuator_controls_0 */
+			for(size_t i = 0; i < 8; i++) {
+				actuator_controls.control[i] = set_actuator_control_target.controls[i];
+			}
+
+			if (_actuator_controls_pub < 0) {
+				_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
+			} else {
+				orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
+			}
+		}
+	}
+
+}
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
@@ -743,42 +756,52 @@ 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;
+	static struct offboard_control_mode_s offboard_control_mode = {};
-	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 == 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;
-		}
 		/* set correct ignore flags for thrust field: copy from mavlink message */
-		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
+		offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
-		/* 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);
 
+		/*
+		 * The tricky part in pasrsing this message is that the offboard sender can set attitude and thrust
+		 * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore
+		 * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits
+		 * set for everything else.
+		 */
 
-		offboard_control_sp.timestamp = hrt_absolute_time();
+		/*
-		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+		 * if attitude or body rate have been used (not ignored) previously and this message only sends
+		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+		 * body rates to keep the controllers running
+		 */
+		bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		if (_offboard_control_sp_pub < 0) {
+		if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+			offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
+			offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
+		} else {
+			offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+			offboard_control_mode.ignore_attitude = ignore_attitude;
+		}
+
+		offboard_control_mode.ignore_position = true;
+		offboard_control_mode.ignore_velocity = true;
+		offboard_control_mode.ignore_acceleration_force = true;
+
+		offboard_control_mode.timestamp = hrt_absolute_time();
+
+		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,15 +816,16 @@ 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) ||
+				if (!(offboard_control_mode.ignore_attitude)) {
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					static struct vehicle_attitude_setpoint_s att_sp = {};
-					struct vehicle_attitude_setpoint_s att_sp;
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
 					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
 					att_sp.R_valid = true;
+					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 						att_sp.thrust = set_attitude_target.thrust;
+					}
 					att_sp.yaw_sp_move_rate = 0.0;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
 					att_sp.q_d_valid = true;
@@ -814,14 +838,15 @@ 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) ||
+				if (!(offboard_control_mode.ignore_bodyrate)) {
-							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					static struct vehicle_rates_setpoint_s rates_sp = {};
-					struct vehicle_rates_setpoint_s rates_sp;
 					rates_sp.timestamp = hrt_absolute_time();
 					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;
+					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 						rates_sp.thrust = set_attitude_target.thrust;
+					}
 
 					if (_att_sp_pub < 0) {
 						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);

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>
@@ -122,6 +122,7 @@ private:
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
@@ -162,7 +163,8 @@ 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 _actuator_controls_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;

src/modules/uORB/objects_common.cpp

@@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 #include "topics/vehicle_control_debug.h"
 ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
 
-#include "topics/offboard_control_setpoint.h"
+#include "topics/offboard_control_mode.h"
-ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
+ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s);
 
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);

src/modules/uORB/topics/offboard_control_mode.h

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *	 Copyright (C) 2008-2015 PX4 Development Team. All rights reserved.
+ *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *		notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *		notice, this list of conditions and the following disclaimer in
+ *		the documentation and/or other materials provided with the
+ *		distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *		used to endorse or promote products derived from this software
+ *		without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file offboard_control_mode.h
+ * Definition of the manual_control_setpoint uORB topic.
+ */
+
+#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_
+#define TOPIC_OFFBOARD_CONTROL_MODE_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * Off-board control mode
+ */
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct offboard_control_mode_s {
+	uint64_t timestamp;
+
+	bool ignore_thrust;
+	bool ignore_attitude;
+	bool ignore_bodyrate;
+	bool ignore_position;
+	bool ignore_velocity;
+	bool ignore_acceleration_force;
+
+}; /**< offboard control inputs */
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(offboard_control_mode);
+
+#endif

src/modules/uORB/topics/offboard_control_setpoint.h

@@ -1,276 +0,0 @@
-/****************************************************************************
- *
- *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *		notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *		notice, this list of conditions and the following disclaimer in
- *		the documentation and/or other materials provided with the
- *		distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *		used to endorse or promote products derived from this software
- *		without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file offboard_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Off-board control inputs.
- *
- * Typically sent by a ground control station / joystick or by
- * some off-board controller via C or SIMULINK.
- */
-enum OFFBOARD_CONTROL_MODE {
-	OFFBOARD_CONTROL_MODE_DIRECT = 0,
-	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
-	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
-	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
-	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
-	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
-};
-
-enum OFFBOARD_CONTROL_FRAME {
-	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
-	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
-	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
-	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
-	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
-};
-
-/* mappings for the ignore bitmask */
-enum {OFB_IGN_BIT_POS_X,
-	OFB_IGN_BIT_POS_Y,
-	OFB_IGN_BIT_POS_Z,
-	OFB_IGN_BIT_VEL_X,
-	OFB_IGN_BIT_VEL_Y,
-	OFB_IGN_BIT_VEL_Z,
-	OFB_IGN_BIT_ACC_X,
-	OFB_IGN_BIT_ACC_Y,
-	OFB_IGN_BIT_ACC_Z,
-	OFB_IGN_BIT_BODYRATE_X,
-	OFB_IGN_BIT_BODYRATE_Y,
-	OFB_IGN_BIT_BODYRATE_Z,
-	OFB_IGN_BIT_ATT,
-	OFB_IGN_BIT_THRUST,
-	OFB_IGN_BIT_YAW,
-	OFB_IGN_BIT_YAWRATE,
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct offboard_control_setpoint_s {
-	uint64_t timestamp;
-
-	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
-
-	double position[3];	/**< lat, lon, alt / x, y, z */
-	float velocity[3];	/**< x vel, y vel, z vel */
-	float acceleration[3];	/**< x acc, y acc, z acc */
-	float attitude[4];	/**< attitude of vehicle (quaternion) */
-	float attitude_rate[3];	/**< body angular rates (x, y, z) */
-	float thrust;	/**< thrust */
-	float yaw;	/**< yaw: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-	float yaw_rate;	/**< yaw rate: this is the yaw from the position_target message
-			  (not from the full attitude_target message) */
-
-	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
-			   for mapping */
-
-	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
-
-	float override_mode_switch;
-
-	float aux1_cam_pan_flaps;
-	float aux2_cam_tilt;
-	float aux3_cam_zoom;
-	float aux4_cam_roll;
-
-}; /**< offboard control inputs */
-/**
- * @}
- */
-
-/**
- * Returns true if the position setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X +  index)));
-}
-
-/**
- * Returns true if all position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some position setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the velocity setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
-}
-
-/**
- * Returns true if all velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some velocity setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the acceleration setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
-}
-
-/**
- * Returns true if all acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return false;
-		}
-	}
-	return true;
-}
-
-/**
- * Returns true if some acceleration setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the bodyrate setpoint at index should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
-}
-
-/**
- * Returns true if some of the bodyrate setpoints should be ignored
- */
-inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	for (int i = 0; i < 3; i++) {
-		if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i))	{
-			return true;
-		}
-	}
-	return false;
-}
-
-/**
- * Returns true if the attitude setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
-}
-
-/**
- * Returns true if the thrust setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
-}
-
-/**
- * Returns true if the yaw setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
-}
-
-/**
- * Returns true if the yaw rate setpoint should be ignored
- */
-inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
-}
-
-
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_control_setpoint);
-
-#endif

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -782,6 +782,10 @@ void VtolAttitudeControl::task_main()
 				fill_mc_att_control_output();
 				fill_mc_att_rates_sp();
 
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
 					if (_actuators_0_pub > 0) {
 						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
@@ -797,6 +801,7 @@ void VtolAttitudeControl::task_main()
 					}
 				}
 			}
+		}
 
 		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
 			_vtol_vehicle_status.vtol_in_rw_mode = false;
@@ -813,6 +818,10 @@ void VtolAttitudeControl::task_main()
 				fill_fw_att_control_output();
 				fill_fw_att_rates_sp();
 
+				/* Only publish if the proper mode(s) are enabled */
+				if(_v_control_mode.flag_control_attitude_enabled ||
+				   _v_control_mode.flag_control_rates_enabled)
+				{
 					if (_actuators_0_pub > 0) {
 						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
@@ -828,6 +837,7 @@ void VtolAttitudeControl::task_main()
 					}
 				}
 			}
+		}
 
 		// publish the attitude rates setpoint
 		if(_v_rates_sp_pub > 0) {