Use backwards compatible manual_control_setpoint instead of manual_co…

…ntrol_input

msg/CMakeLists.txt

@@ -103,7 +103,6 @@ set(msg_files
 	logger_status.msg
 	mag_worker_data.msg
 	magnetometer_bias_estimate.msg
-	manual_control_input.msg
 	manual_control_setpoint.msg
 	manual_control_switches.msg
 	mavlink_log.msg

msg/manual_control_setpoint.msg

@@ -1,6 +1,56 @@
 uint64 timestamp                        # time since system start (microseconds)
+uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)
 
 bool valid
-px4/manual_control_input chosen_input
 
-bool user_override
+uint8 SOURCE_UNKNOWN   = 0
+uint8 SOURCE_RC        = 1		# radio control (input_rc)
+uint8 SOURCE_MAVLINK_0 = 2		# mavlink instance 0
+uint8 SOURCE_MAVLINK_1 = 3		# mavlink instance 1
+uint8 SOURCE_MAVLINK_2 = 4		# mavlink instance 2
+uint8 SOURCE_MAVLINK_3 = 5		# mavlink instance 3
+uint8 SOURCE_MAVLINK_4 = 6		# mavlink instance 4
+uint8 SOURCE_MAVLINK_5 = 7		# mavlink instance 5
+
+uint8 data_source
+
+# 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 0..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
+
+float32 r			 # yaw stick/twist position, -1..1
+				 # in general corresponds to the righthand rotation around the vertical
+				 # (downwards) axis of the vehicle
+
+float32 flaps			 # flap position
+
+float32 aux1
+float32 aux2
+float32 aux3
+float32 aux4
+float32 aux5
+float32 aux6
+
+bool sticks_moving
+
+# TOPICS manual_control_setpoint manual_control_input

src/examples/fixedwing_control/main.cpp

@@ -388,9 +388,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.chosen_input.z) &&
-				    (manual_control_setpoint.chosen_input.z >= 0.6f) &&
-				    (manual_control_setpoint.chosen_input.z <= 1.0f)) {
+				if (PX4_ISFINITE(manual_control_setpoint.z) &&
+				    (manual_control_setpoint.z >= 0.6f) &&
+				    (manual_control_setpoint.z <= 1.0f)) {
 				}
 
 				/* get the system status and the flight mode we're in */

src/lib/mixer_module/functions.cpp

@@ -119,17 +119,17 @@ void FunctionManualRC::update()
 	manual_control_setpoint_s manual_control_setpoint;
 
 	if (_topic.update(&manual_control_setpoint)) {
-		_data[0] = manual_control_setpoint.chosen_input.y; // roll
-		_data[1] = manual_control_setpoint.chosen_input.x; // pitch
-		_data[2] = manual_control_setpoint.chosen_input.z * 2.f - 1.f; // throttle
-		_data[3] = manual_control_setpoint.chosen_input.r; // yaw
-		_data[4] = manual_control_setpoint.chosen_input.flaps;
-		_data[5] = manual_control_setpoint.chosen_input.aux1;
-		_data[6] = manual_control_setpoint.chosen_input.aux2;
-		_data[7] = manual_control_setpoint.chosen_input.aux3;
-		_data[8] = manual_control_setpoint.chosen_input.aux4;
-		_data[9] = manual_control_setpoint.chosen_input.aux5;
-		_data[10] = manual_control_setpoint.chosen_input.aux6;
+		_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[3] = manual_control_setpoint.r; // yaw
+		_data[4] = manual_control_setpoint.flaps;
+		_data[5] = manual_control_setpoint.aux1;
+		_data[6] = manual_control_setpoint.aux2;
+		_data[7] = manual_control_setpoint.aux3;
+		_data[8] = manual_control_setpoint.aux4;
+		_data[9] = manual_control_setpoint.aux5;
+		_data[10] = manual_control_setpoint.aux6;
 	}
 }
 

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_sp.chosen_input.x;
-		_actuators.control[2] = _manual_control_sp.chosen_input.r;
-		_actuators.control[3] = _manual_control_sp.chosen_input.z;
+		_actuators.control[1] = _manual_control_sp.x;
+		_actuators.control[2] = _manual_control_sp.r;
+		_actuators.control[3] = _manual_control_sp.z;
 	}
 
 	// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()

src/modules/commander/Commander.cpp

@@ -2360,7 +2360,7 @@ Commander::run()
 				}
 
 				const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0);
-				const bool is_mavlink = manual_control_setpoint.chosen_input.data_source > manual_control_input_s::SOURCE_RC;
+				const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
 
 				if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
 					// if there's never been a mode change force position control as initial state
@@ -2369,8 +2369,8 @@ Commander::run()
 				}
 
 				_status.rc_signal_lost = false;
-				_is_throttle_above_center = manual_control_setpoint.chosen_input.z > 0.6f;
-				_is_throttle_low = manual_control_setpoint.chosen_input.z < 0.1f;
+				_is_throttle_above_center = manual_control_setpoint.z > 0.6f;
+				_is_throttle_low = manual_control_setpoint.z < 0.1f;
 				_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
 
 			} else {
@@ -2400,7 +2400,7 @@ Commander::run()
 			    && _armed.armed
 			    && !_status_flags.rc_input_blocked
 			    && manual_control_setpoint.valid
-			    && manual_control_setpoint.user_override
+			    && manual_control_setpoint.sticks_moving
 			    && override_enabled) {
 				const transition_result_t posctl_result =
 					main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);

