manual_control_setpoint split switches into new manual_control_switches

msg/CMakeLists.txt

@@ -76,6 +76,7 @@ set(msg_files
 	led_control.msg
 	log_message.msg
 	manual_control_setpoint.msg
+	manual_control_switches.msg
 	mavlink_log.msg
 	mission.msg
 	mission_result.msg

msg/manual_control_setpoint.msg

@@ -1,23 +1,15 @@
 uint64 timestamp			# time since system start (microseconds)
 
-uint8 SWITCH_POS_NONE = 0		# switch is not mapped
-uint8 SWITCH_POS_ON = 1		# switch activated (value = 1)
-uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
-uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
-uint8 MODE_SLOT_NONE = 0		# no mode slot assigned
-uint8 MODE_SLOT_1 = 1			# mode slot 1 selected
-uint8 MODE_SLOT_2 = 2			# mode slot 2 selected
-uint8 MODE_SLOT_3 = 3			# mode slot 3 selected
-uint8 MODE_SLOT_4 = 4			# mode slot 4 selected
-uint8 MODE_SLOT_5 = 5			# mode slot 5 selected
-uint8 MODE_SLOT_6 = 6			# mode slot 6 selected
-uint8 MODE_SLOT_NUM = 6			# number of slots
+uint64 timestamp_last_signal		# last valid reception time
+
 uint8 SOURCE_RC = 1			# radio control
 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 4
 
+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
@@ -30,38 +22,27 @@ 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			 # default function: camera yaw / azimuth
 float32 aux2			 # default function: camera pitch / tilt
 float32 aux3			 # default function: camera trigger
 float32 aux4			 # default function: camera roll
 float32 aux5			 # default function: payload drop
 float32 aux6
-
-uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
-uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
-uint8 rattitude_switch	 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
-uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
-uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
-uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
-uint8 offboard_switch	 	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
-uint8 kill_switch		 # throttle kill: _NORMAL_, KILL
-uint8 arm_switch		 # arm/disarm switch: _DISARMED_, ARMED
-uint8 transition_switch	 # VTOL transition switch: _HOVER, FORWARD_FLIGHT
-uint8 gear_switch	 # landing gear switch: _DOWN_, UP
-uint8 mode_slot			 # the slot a specific model selector is in
-uint8 data_source		 # where this input is coming from
-uint8 stab_switch 		 # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
-uint8 man_switch		 # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL

msg/manual_control_switches.msg

@@ -0,0 +1,33 @@
+uint64 timestamp			# time since system start (microseconds)
+
+uint8 SWITCH_POS_NONE = 0		# switch is not mapped
+uint8 SWITCH_POS_ON = 1		# switch activated (value = 1)
+uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
+uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
+
+uint8 MODE_SLOT_NONE = 0		# no mode slot assigned
+uint8 MODE_SLOT_1 = 1			# mode slot 1 selected
+uint8 MODE_SLOT_2 = 2			# mode slot 2 selected
+uint8 MODE_SLOT_3 = 3			# mode slot 3 selected
+uint8 MODE_SLOT_4 = 4			# mode slot 4 selected
+uint8 MODE_SLOT_5 = 5			# mode slot 5 selected
+uint8 MODE_SLOT_6 = 6			# mode slot 6 selected
+uint8 MODE_SLOT_NUM = 6			# number of slots
+
+uint64 timestamp_last_signal		# last valid reception time
+
+uint8 mode_slot			 # the slot a specific model selector is in
+
+uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
+uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
+uint8 rattitude_switch	 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
+uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
+uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
+uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
+uint8 offboard_switch	 	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 kill_switch		 # throttle kill: _NORMAL_, KILL
+uint8 arm_switch		 # arm/disarm switch: _DISARMED_, ARMED
+uint8 transition_switch	 # VTOL transition switch: _HOVER, FORWARD_FLIGHT
+uint8 gear_switch	 # landing gear switch: _DOWN_, UP
+uint8 stab_switch 		 # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
+uint8 man_switch		 # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL

msg/rc_channels.msg

@@ -1,32 +1,32 @@
 uint64 timestamp						# time since system start (microseconds)
 
-uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
-uint8 RC_CHANNELS_FUNCTION_ROLL=1
-uint8 RC_CHANNELS_FUNCTION_PITCH=2
-uint8 RC_CHANNELS_FUNCTION_YAW=3
-uint8 RC_CHANNELS_FUNCTION_MODE=4
-uint8 RC_CHANNELS_FUNCTION_RETURN=5
-uint8 RC_CHANNELS_FUNCTION_POSCTL=6
-uint8 RC_CHANNELS_FUNCTION_LOITER=7
-uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
-uint8 RC_CHANNELS_FUNCTION_ACRO=9
-uint8 RC_CHANNELS_FUNCTION_FLAPS=10
-uint8 RC_CHANNELS_FUNCTION_AUX_1=11
-uint8 RC_CHANNELS_FUNCTION_AUX_2=12
-uint8 RC_CHANNELS_FUNCTION_AUX_3=13
-uint8 RC_CHANNELS_FUNCTION_AUX_4=14
-uint8 RC_CHANNELS_FUNCTION_AUX_5=15
-uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
-uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
-uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
-uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
-uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
-uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
-uint8 RC_CHANNELS_FUNCTION_GEAR=22
-uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23
-uint8 RC_CHANNELS_FUNCTION_STAB=24
-uint8 RC_CHANNELS_FUNCTION_AUX_6=25
-uint8 RC_CHANNELS_FUNCTION_MAN=26
+uint8 FUNCTION_THROTTLE=0
+uint8 FUNCTION_ROLL=1
+uint8 FUNCTION_PITCH=2
+uint8 FUNCTION_YAW=3
+uint8 FUNCTION_MODE=4
+uint8 FUNCTION_RETURN=5
+uint8 FUNCTION_POSCTL=6
+uint8 FUNCTION_LOITER=7
+uint8 FUNCTION_OFFBOARD=8
+uint8 FUNCTION_ACRO=9
+uint8 FUNCTION_FLAPS=10
+uint8 FUNCTION_AUX_1=11
+uint8 FUNCTION_AUX_2=12
+uint8 FUNCTION_AUX_3=13
+uint8 FUNCTION_AUX_4=14
+uint8 FUNCTION_AUX_5=15
+uint8 FUNCTION_PARAM_1=16
+uint8 FUNCTION_PARAM_2=17
+uint8 FUNCTION_PARAM_3_5=18
+uint8 FUNCTION_RATTITUDE=19
+uint8 FUNCTION_KILLSWITCH=20
+uint8 FUNCTION_TRANSITION=21
+uint8 FUNCTION_GEAR=22
+uint8 FUNCTION_ARMSWITCH=23
+uint8 FUNCTION_STAB=24
+uint8 FUNCTION_AUX_6=25
+uint8 FUNCTION_MAN=26
 
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)

