Switch manual_control_setpoint.z scaling from [0,1] to [-1,1]

To be consistent with all other axes of stick input and avoid future
rescaling confusion.

Note: for the MAVLink message 69 MANUAL_CONTROL it's using the full range
according to the message specs now [-1000,1000].

msg/ManualControlSetpoint.msg

@@ -32,11 +32,11 @@ float32 y			 # stick position in y direction -1..1
 				 # in general a positive value means right or positive roll and
 				 # a negative value means left or negative roll
 
-float32 z			 # throttle stick position 0..1
+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.5 means up and any value < 0.5 means down
+				 # 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

src/lib/mixer_module/functions/FunctionManualRC.hpp

@@ -59,7 +59,7 @@ public:
 		if (_topic.update(&manual_control_setpoint)) {
 			_data[0] = manual_control_setpoint.y; // roll
 			_data[1] = manual_control_setpoint.x; // pitch
-			_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
+			_data[2] = manual_control_setpoint.z; // throttle
 			_data[3] = manual_control_setpoint.r; // yaw
 			_data[4] = manual_control_setpoint.flaps;
 			_data[5] = manual_control_setpoint.aux1;

src/modules/airship_att_control/airship_att_control_main.cpp

@@ -93,7 +93,7 @@ AirshipAttitudeControl::publish_actuator_controls()
 		_actuator_controls.control[0] = 0.0f;
 		_actuator_controls.control[1] = _manual_control_setpoint.x;
 		_actuator_controls.control[2] = _manual_control_setpoint.r;
-		_actuator_controls.control[3] = _manual_control_setpoint.z;
+		_actuator_controls.control[3] = (_manual_control_setpoint.z + 1.f) * .5f;
 	}
 
 	// note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run()

src/modules/commander/Commander.cpp

@@ -2714,8 +2714,8 @@ void Commander::manualControlCheck()
 
 	if (manual_control_updated && manual_control_setpoint.valid) {
 
-		_is_throttle_above_center = (manual_control_setpoint.z > 0.6f);
+		_is_throttle_above_center = (manual_control_setpoint.z > 0.2f);
-		_is_throttle_low = (manual_control_setpoint.z < 0.1f);
+		_is_throttle_low = (manual_control_setpoint.z < -0.8f);
 
 		if (_arm_state_machine.isArmed()) {
 			// Abort autonomous mode and switch to position mode if sticks are moved significantly

src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp

@@ -53,8 +53,7 @@ bool Sticks::checkAndUpdateStickInputs()
 		// 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) = -(math::constrain(manual_control_setpoint.z, 0.0f,
+		_positions(2) = -manual_control_setpoint.z; // NED z, thrust [-1,1]
-						  1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
 		_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
 
 		// Exponential scale

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -141,6 +141,8 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 
 			if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
 
+				const float throttle = (_manual_control_setpoint.z + 1.f) * .5f;
+
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
 					// STABILIZED mode generate the attitude setpoint from manual user inputs
 
@@ -152,7 +154,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 								       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
 
 					_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
-					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+					_att_sp.thrust_body[0] = throttle;
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -171,7 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 					_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.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+					_rates_sp.thrust_body[0] = throttle;
 
 					_rate_sp_pub.publish(_rates_sp);
 
@@ -183,8 +185,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 						-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
 					_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
 						_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
-					_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
+					_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
-							1.0f);
 					_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.r;
 				}
 			}

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -317,20 +317,20 @@ FixedwingPositionControl::manual_control_setpoint_poll()
 	_manual_control_setpoint_sub.update(&_manual_control_setpoint);
 
 	_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
