manual_control_setpoint: change x, y, z, r naming

In review it was requested to have a different name for
manual_control_setpoint.z because of the adjusted range.
It was agreed to switch to an array for stick inputs called xyzr.

Author: MaEtUgR

msg/manual_control_setpoint.msg

@@ -12,31 +12,17 @@ uint8 data_source		 # where this input is coming from
 
 # Any of the channels may not be available and be set to NaN
 # to indicate that it does not contain valid data.
-# The variable names follow the definition of the
-# MANUAL_CONTROL mavlink message.
-# The default range is from -1 to 1 (mavlink message -1000 to 1000)
-# The range for the z variable is defined from 0 to 1. (The z field of
-# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
-
-float32 x			 # stick position in x direction -1..1
-				 # in general corresponds to forward/back motion or pitch of vehicle,
-				 # in general a positive value means forward or negative pitch and
-				 # a negative value means backward or positive pitch
-
-float32 y			 # stick position in y direction -1..1
-				 # in general corresponds to right/left motion or roll of vehicle,
-				 # in general a positive value means right or positive roll and
-				 # a negative value means left or negative roll
-
-float32 z			 # throttle stick position -1..1
-				 # in general corresponds to up/down motion or thrust of vehicle,
-				 # in general the value corresponds to the demanded throttle by the user,
-				 # if the input is used for setting the setpoint of a vertical position
-				 # controller any value > 0 means up and any value < 0 means down
-
-float32 r			 # yaw stick/twist position, -1..1
-				 # in general corresponds to the righthand rotation around the vertical
-				 # (downwards) axis of the vehicle
+
+# Stick positions [-1,1]
+# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right
+# Positive directions are generally used for:
+# x - forward, negative pitch,  nose down
+# y - right,   positive roll,   right side down
+# z - up,      positive thrust, -1 is 0% thrust +1 is 100% thrust
+# r - righthand rotation around vertical downwards axis (= clockwise yaw rotation when seen from above)
+# The definition is analogous to the MANUAL_CONTROL mavlink message's x, y, z, r fields.
+# Note: QGC sends z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible.
+float32[4] xyzr
 
 float32 flaps			 # flap position
 
@@ -47,3 +33,7 @@ float32 aux4			 # default function: camera roll
 float32 aux5			 # default function: payload drop
 float32 aux6
 
+# DEPRECATED: float32 x
+# DEPRECATED: float32 y
+# DEPRECATED: float32 z
+# DEPRECATED: float32 r

src/examples/fixedwing_control/main.cpp

@@ -386,9 +386,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				}
 
 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
-				if (PX4_ISFINITE(manual_control_setpoint.z) &&
-				    (manual_control_setpoint.z >= .2f) &&
-				    (manual_control_setpoint.z <= 1.f)) {
+				if (PX4_ISFINITE(manual_control_setpoint.xyzr[2]) &&
+				    (manual_control_setpoint.xyzr[2] >= .2f) &&
+				    (manual_control_setpoint.xyzr[2] <= 1.f)) {
 				}
 
 				/* get the system status and the flight mode we're in */

src/lib/flight_tasks/tasks/Utility/Sticks.cpp

@@ -51,10 +51,8 @@ bool Sticks::checkAndSetStickInputs()
 
 	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 		// Linear scale
-		_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
-		_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
-		_positions(2) = -manual_control_setpoint.z; // NED z, thrust resacaled from [0,1] to [-1,1]
-		_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
+		_positions = matrix::Vector<float, 4>(manual_control_setpoint.xyzr);
+		_positions(2) = -_positions(2); // NED z direction is down not up
 
 		// Exponential scale
 		_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());

src/modules/airship_att_control/airship_att_control_main.cpp

@@ -91,9 +91,9 @@ AirshipAttitudeControl::publish_actuator_controls()
 
 	} else {
 		_actuators.control[0] = 0.0f;
-		_actuators.control[1] = _manual_control_setpoint.x;
-		_actuators.control[2] = _manual_control_setpoint.r;
-		_actuators.control[3] = (_manual_control_setpoint.z + 1.f) * .5f;
+		_actuators.control[1] = _manual_control_setpoint.xyzr[0];
+		_actuators.control[2] = _manual_control_setpoint.xyzr[3];
+		_actuators.control[3] = (_manual_control_setpoint.xyzr[2] + 1.f) * .5f;
 	}
 
 	// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()

src/modules/commander/Commander.cpp

@@ -705,8 +705,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							break;
 						}
 
