new manual_control_switches msg (split out of manual_control_setpoint) (#16270)

- split out switches from manual_control_setpoint into new message manual_control_switches
 - manual_control_switches published at minimal rate (~ 1 Hz) or immediately on change
 - simple switch debounce in rc_update (2 consecutive identical decodes required)
 - manual_control_switches logged at full rate rather than sampled at (5-10% of messages logged)
 - manual_control_setpoint publish at minimal rate unless changing
 - commander handle landing gear switch for manual modes
 - processing of mode_slot and mode_switch is now split so we only do one or the other (not both)
     - a future step will be to finally drop mode_switch and accompanying switches entirely

Co-authored-by: Matthias Grob <maetugr@gmail.com>

msg/CMakeLists.txt

@@ -78,6 +78,7 @@ set(msg_files
 	log_message.msg
 	logger_status.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_sample                # the timestamp of the raw data (microseconds)
+
 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,19 +22,24 @@ 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
@@ -50,18 +47,3 @@ 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,37 @@
+uint64 timestamp                 # time since system start (microseconds)
+
+uint64 timestamp_sample          # the timestamp of the raw data (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
+
+uint8 mode_slot                  # the slot a specific model selector is in
+
+uint8 arm_switch                 # arm/disarm switch: _DISARMED_, ARMED
+uint8 return_switch              # return to launch 2 position switch (mandatory): _NORMAL_, RTL
+uint8 loiter_switch              # loiter 2 position switch (optional): _MISSION_, LOITER
+uint8 offboard_switch            # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 kill_switch                # throttle kill: _NORMAL_, KILL
+uint8 gear_switch                # landing gear switch: _DOWN_, UP
+uint8 transition_switch          # VTOL transition switch: _HOVER, FORWARD_FLIGHT
+
+# legacy "advanced" switch configuration (will be removed soon)
+uint8 mode_switch                # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
+uint8 man_switch                 # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
+uint8 acro_switch                # acro 2 position switch (optional): _MANUAL_, ACRO
+uint8 rattitude_switch           # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
+uint8 stab_switch                # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
+uint8 posctl_switch              # position control 2 position switch (optional): _ALTCTL_, POSCTL
+
+uint32 switch_changes            # number of switch changes

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

@@ -289,6 +289,8 @@ rtps:
     id: 138
   - msg: estimator_selector_status
     id: 139
+  - msg: manual_control_switches
+    id: 140
   ########## 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/AutoMapper/FlightTaskAutoMapper.cpp

@@ -42,7 +42,7 @@ using namespace matrix;
 
 FlightTaskAutoMapper::FlightTaskAutoMapper() :
 	_sticks(this)
-{};
+{}
 
 bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
@@ -187,7 +187,7 @@ float FlightTaskAutoMapper::_getLandSpeed()
 	// user input assisted land speed
 	if (_param_mpc_land_rc_help.get()
 	    && (_dist_to_ground < _param_mpc_land_alt1.get())
-	    && _sticks.checkAndSetStickInputs(_time_stamp_current)) {
+	    && _sticks.checkAndSetStickInputs()) {
 		// stick full up -1 -> stop, stick full down 1 -> double the speed
 		land_speed *= (1 + _sticks.getPositionExpo()(2));
 	}

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -44,14 +44,13 @@ using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
 	_sticks(this)
-{};
+{}
 
 bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
 
-	_sticks.checkAndSetStickInputs(_time_stamp_current);
-	_sticks.setGearAccordingToSwitch(_gear);
+	_sticks.checkAndSetStickInputs();
 
 	if (_sticks_data_required) {
 		ret = ret && _sticks.isAvailable();

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -42,6 +42,7 @@
 #include "FlightTask.hpp"
 #include "Sticks.hpp"
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <uORB/Subscription.hpp>
 
 class FlightTaskManualAltitude : public FlightTask
 {
@@ -129,6 +130,8 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
+	void setGearAccordingToSwitch();
+
 	float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	float _max_speed_up = 10.0f;

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

@@ -44,36 +44,40 @@ Sticks::Sticks(ModuleParams *parent) :
 	ModuleParams(parent)
 {};
 
-bool Sticks::checkAndSetStickInputs(hrt_abstime now)
+bool Sticks::checkAndSetStickInputs()
 {
-	_sub_manual_control_setpoint.update();
-
-	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
-
 	// Sticks are rescaled linearly and exponentially to [-1,1]
-	if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
+	manual_control_setpoint_s manual_control_setpoint;
+
+	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 		// Linear scale
-		_positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1]
-		_positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1]
-		_positions(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
-		_positions(3) = _sub_manual_control_setpoint.get().r; // yaw [-1,1]
+		_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 - 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
 		_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
 		_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
-		_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
-		_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
+		_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(),  _param_mpc_hold_dz.get());
+		_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(),    _param_mpc_hold_dz.get());
 
 		// valid stick inputs are required
-		const bool valid_sticks =  PX4_ISFINITE(_positions(0))
-					   && PX4_ISFINITE(_positions(1))
-					   && PX4_ISFINITE(_positions(2))
-					   && PX4_ISFINITE(_positions(3));
+		const bool valid_sticks = PX4_ISFINITE(_positions(0))
+					  && PX4_ISFINITE(_positions(1))
+					  && PX4_ISFINITE(_positions(2))
+					  && PX4_ISFINITE(_positions(3));
 
 		_input_available = valid_sticks;
 
 	} else {
-		_input_available = false;
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.update(&vehicle_status)) {
+			if (vehicle_status.rc_signal_lost) {
+				_input_available = false;
+			}
+		}
 	}
 
 	if (!_input_available) {
@@ -85,31 +89,6 @@ bool Sticks::checkAndSetStickInputs(hrt_abstime now)
 	return _input_available;
 }
 