-	_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+	_manual_control_setpoint_for_airspeed = _manual_control_setpoint.z;
 
 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
 		/* 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_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
+		_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.z;
-		_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
+		_manual_control_setpoint_for_airspeed = _manual_control_setpoint.x;
 	}
 
 	// send neutral setpoints if no update for 1 s
 	if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
 		_manual_control_setpoint_for_height_rate = 0.f;
-		_manual_control_setpoint_for_airspeed = 0.5f;
+		_manual_control_setpoint_for_airspeed = 0.f;
 	}
 }
 
@@ -379,18 +379,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
 
 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
 		// neutral throttle corresponds to trim airspeed
-		if (_manual_control_setpoint_for_airspeed < 0.5f) {
+		return math::interpolateNXY(_manual_control_setpoint_for_airspeed,
-			// lower half of throttle is min to trim airspeed
+		{-1.f, 0.f, 1.f},
-			altctrl_airspeed = _param_fw_airspd_min.get() +
+		{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
-					   (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
-					   _manual_control_setpoint_for_airspeed * 2;
-
-		} else {
-			// upper half of throttle is trim to max airspeed
-			altctrl_airspeed = _param_fw_airspd_trim.get() +
-					   (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
-					   (_manual_control_setpoint_for_airspeed * 2 - 1);
-		}
 
 	} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
 		altctrl_airspeed = _commanded_manual_airspeed_setpoint;
@@ -1910,7 +1901,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 	float throttle_max = _param_fw_thr_max.get();
 
 	// enable the operator to kill the throttle on ground
-	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 
@@ -1958,7 +1949,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	float throttle_max = _param_fw_thr_max.get();
 
 	// enable the operator to kill the throttle on ground
-	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
+	if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -127,7 +127,7 @@ static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
 static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
 
 // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float THROTTLE_THRESH = 0.05f;
+static constexpr float THROTTLE_THRESH = -.9f;
 
 // [m/s/s] slew rate limit for airspeed setpoint changes
 static constexpr float ASPD_SP_SLEW_RATE = 1.f;
@@ -293,7 +293,7 @@ private:
 	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
 	float _manual_control_setpoint_for_height_rate{0.0f};
 
-	// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
+	// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
 	float _manual_control_setpoint_for_airspeed{0.0f};
 
 	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -24,7 +24,6 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/vehicle_air_data.h>

src/modules/manual_control/ManualControl.cpp

@@ -162,15 +162,10 @@ void ManualControl::Run()
 		const float dt_s = (now - _last_time) / 1e6f;
 		const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
 
-		const bool rpy_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
+		_selector.setpoint().sticks_moving = (fabsf(_x_diff.update(_selector.setpoint().x, dt_s)) > minimum_stick_change)
-					|| (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
+						     || (fabsf(_y_diff.update(_selector.setpoint().y, dt_s)) > minimum_stick_change)
-					|| (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change);
+						     || (fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) > minimum_stick_change)
-
+						     || (fabsf(_r_diff.update(_selector.setpoint().r, dt_s)) > minimum_stick_change);
-		// Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
-		const bool throttle_moving =
-			(fabsf(_z_diff.update(_selector.setpoint().z, dt_s)) * 2.f) > minimum_stick_change;
-
-		_selector.setpoint().sticks_moving = rpy_moving || throttle_moving;
 
 		if (switches_updated) {
 			// Only use switches if current source is RC as well.
@@ -336,7 +331,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input)
 {
 	// Arm gesture
 	const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);
-	const bool left_stick_lower_right = (input.z < 0.1f) && (input.r > 0.9f);
+	const bool left_stick_lower_right = (input.z < -0.8f) && (input.r > 0.9f);
 
 	const bool previous_stick_arm_hysteresis = _stick_arm_hysteresis.get_state();
 	_stick_arm_hysteresis.set_state_and_update(left_stick_lower_right && right_stick_centered, input.timestamp);
@@ -346,7 +341,7 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input)
 	}
 
 	// Disarm gesture
-	const bool left_stick_lower_left = (input.z < 0.1f) && (input.r < -0.9f);
+	const bool left_stick_lower_left = (input.z < -0.8f) && (input.r < -0.9f);
 
 	const bool previous_stick_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
 	_stick_disarm_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_centered, input.timestamp);

src/modules/mavlink/mavlink_receiver.cpp

@@ -2138,10 +2138,11 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	}
 
 	manual_control_setpoint_s manual_control_setpoint{};
-	manual_control_setpoint.x = mavlink_manual_control.x / 1000.0f;
+	manual_control_setpoint.x = mavlink_manual_control.x / 1000.f;
-	manual_control_setpoint.y = mavlink_manual_control.y / 1000.0f;
+	manual_control_setpoint.y = mavlink_manual_control.y / 1000.f;
-	manual_control_setpoint.r = mavlink_manual_control.r / 1000.0f;
+	// For backwards compatibility at the moment interpret throttle in range [0,1000]
-	manual_control_setpoint.z = mavlink_manual_control.z / 1000.0f;
+	manual_control_setpoint.z = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f;
+	manual_control_setpoint.r = mavlink_manual_control.r / 1000.f;
 	manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
 	manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
 	_manual_control_input_pub.publish(manual_control_setpoint);

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -116,8 +116,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 	if (reset_yaw_sp) {
 		_man_yaw_sp = yaw;
 
-	} else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f
+	} else if ((_manual_control_setpoint.z > -.9f)
-		   || _param_mc_airmode.get() == 2) {
+		   || (_param_mc_airmode.get() == 2)) {
 
 		const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
 		attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
@@ -200,7 +200,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 	Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
 	q_sp.copyTo(attitude_setpoint.q_d);
 
-	attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.f, 1.f));
+	attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.z + 1.f) * .5f);
 	attitude_setpoint.timestamp = hrt_absolute_time();
 
 	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -178,7 +178,7 @@ MulticopterRateControl::Run()
 					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
 
 				_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
-				_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_setpoint(2) = -(manual_control_setpoint.z + 1.f) * .5f;
 				_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
 
 				// publish rate setpoint