-						const bool throttle_above_low = (_manual_control_setpoint.z > -.8f);
-						const bool throttle_above_center = (_manual_control_setpoint.z > .2f);
+						const bool throttle_above_low = (_manual_control_setpoint.xyzr[2] > -.8f);
+						const bool throttle_above_center = (_manual_control_setpoint.xyzr[2] > .2f);
 
 						if (cmd_arms && throttle_above_center &&
 						    (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
@@ -2092,8 +2092,8 @@ Commander::run()
 
 			// transition to previous state if sticks are touched
 			if (!_status.rc_signal_lost &&
-			    ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
-			     (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
+			    ((fabsf(_manual_control_setpoint.xyzr[0]) > minimum_stick_deflection) ||
+			     (fabsf(_manual_control_setpoint.xyzr[1]) > minimum_stick_deflection))) {
 				// revert to position control in any case
 				main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
 				mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
@@ -2152,8 +2152,8 @@ Commander::run()
 			 * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
 			 * do it only for rotary wings in manual mode or fixed wing if landed.
 			 * Disable stick-disarming if arming switch or button is mapped */
-			const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
-							 && (_manual_control_setpoint.z < -.8f)
+			const bool stick_in_lower_left = _manual_control_setpoint.xyzr[3] < -STICK_ON_OFF_LIMIT
+							 && (_manual_control_setpoint.xyzr[2] < -.8f)
 							 && !arm_switch_or_button_mapped;
 			const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
 					(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
@@ -2190,7 +2190,8 @@ Commander::run()
 			 * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
 			 * and we're in MANUAL mode.
 			 * Disable stick-arming if arming switch or button is mapped */
-			const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < -.8f
+			const bool stick_in_lower_right = _manual_control_setpoint.xyzr[3] > STICK_ON_OFF_LIMIT
+							  && _manual_control_setpoint.xyzr[2] < -.8f
 							  && !arm_switch_or_button_mapped;
 			/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
 			 * for example for accidental in-air disarming */
@@ -2200,7 +2201,7 @@ Commander::run()
 			const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
 					(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF) &&
 					(_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
-					(_manual_control_setpoint.z < -.8f || in_rearming_grace_period);
+					(_manual_control_setpoint.xyzr[2] < -.8f || in_rearming_grace_period);
 
 			if (!in_armed_state &&
 			    (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&

src/modules/commander/rc_calibration.cpp

@@ -82,15 +82,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
 	/* set parameters: the new trim values are the combination of active trim values
 	   and the values coming from the remote control of the user
 	*/
-	float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
+	float p = manual_control_setpoint.xyzr[1] * roll_scale + roll_trim_active;
 	int p1r = param_set(param_find("TRIM_ROLL"), &p);
 	/*
 	 we explicitly swap sign here because the trim is added to the actuator controls
 	 which are moving in an inverse sense to manual pitch inputs
 	*/
-	p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
+	p = -manual_control_setpoint.xyzr[0] * pitch_scale + pitch_trim_active;
 	int p2r = param_set(param_find("TRIM_PITCH"), &p);
-	p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
+	p = manual_control_setpoint.xyzr[3] * yaw_scale + yaw_trim_active;
 	int p3r = param_set(param_find("TRIM_YAW"), &p);
 
 	if (p1r != 0 || p2r != 0 || p3r != 0) {

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -145,24 +145,25 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 
 			// Check if we are in rattitude mode and the pilot is above the threshold on pitch
 			if (_vcontrol_mode.flag_control_rattitude_enabled) {
-				if (fabsf(_manual_control_setpoint.y) > _param_fw_ratt_th.get()
-				    || fabsf(_manual_control_setpoint.x) > _param_fw_ratt_th.get()) {
+				if (fabsf(_manual_control_setpoint.xyzr[1]) > _param_fw_ratt_th.get()
+				    || fabsf(_manual_control_setpoint.xyzr[0]) > _param_fw_ratt_th.get()) {
 					_vcontrol_mode.flag_control_attitude_enabled = false;
 				}
 			}
 
 			if (!_vcontrol_mode.flag_control_climb_rate_enabled &&
 			    !_vcontrol_mode.flag_control_offboard_enabled) {
-				const float throttle = (_manual_control_setpoint.z + 1.f) * .5f;
+				const float throttle = (_manual_control_setpoint.xyzr[2] + 1.f) * .5f;
 
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
 					// STABILIZED mode generate the attitude setpoint from manual user inputs
 
-					_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get());
+					_att_sp.roll_body = _manual_control_setpoint.xyzr[1] * radians(_param_fw_man_r_max.get())
+							    + radians(_param_fw_rsp_off.get());
 					_att_sp.roll_body = constrain(_att_sp.roll_body,
 								      -radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get()));
 
-					_att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get())
+					_att_sp.pitch_body = -_manual_control_setpoint.xyzr[0] * radians(_param_fw_man_p_max.get())
 							     + radians(_param_fw_psp_off.get());
 					_att_sp.pitch_body = constrain(_att_sp.pitch_body,
 								       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
@@ -182,21 +183,21 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 
 					// RATE mode we need to generate the rate setpoint from manual user inputs
 					_rates_sp.timestamp = hrt_absolute_time();
-					_rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
-					_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
-					_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
+					_rates_sp.roll = _manual_control_setpoint.xyzr[1] * radians(_param_fw_acro_x_max.get());
+					_rates_sp.pitch = -_manual_control_setpoint.xyzr[0] * radians(_param_fw_acro_y_max.get());
+					_rates_sp.yaw = _manual_control_setpoint.xyzr[3] * radians(_param_fw_acro_z_max.get());
 					_rates_sp.thrust_body[0] = throttle;
 
 					_rate_sp_pub.publish(_rates_sp);
 
 				} else {
 					/* manual/direct control */
 					_actuators.control[actuator_controls_s::INDEX_ROLL] =
-						_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
+						_manual_control_setpoint.xyzr[1] * _param_fw_man_r_sc.get() + _param_trim_roll.get();
 					_actuators.control[actuator_controls_s::INDEX_PITCH] =
-						-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
+						-_manual_control_setpoint.xyzr[0] * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
 					_actuators.control[actuator_controls_s::INDEX_YAW] =
-						_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
+						_manual_control_setpoint.xyzr[3] * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
 					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
 				}
 			}
@@ -552,7 +553,7 @@ void FixedwingAttitudeControl::Run()
 
 					/* add in manual rudder control in manual modes */
 					if (_vcontrol_mode.flag_control_manual_enabled) {
-						_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
+						_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.xyzr[3];
 					}
 
 					if (!PX4_ISFINITE(yaw_u)) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -207,15 +207,15 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 {
 	_manual_control_setpoint_sub.update(&_manual_control_setpoint);
 
-	_manual_control_setpoint_altitude = _manual_control_setpoint.x;
-	_manual_control_setpoint_airspeed = _manual_control_setpoint.z;
+	_manual_control_setpoint_altitude = _manual_control_setpoint.xyzr[0];
+	_manual_control_setpoint_airspeed = _manual_control_setpoint.xyzr[2];
 
 	if (_param_fw_posctl_inv_st.get()) {
 		/* Alternate stick allocation (similar concept as for multirotor systems:
 		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
 		 */
-		_manual_control_setpoint_altitude = -_manual_control_setpoint.z;
-		_manual_control_setpoint_airspeed = _manual_control_setpoint.x;
+		_manual_control_setpoint_altitude = -_manual_control_setpoint.xyzr[2];
+		_manual_control_setpoint_airspeed = _manual_control_setpoint.xyzr[0];
 	}
 }
 
@@ -889,7 +889,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 
 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
-			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
+			_att_sp.roll_body = _manual_control_setpoint.xyzr[1] * radians(_param_fw_man_r_max.get());
 			_att_sp.yaw_body = 0;
 		}
 
@@ -925,8 +925,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					   tecs_status_s::TECS_MODE_NORMAL);
 
 		/* heading control */
-		if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
-		    fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
+		if (fabsf(_manual_control_setpoint.xyzr[1]) < HDG_HOLD_MAN_INPUT_THRESH &&
+		    fabsf(_manual_control_setpoint.xyzr[3]) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 			/* heading / roll is zero, lock onto current heading */
 			if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
@@ -977,12 +977,12 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			}
 		}
 
-		if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
-		    fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
+		if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.xyzr[1]) >= HDG_HOLD_MAN_INPUT_THRESH ||
+		    fabsf(_manual_control_setpoint.xyzr[3]) >= HDG_HOLD_MAN_INPUT_THRESH) {
 
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
-			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
+			_att_sp.roll_body = _manual_control_setpoint.xyzr[1] * radians(_param_fw_man_r_max.get());
 			_att_sp.yaw_body = 0;
 		}
 
@@ -1026,7 +1026,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 					   pitch_limit_min,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
-		_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
+		_att_sp.roll_body = _manual_control_setpoint.xyzr[1] * radians(_param_fw_man_r_max.get());
 		_att_sp.yaw_body = 0;
 
 	} else {

src/modules/mavlink/mavlink_messages.cpp

@@ -3621,10 +3621,10 @@ class MavlinkStreamManualControl : public MavlinkStream
 			mavlink_manual_control_t msg{};
 
 			msg.target = mavlink_system.sysid;
-			msg.x = manual_control_setpoint.x * 1000;
-			msg.y = manual_control_setpoint.y * 1000;
-			msg.z = manual_control_setpoint.z * 1000;
-			msg.r = manual_control_setpoint.r * 1000;
+			msg.x = manual_control_setpoint.xyzr[0] * 1e3f;
+			msg.y = manual_control_setpoint.xyzr[1] * 1e3f;
+			msg.z = manual_control_setpoint.xyzr[2] * 1e3f;
+			msg.r = manual_control_setpoint.xyzr[3] * 1e3f;
 
 			manual_control_switches_s manual_control_switches{};
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -2060,14 +2060,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 
 	} else {
 		manual_control_setpoint_s manual_control_setpoint{};
-
 		manual_control_setpoint.timestamp = hrt_absolute_time();
-		manual_control_setpoint.x = man.x / 1e3f;
-		manual_control_setpoint.y = man.y / 1e3f;
-		manual_control_setpoint.z = man.z / 1e3f;
-		manual_control_setpoint.r = man.r / 1e3f;
+		manual_control_setpoint.xyzr[0] = man.x / 1e3f;
+		manual_control_setpoint.xyzr[1] = man.y / 1e3f;
+		manual_control_setpoint.xyzr[2] = (man.z / 1e3f * 2.f) - 1.f; // backwards compatibility QGC
+		manual_control_setpoint.xyzr[3] = man.r / 1e3f;
 		manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
-
 		_manual_control_setpoint_pub.publish(manual_control_setpoint);
 	}
 }