MAVLink/Commander: @Pedro-Roque's offboard yawrate handling

msg/offboard_control_mode.msg

@@ -5,6 +5,9 @@ uint64 timestamp		# time since system start (microseconds)
 bool ignore_thrust
 bool ignore_attitude
 bool ignore_bodyrate
+bool ignore_bodyrate_x
+bool ignore_bodyrate_y
+bool ignore_bodyrate_z
 bool ignore_position
 bool ignore_velocity
 bool ignore_acceleration_force

msg/vehicle_control_mode.msg

@@ -8,6 +8,7 @@ bool flag_control_auto_enabled			# true if onboard autopilot should act
 bool flag_control_offboard_enabled		# true if offboard control should be used
 bool flag_control_rates_enabled			# true if rates are stabilized
 bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
+bool flag_control_yawrate_override_enabled		# true if the yaw rate override is enabled
 bool flag_control_rattitude_enabled		# true if rate/attitude stabilization is enabled
 bool flag_control_force_enabled			# true if force control is mixed in
 bool flag_control_acceleration_enabled			# true if acceleration is controlled

src/modules/commander/Commander.cpp

@@ -3322,7 +3322,10 @@ set_control_mode()
 		 * The control flags depend on what is ignored according to the offboard control mode topic
 		 * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
 		 */
-		control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
+		control_mode.flag_control_rates_enabled =
+				!offboard_control_mode.ignore_bodyrate_x ||
+				!offboard_control_mode.ignore_bodyrate_y ||
+				!offboard_control_mode.ignore_bodyrate_z ||
 				!offboard_control_mode.ignore_attitude ||
 				!offboard_control_mode.ignore_position ||
 				!offboard_control_mode.ignore_velocity ||
@@ -3333,6 +3336,12 @@ set_control_mode()
 				!offboard_control_mode.ignore_velocity ||
 				!offboard_control_mode.ignore_acceleration_force;
 
+		control_mode.flag_control_yawrate_override_enabled =
+				offboard_control_mode.ignore_bodyrate_x &&
+				offboard_control_mode.ignore_bodyrate_y &&
+				!offboard_control_mode.ignore_bodyrate_z &&
+				!offboard_control_mode.ignore_attitude;
+
 		control_mode.flag_control_rattitude_enabled = false;
 
 		control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&

src/modules/mavlink/mavlink_receiver.cpp

@@ -826,8 +826,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		/* 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_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
+		offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800);
+		offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800);
+		offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800);
 
 		bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
 		bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
@@ -938,7 +940,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yawrate sp value */
-					if (!offboard_control_mode.ignore_bodyrate) {
+					if (!(offboard_control_mode.ignore_bodyrate_x ||
+					      offboard_control_mode.ignore_bodyrate_y ||
+					      offboard_control_mode.ignore_bodyrate_z)) {
 						pos_sp_triplet.current.yawspeed_valid = true;
 						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
@@ -994,7 +998,9 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
 		/* 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_bodyrate_x         = true;
+		offboard_control_mode.ignore_bodyrate_y         = true;
+		offboard_control_mode.ignore_bodyrate_z         = true;
 		offboard_control_mode.ignore_position           = true;
 		offboard_control_mode.ignore_velocity           = true;
 		offboard_control_mode.ignore_acceleration_force = true;
@@ -1289,16 +1295,28 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		 * 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_msg = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & 0x1);
+		bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & 0x2);
+		bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & 0x4);
 		bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
+
+		if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y ||
+		     ignore_bodyrate_msg_z) &&
+		    ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
 			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
+			_offboard_control_mode.ignore_bodyrate_x =
+				ignore_bodyrate_msg_x && _offboard_control_mode.ignore_bodyrate_x;
+			_offboard_control_mode.ignore_bodyrate_y =
+				ignore_bodyrate_msg_y && _offboard_control_mode.ignore_bodyrate_y;
+			_offboard_control_mode.ignore_bodyrate_z =
+				ignore_bodyrate_msg_z && _offboard_control_mode.ignore_bodyrate_z;
 			_offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
 
 		} else {
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
+			_offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x;
+			_offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y;
+			_offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z;
 			_offboard_control_mode.ignore_attitude = ignore_attitude_msg;
 		}
 
@@ -1357,13 +1375,22 @@ 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_mode.ignore_bodyrate)) {
+				if (!_offboard_control_mode.ignore_bodyrate_x ||
+				    !_offboard_control_mode.ignore_bodyrate_y ||
+				    !_offboard_control_mode.ignore_bodyrate_z) {
 					vehicle_rates_setpoint_s rates_sp = {};
 					rates_sp.timestamp = hrt_absolute_time();
 
-					if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
+					// only copy att rates sp if message contained new data
+					if (!ignore_bodyrate_msg_x) {
 						rates_sp.roll = set_attitude_target.body_roll_rate;
+					}
+
+					if (!ignore_bodyrate_msg_y) {
 						rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					}
+
+					if (!ignore_bodyrate_msg_z) {
 						rates_sp.yaw = set_attitude_target.body_yaw_rate;
 					}
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -837,6 +837,14 @@ MulticopterAttitudeControl::run()
 					}
 
 					control_attitude();
+					if (_v_control_mode.flag_control_yawrate_override_enabled)
+					{
+						/* Yaw rate override enabled, overwrite the yaw setpoint */
+						vehicle_rates_setpoint_poll();
+						const auto yawrate_reference = _v_rates_sp.yaw;
+						_rates_sp(2) = yawrate_reference;
+					}
+
 					publish_rates_setpoint();
 				}