-void Sticks::setGearAccordingToSwitch(landing_gear_s &gear)
-{
-	// 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.
-	if (!isAvailable()) {
-		gear.landing_gear = landing_gear_s::GEAR_KEEP;
-
-	} else {
-		const int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
-
-		if (_gear_switch_old != gear_switch) {
-			if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-				gear.landing_gear = landing_gear_s::GEAR_DOWN;
-			}
-
-			if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
-				gear.landing_gear = landing_gear_s::GEAR_UP;
-			}
-		}
-
-		_gear_switch_old = gear_switch;
-	}
-}
-
 void Sticks::limitStickUnitLengthXY(Vector2f &v)
 {
 	const float vl = v.length();

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

@@ -44,8 +44,8 @@
 #include <px4_platform_common/module_params.h>
 #include <matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/landing_gear.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
 
 class Sticks : public ModuleParams
 {
@@ -53,8 +53,7 @@ public:
 	Sticks(ModuleParams *parent);
 	~Sticks() = default;
 
-	bool checkAndSetStickInputs(hrt_abstime now);
-	void setGearAccordingToSwitch(landing_gear_s &gear);
+	bool checkAndSetStickInputs();
 	bool isAvailable() { return _input_available; };
 	const matrix::Vector<float, 4> &getPosition() { return _positions; };
 	const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
@@ -75,18 +74,17 @@ public:
 	static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
 
 private:
-	bool _input_available = false;
+	bool _input_available{false};
 	matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
 	matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
-	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state
 
-	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 _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
 		(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
 		(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
-		(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo,
-		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t
+		(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo
 	)
 };

src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp

@@ -35,7 +35,7 @@
 
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
 
 using namespace time_literals;
 
@@ -43,14 +43,13 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
 {
 	bool success = true;
 
-	uORB::SubscriptionData<manual_control_setpoint_s> manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	manual_control_setpoint_sub.update();
-	const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get();
+	uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get();
 
-	if (manual_control_setpoint.timestamp != 0) {
+	if (manual_control_switches.timestamp != 0) {
 
-		//check action switches
-		if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		// check action switches
+		if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
 			success = false;
 
 			if (report_fail) {
@@ -58,7 +57,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
 			}
 		}
 
-		if (manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
 			success = false;
 
 			if (report_fail) {
@@ -66,7 +65,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
 			}
 		}
 
-		if (manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
 			success = false;
 
 			if (report_fail) {

src/modules/commander/Commander.hpp

@@ -48,6 +48,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/landing_gear.h>
 #include <uORB/topics/test_motor.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -68,6 +69,7 @@
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
@@ -356,11 +358,12 @@ private:
 
 	unsigned int	_leds_counter{0};
 
-	manual_control_setpoint_s	_manual_control_setpoint{};		///< the current manual control setpoint
-	manual_control_setpoint_s	_last_manual_control_setpoint{};	///< the manual control setpoint valid at the last mode switch
+	manual_control_setpoint_s _manual_control_setpoint{};		///< the current manual control setpoint
+	manual_control_switches_s _manual_control_switches{};
+	manual_control_switches_s _last_manual_control_switches{};
 	hrt_abstime	_rc_signal_lost_timestamp{0};		///< Time at which the RC reception was lost
-	int32_t		_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {};
-	uint8_t		_last_manual_control_setpoint_arm_switch{0};
+	int32_t		_flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
+	uint8_t		_last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
 	uint32_t	_stick_off_counter{0};
 	uint32_t	_stick_on_counter{0};
 
@@ -406,6 +409,7 @@ private:
 	uORB::Subscription					_parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription					_manual_control_switches_sub{ORB_ID(manual_control_switches)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
@@ -433,6 +437,7 @@ private:
 	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
 	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
 	uORB::Publication<mission_s>				_mission_pub{ORB_ID(mission)};
+	uORB::Publication<landing_gear_s>			_landing_gear_pub{ORB_ID(landing_gear)};
 
 	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
 

src/modules/commander/commander_params.c

@@ -250,6 +250,7 @@ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
  * @group Commander
  * @min 100
  * @max 1500
+ * @unit ms
  */
 PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
 

src/modules/logger/logged_topics.cpp

@@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
 	add_topic("manual_control_setpoint", 200);
+	add_topic("manual_control_switches");
 	add_topic("mission");
 	add_topic("mission_result");
 	add_topic("navigator_mission_item");

src/modules/mavlink/mavlink_messages.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_baro.h>
@@ -3602,6 +3603,7 @@ public:
 
 private:
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
 
 	/* do not allow top copying this class */
 	MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete;
@@ -3623,15 +3625,20 @@ protected:
 			msg.y = manual_control_setpoint.y * 1000;
 			msg.z = manual_control_setpoint.z * 1000;
 			msg.r = manual_control_setpoint.r * 1000;
-			unsigned shift = 2;
-			msg.buttons = 0;
-			msg.buttons |= (manual_control_setpoint.mode_switch << (shift * 0));
-			msg.buttons |= (manual_control_setpoint.return_switch << (shift * 1));
-			msg.buttons |= (manual_control_setpoint.posctl_switch << (shift * 2));
-			msg.buttons |= (manual_control_setpoint.loiter_switch << (shift * 3));
-			msg.buttons |= (manual_control_setpoint.acro_switch << (shift * 4));
-			msg.buttons |= (manual_control_setpoint.offboard_switch << (shift * 5));
-			msg.buttons |= (manual_control_setpoint.kill_switch << (shift * 6));
+
+			manual_control_switches_s manual_control_switches{};
+
+			if (_manual_control_switches_sub.copy(&manual_control_switches)) {
+				unsigned shift = 2;
+				msg.buttons = 0;
+				msg.buttons |= (manual_control_switches.mode_switch << (shift * 0));
+				msg.buttons |= (manual_control_switches.return_switch << (shift * 1));
+				msg.buttons |= (manual_control_switches.posctl_switch << (shift * 2));
+				msg.buttons |= (manual_control_switches.loiter_switch << (shift * 3));
+				msg.buttons |= (manual_control_switches.acro_switch << (shift * 4));
+				msg.buttons |= (manual_control_switches.offboard_switch << (shift * 5));
+				msg.buttons |= (manual_control_switches.kill_switch << (shift * 6));
+			}
 
 			mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -336,8 +336,6 @@ void MulticopterPositionControl::Run()
 			// publish trajectory setpoint
 			_traj_sp_pub.publish(setpoint);
 
-			landing_gear_s gear = _flight_tasks.getGear();
-
 			// check if all local states are valid and map accordingly
 			set_vehicle_states(setpoint.vz);
 
@@ -445,14 +443,16 @@ void MulticopterPositionControl::Run()
 			_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
 
 			// if there's any change in landing gear setpoint publish it
-			if (gear.landing_gear != _old_landing_gear_position
-			    && gear.landing_gear != landing_gear_s::GEAR_KEEP) {
-				_landing_gear.timestamp = time_stamp_now;
-				_landing_gear.landing_gear = gear.landing_gear;
-				_landing_gear_pub.publish(_landing_gear);
+			landing_gear_s landing_gear = _flight_tasks.getGear();
+
+			if (landing_gear.landing_gear != _old_landing_gear_position
+			    && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
+
+				landing_gear.timestamp = hrt_absolute_time();
+				_landing_gear_pub.publish(landing_gear);
 			}
 
-			_old_landing_gear_position = gear.landing_gear;
+			_old_landing_gear_position = landing_gear.landing_gear;
 
 		} else {
 			// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -135,7 +135,7 @@ private:
 	vehicle_control_mode_s	_control_mode{};		/**< vehicle control mode */
 	vehicle_local_position_s _local_pos{};			/**< vehicle local position */
 	home_position_s	_home_pos{};			/**< home position */
-	landing_gear_s _landing_gear{};
+
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 
 	DEFINE_PARAMETERS(

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -96,30 +96,6 @@ MulticopterRateControl::parameters_updated()
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
 }
 
-float
-MulticopterRateControl::get_landing_gear_state()
-{
-	// Only switch the landing gear up if we are not landed and 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.
-	if (_landed) {
-		_gear_state_initialized = false;
-	}
-
-	float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
-
-	if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
-		landing_gear = landing_gear_s::GEAR_UP;
-
-	} else if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-		// Switching the gear off does put it into a safe defined state
-		_gear_state_initialized = true;
-	}
-
-	return landing_gear;
-}
-
 void
 MulticopterRateControl::Run()
 {
@@ -173,6 +149,16 @@ MulticopterRateControl::Run()
 
 		_vehicle_status_sub.update(&_vehicle_status);
 
+		if (_landing_gear_sub.updated()) {
+			landing_gear_s landing_gear;
+
+			if (_landing_gear_sub.copy(&landing_gear)) {
+				if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
+					_landing_gear = landing_gear.landing_gear;
+				}
+			}
+		}
+
 		const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
 
 		// generate the rate setpoint from sticks?
@@ -183,14 +169,6 @@ MulticopterRateControl::Run()
 		    !_v_control_mode.flag_control_velocity_enabled &&
 		    !_v_control_mode.flag_control_position_enabled) {
 
-			// landing gear controlled from stick inputs if we are in Manual/Stabilized mode
-			//  limit landing gear update rate to 10 Hz
-			if ((now - _landing_gear.timestamp) > 100_ms) {
-				_landing_gear.landing_gear = get_landing_gear_state();
-				_landing_gear.timestamp = hrt_absolute_time();
-				_landing_gear_pub.publish(_landing_gear);
-			}
-
 			if (!_v_control_mode.flag_control_attitude_enabled) {
 				manual_rate_sp = true;
 			}
@@ -202,9 +180,6 @@ MulticopterRateControl::Run()
 					(fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) ||
 					(fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get());
 			}
-
-		} else {
-			_landing_gear_sub.update(&_landing_gear);
 		}
 
 		if (manual_rate_sp) {
@@ -279,7 +254,7 @@ MulticopterRateControl::Run()
 			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
+			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
 			actuators.timestamp_sample = angular_velocity.timestamp_sample;
 
 			// scale effort by battery status if enabled

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -85,12 +85,6 @@ private:
 	 */
 	void		parameters_updated();
 
-	/**
-	 * Get the landing gear state based on the manual control switch position
-	 * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
-	 */
-	float		get_landing_gear_state();
-
 	RateControl _rate_control; ///< class for rate control calculations
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
@@ -111,7 +105,6 @@ private:
 	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
 
-	landing_gear_s 			_landing_gear{};
 	manual_control_setpoint_s	_manual_control_setpoint{};
 	vehicle_control_mode_s		_v_control_mode{};
 	vehicle_status_s		_vehicle_status{};
@@ -128,10 +121,10 @@ private:
 
 	float		_thrust_sp{0.0f};		/**< thrust setpoint */
 
-	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
-
 	hrt_abstime _last_run{0};
 
+	int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,

src/modules/rc_update/rc_update.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -47,7 +47,6 @@
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
-#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
@@ -55,6 +54,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/rc_parameter_map.h>
@@ -85,6 +85,8 @@ public:
 
 	bool init();
 
+	int print_status() override;
+
 private:
 
 	void Run() override;
@@ -99,6 +101,9 @@ private:
 	 */
 	void		update_rc_functions();
 
+	void		UpdateManualSetpoint(const hrt_abstime &timestamp_sample);
+	void		UpdateManualSwitches(const hrt_abstime &timestamp_sample);
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -107,13 +112,13 @@ private:
 	/**
 	 * Get and limit value for specified RC function. Returns NAN if not mapped.
 	 */
-	float		get_rc_value(uint8_t func, float min_value, float max_value);
+	float		get_rc_value(uint8_t func, float min_value, float max_value) const;
 
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
-	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
+	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const;
+	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th) const;
 
 	/**
 	 * Update parameters from RC channels if the functionality is activated and the
@@ -123,7 +128,7 @@ private:
 	 */
 	void		set_params_from_rc();
 
-	static constexpr unsigned RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */
+	static constexpr uint8_t RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */
 
 	struct Parameters {
 		uint16_t min[RC_MAX_CHAN_COUNT];
@@ -151,25 +156,35 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)};
 
-	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};	/**< notification of parameter updates */
-	uORB::Subscription	_rc_parameter_map_sub{ORB_ID(rc_parameter_map)};	/**< rc parameter map subscription */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
 
-	uORB::Publication<rc_channels_s>	_rc_pub{ORB_ID(rc_channels)};				/**< raw r/c control topic */
-	uORB::Publication<actuator_controls_s>	_actuator_group_3_pub{ORB_ID(actuator_controls_3)};	/**< manual control as actuator topic */
+	uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};
+	uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
+	uORB::Publication<manual_control_switches_s> _manual_control_switches_pub{ORB_ID(manual_control_switches)};
+	uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};
 
-	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};	/**< manual control signal topic */
-
-	rc_channels_s _rc {};			/**< r/c channel data */
+	manual_control_switches_s _manual_switches_previous{};
+	manual_control_switches_s _manual_switches_last_publish{};
+	rc_channels_s _rc{};
 
 	rc_parameter_map_s _rc_parameter_map {};
 	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {};	/**< parameter values for RC control */
 
-	hrt_abstime _last_rc_to_param_map_time = 0;
+	hrt_abstime _last_manual_control_setpoint_publish{0};
+	hrt_abstime _last_rc_to_param_map_time{0};
+	hrt_abstime _last_timestamp_signal{0};
+
+	uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {};
 
 	uint8_t _channel_count_previous{0};
 	uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};
 
-	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	uint8_t _channel_count_max{0};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+	perf_counter_t _valid_data_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": valid data interval")};
 
 	DEFINE_PARAMETERS(
 
@@ -223,9 +238,5 @@ private:
 
 		(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt
 	)
-
 };
-
-
-
 } /* namespace RCUpdate */

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -198,9 +198,9 @@ VtolAttitudeControl::is_fixed_wing_requested()
 {
 	bool to_fw = false;
 
-	if (_manual_control_setpoint.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
+	if (_manual_control_switches.transition_switch != manual_control_switches_s::SWITCH_POS_NONE &&
 	    _v_control_mode.flag_control_manual_enabled) {
-		to_fw = (_manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);
+		to_fw = (_manual_control_switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON);
 
 	} else {
 		// listen to transition commands if not in manual or mode switch is not mapped
@@ -403,7 +403,7 @@ VtolAttitudeControl::Run()
 		}
 
 		_v_control_mode_sub.update(&_v_control_mode);
-		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+		_manual_control_switches_sub.update(&_manual_control_switches);
 		_v_att_sub.update(&_v_att);
 		_local_pos_sub.update(&_local_pos);
 		_local_pos_sp_sub.update(&_local_pos_sp);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -66,7 +66,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed_validated.h>
-#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/tecs_status.h>
@@ -138,7 +138,7 @@ private:
 	uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};			// setpoint subscription
 	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};			// sensor subscription
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	//manual control setpoint subscription
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};	//manual control setpoint subscription
 	uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};			// local position setpoint subscription
@@ -165,7 +165,7 @@ private:
 	actuator_controls_s			_actuators_out_1{};	//actuator controls going to the fw mixer (used for elevons)
 
 	airspeed_validated_s 				_airspeed_validated{};			// airspeed
-	manual_control_setpoint_s		_manual_control_setpoint{}; //manual control setpoint
+	manual_control_switches_s		_manual_control_switches{}; //manual control setpoint
 	position_setpoint_triplet_s		_pos_sp_triplet{};
 	tecs_status_s				_tecs_status{};
 	vehicle_attitude_s			_v_att{};				//vehicle attitude