src/modules/commander/rc_calibration.cpp

@@ -85,15 +85,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.chosen_input.y * roll_scale + roll_trim_active;
+	float p = manual_control_setpoint.y * 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.chosen_input.x * pitch_scale + pitch_trim_active;
+	p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
 	int p2r = param_set(param_find("TRIM_PITCH"), &p);
-	p = manual_control_setpoint.chosen_input.r * yaw_scale + yaw_trim_active;
+	p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
 	int p3r = param_set(param_find("TRIM_YAW"), &p);
 
 	if (p1r != 0 || p2r != 0 || p3r != 0) {

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

@@ -51,11 +51,11 @@ bool Sticks::checkAndSetStickInputs()
 
 	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 		// Linear scale
-		_positions(0) = manual_control_setpoint.chosen_input.x; // NED x, pitch [-1,1]
-		_positions(1) = manual_control_setpoint.chosen_input.y; // NED y, roll [-1,1]
-		_positions(2) = -(math::constrain(manual_control_setpoint.chosen_input.z, 0.0f,
+		_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,
 						  1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
-		_positions(3) = manual_control_setpoint.chosen_input.r; // yaw [-1,1]
+		_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
 
 		// Exponential scale
 		_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -149,15 +149,15 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
 					// STABILIZED mode generate the attitude setpoint from manual user inputs
 
-					_att_sp.roll_body = _manual_control_setpoint.chosen_input.y * radians(_param_fw_man_r_max.get());
+					_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
 
-					_att_sp.pitch_body = -_manual_control_setpoint.chosen_input.x * radians(_param_fw_man_p_max.get())
+					_att_sp.pitch_body = -_manual_control_setpoint.x * 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()));
 
 					_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.chosen_input.z, 0.0f, 1.0f);
+					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -171,22 +171,22 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 
 					// 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.chosen_input.y * radians(_param_fw_acro_x_max.get());
-					_rates_sp.pitch = -_manual_control_setpoint.chosen_input.x * radians(_param_fw_acro_y_max.get());
-					_rates_sp.yaw = _manual_control_setpoint.chosen_input.r * radians(_param_fw_acro_z_max.get());
-					_rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f, 1.0f);
+					_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);
 
 					_rate_sp_pub.publish(_rates_sp);
 
 				} else {
 					/* manual/direct control */
 					_actuators.control[actuator_controls_s::INDEX_ROLL] =
-						_manual_control_setpoint.chosen_input.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
+						_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
 					_actuators.control[actuator_controls_s::INDEX_PITCH] =
-						-_manual_control_setpoint.chosen_input.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
+						-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
 					_actuators.control[actuator_controls_s::INDEX_YAW] =
-						_manual_control_setpoint.chosen_input.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
-					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.chosen_input.z, 0.0f,
+						_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
 							1.0f);
 				}
 			}
@@ -563,7 +563,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.chosen_input.r;
+						_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
 					}
 
 					if (!PX4_ISFINITE(yaw_u)) {
@@ -644,10 +644,10 @@ void FixedwingAttitudeControl::Run()
 				* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
 
 		_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
-		_actuators.control[5] = _manual_control_setpoint.chosen_input.aux1;
+		_actuators.control[5] = _manual_control_setpoint.aux1;
 		_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
 		// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
-		_actuators.control[7] = _manual_control_setpoint.chosen_input.aux3;
+		_actuators.control[7] = _manual_control_setpoint.aux3;
 
 		/* lazily publish the setpoint only once available */
 		_actuators.timestamp = hrt_absolute_time();
@@ -672,9 +672,9 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 	float flap_control = 0.0f;
 
 	/* map flaps by default to manual if valid */
-	if (PX4_ISFINITE(_manual_control_setpoint.chosen_input.flaps) && _vcontrol_mode.flag_control_manual_enabled
+	if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled
 	    && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
-		flap_control = _manual_control_setpoint.chosen_input.flaps * _param_fw_flaps_scl.get();
+		flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get();
 
 	} else if (_vcontrol_mode.flag_control_auto_enabled
 		   && fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
@@ -706,10 +706,10 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 	float flaperon_control = 0.0f;
 
 	/* map flaperons by default to manual if valid */
-	if (PX4_ISFINITE(_manual_control_setpoint.chosen_input.aux2) && _vcontrol_mode.flag_control_manual_enabled
+	if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
 	    && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
 
-		flaperon_control = 0.5f * (_manual_control_setpoint.chosen_input.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
+		flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
 
 	} else if (_vcontrol_mode.flag_control_auto_enabled
 		   && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -423,8 +423,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	if (_state != state::wait_for_disarm
 	    && _state != state::idle
 	    && (((now - _state_start_time) > 20_s)
-		|| (fabsf(manual_control_setpoint.chosen_input.x) > 0.2f)
-		|| (fabsf(manual_control_setpoint.chosen_input.y) > 0.2f))) {
+		|| (fabsf(manual_control_setpoint.x) > 0.2f)
+		|| (fabsf(manual_control_setpoint.y) > 0.2f))) {
 		_state = state::fail;
 		_state_start_time = now;
 	}