msg/tools/uorb_rtps_message_ids.yaml

@@ -275,6 +275,8 @@ rtps:
     id: 122
   - msg: sensor_gyro_integrated
     id: 123
+  - msg: manual_control_switches
+    id: 124
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -322,7 +322,7 @@ void task_main(int argc, char *argv[])
 			_controls[0].control[0] = 0.f;
 			_controls[0].control[1] = 0.f;
 			_controls[0].control[2] = 0.f;
-			int channel = rc_channels.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE];
+			int channel = rc_channels.function[rc_channels_s::FUNCTION_THROTTLE];
 
 			if (ret == 0 && channel >= 0 && channel < (int)(sizeof(rc_channels.channels) / sizeof(rc_channels.channels[0]))) {
 				_controls[0].control[3] = rc_channels.channels[channel];

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -64,7 +64,6 @@ bool FlightTaskAuto::updateInitialize()
 	bool ret = FlightTask::updateInitialize();
 
 	_sub_home_position.update();
-	_sub_manual_control_setpoint.update();
 	_sub_vehicle_status.update();
 	_sub_triplet_setpoint.update();
 

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp

@@ -100,7 +100,6 @@ class FlightTaskAuto : public FlightTask
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
-	uORB::SubscriptionData<manual_control_setpoint_s>	_sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
 
 	State _current_state{State::none};

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -181,13 +181,18 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 
 float FlightTaskAutoMapper::_getLandSpeed()
 {
-	bool rc_assist_enabled = _param_mpc_land_rc_help.get();
-	bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
-
 	float throttle = 0.5f;
 
-	if (rc_is_valid && rc_assist_enabled) {
-		throttle = _sub_manual_control_setpoint.get().z;
+	// manual landing assist if enabled and valid
+	if (_param_mpc_land_rc_help.get()) {
+		if (!_sub_vehicle_status.get().rc_signal_lost) {
+
+			manual_control_setpoint_s manual_control_setpoint;
+
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				throttle = manual_control_setpoint.z;
+			}
+		}
 	}
 
 	float speed = 0;

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -42,6 +42,8 @@
 
 #include "FlightTaskAuto.hpp"
 
+#include <uORB/topics/manual_control_setpoint.h>
+
 class FlightTaskAutoMapper : public FlightTaskAuto
 {
 public:
@@ -62,6 +64,8 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 
 	void updateParams() override; /**< See ModuleParam class */
 
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp

@@ -46,8 +46,6 @@ bool FlightTaskManual::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
 
-	_sub_manual_control_setpoint.update();
-
 	const bool sticks_available = _evaluateSticks();
 
 	if (_sticks_data_required) {
@@ -59,58 +57,57 @@ bool FlightTaskManual::updateInitialize()
 
 bool FlightTaskManual::_evaluateSticks()
 {
-	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
+	_vehicle_status_sub.update();
 
-	/* Sticks are rescaled linearly and exponentially to [-1,1] */
-	if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
+	if (_vehicle_status_sub.get().rc_signal_lost) {
+		/* Timeout: set all sticks to zero */
+		_sticks.zero();
+		_sticks_expo.zero();
+		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
+		return false;
+	}
 
-		/* Linear scale  */
-		_sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */
-		_sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */
-		_sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
-		_sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */
+	// Only switch the landing gear up if the user switched from gear down to gear up.
+	// If the user had the switch in the gear up position and took off ignore it
+	// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+	manual_control_switches_s manual_control_switches;
 
-		/* Exponential scale */
-		_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
-		_sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
+	if (_manual_control_switches_sub.update(&manual_control_switches)) {
+		if (_gear_switch_old != manual_control_switches.gear_switch) {
+			if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
+				_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+			}
 
-		// Only switch the landing gear up if the user switched from gear down to gear up.
-		// If the user had the switch in the gear up position and took off ignore it
-		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
-		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
-
-		if (_gear_switch_old != gear_switch) {
-			_applyGearSwitch(gear_switch);
+			if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
+				_gear.landing_gear = landing_gear_s::GEAR_UP;
+			}
 		}
 
-		_gear_switch_old = gear_switch;
+		_gear_switch_old = manual_control_switches.gear_switch;
+	}
 
-		// valid stick inputs are required
-		const bool valid_sticks =  PX4_ISFINITE(_sticks(0))
-					   && PX4_ISFINITE(_sticks(1))
-					   && PX4_ISFINITE(_sticks(2))
-					   && PX4_ISFINITE(_sticks(3));
+	manual_control_setpoint_s manual_control_setpoint;
 
-		return valid_sticks;
+	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 
-	} else {
-		/* Timeout: set all sticks to zero */
-		_sticks.zero();
-		_sticks_expo.zero();
-		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
-		return false;
-	}
-}
+		/* Sticks are rescaled linearly and exponentially to [-1,1] */
+		_sticks(0) = manual_control_setpoint.x; /* NED x, "pitch" [-1,1] */
+		_sticks(1) = manual_control_setpoint.y; /* NED y, "roll" [-1,1] */
+		_sticks(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */
+		_sticks(3) = manual_control_setpoint.r; /* "yaw" [-1,1] */
 
-void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
-{
-	if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-		_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+		/* Exponential scale */
+		_sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
+		_sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
 	}
 
-	if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
-		_gear.landing_gear = landing_gear_s::GEAR_UP;
-	}
+	// valid stick inputs are required
+	const bool valid_sticks = PX4_ISFINITE(_sticks(0))
+				  && PX4_ISFINITE(_sticks(1))
+				  && PX4_ISFINITE(_sticks(2))
+				  && PX4_ISFINITE(_sticks(3));
+
+	return valid_sticks;
 }

src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp

@@ -42,6 +42,8 @@
 
 #include "FlightTask.hpp"
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
+#include <uORB/topics/vehicle_status.h>
 
 class FlightTaskManual : public FlightTask
 {
@@ -58,16 +60,17 @@ class FlightTaskManual : public FlightTask
 	bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */
 	matrix::Vector<float, 4> _sticks; /**< unmodified manual stick inputs */
 	matrix::Vector<float, 4> _sticks_expo; /**< modified manual sticks using expo function*/
-	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/
+	int _gear_switch_old = manual_control_switches_s::SWITCH_POS_NONE; /**< old switch state*/
 
 	float stickDeadzone() const { return _param_mpc_hold_dz.get(); }
 
 private:
 
 	bool _evaluateSticks(); /**< checks and sets stick inputs */
-	void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
 
-	uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */