new manual_control_switches msg (split out of manual_control_setpoint)

Author: dagar

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,38 +22,28 @@ 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
+
+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

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,9 @@ using namespace matrix;
 
 FlightTaskAutoMapper::FlightTaskAutoMapper() :
 	_sticks(this)
-{};
+{
+	_gear.landing_gear = landing_gear_s::GEAR_KEEP;
+}
 
 bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
@@ -187,7 +189,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

@@ -50,8 +50,32 @@ bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
 
-	_sticks.checkAndSetStickInputs(_time_stamp_current);
-	_sticks.setGearAccordingToSwitch(_gear);
+	manual_control_switches_s manual_control_switches;
+
+	if (_manual_control_switches_sub.copy(&manual_control_switches) && (manual_control_switches.timestamp > 0)) {
+
+		const int8_t gear_switch = manual_control_switches.gear_switch;
+
+		if (_gear_switch_old != gear_switch) {
+			if (gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
+				_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+			}
+
+			if (gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
+				_gear.landing_gear = landing_gear_s::GEAR_UP;
+			}
+		}
+
+		_gear_switch_old = gear_switch;
+
+	} else {
+		// 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.
+		_gear.landing_gear = landing_gear_s::GEAR_KEEP;
+	}
+
+	_sticks.checkAndSetStickInputs();
 
 	if (_sticks_data_required) {
 		ret = ret && _sticks.isAvailable();

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

@@ -42,6 +42,8 @@
 #include "FlightTask.hpp"
 #include "Sticks.hpp"
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_switches.h>
 
 class FlightTaskManualAltitude : public FlightTask
 {
@@ -145,4 +147,9 @@ class FlightTaskManualAltitude : public FlightTask
 	float _dist_to_ground_lock = NAN;
 
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
+
+	int _gear_switch_old{manual_control_switches_s::SWITCH_POS_NONE}; ///< old switch state
+
 };

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

@@ -44,25 +44,23 @@ 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))
@@ -71,9 +69,6 @@ bool Sticks::checkAndSetStickInputs(hrt_abstime now)
 					   && PX4_ISFINITE(_positions(3));
 
 		_input_available = valid_sticks;
-
-	} else {
-		_input_available = false;
 	}
 
 	if (!_input_available) {
@@ -85,31 +80,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();