lib/rc: don't share decode buffer between protocols

commander: don't play failsafe tune if disarmed
vscode: do not disable autocomplete on enter
2026-05-24 23:37:35 +08:00 · 2021-09-16 14:40:26 -04:00 · 2021-09-16 09:17:00 -04:00 · 2021-09-16 09:11:52 -04:00 · 2021-09-16 13:59:27 +02:00 · 2021-09-16 13:59:27 +02:00
22 changed files with 229 additions and 101 deletions
@@ -31,7 +31,6 @@
                "esc"
        ],
        "debug.toolBarLocation": "docked",
-        "editor.acceptSuggestionOnEnter": "off",
        "editor.defaultFormatter": "chiehyu.vscode-astyle",
        "editor.dragAndDrop": false,
        "editor.insertSpaces": false,
@@ -1,5 +1,7 @@
 #! /usr/bin/env bash

+set -e
+
 ## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
 ## Can also be used in docker.
 ##
@@ -193,7 +195,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		java_version=11
 		gazebo_version=9
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-		java_version=14
+		java_version=13
 		gazebo_version=11
 	else
 		java_version=14
@@ -14,7 +14,8 @@ px4_add_board(
 	DRIVERS
 		#adc/ads1115
 		adc/board_adc
-		barometer # all available barometer drivers
+		#barometer # all available barometer drivers
+		barometer/ms5611
 		batt_smbus
 		camera_capture
 		camera_trigger
@@ -1,36 +1,44 @@
 uint64 timestamp						# time since system start (microseconds)

-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_KILLSWITCH = 19
-uint8 FUNCTION_TRANSITION = 20
-uint8 FUNCTION_GEAR       = 21
-uint8 FUNCTION_ARMSWITCH  = 22
-uint8 FUNCTION_STAB       = 23
-uint8 FUNCTION_AUX_6      = 24
-uint8 FUNCTION_MAN        = 25
+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_KILLSWITCH    = 19
+uint8 FUNCTION_TRANSITION    = 20
+uint8 FUNCTION_GEAR          = 21
+uint8 FUNCTION_ARMSWITCH     = 22
+uint8 FUNCTION_STAB          = 23
+uint8 FUNCTION_AUX_6         = 24
+uint8 FUNCTION_MAN           = 25
+uint8 FUNCTION_FLTBTN_SLOT_1 = 26
+uint8 FUNCTION_FLTBTN_SLOT_2 = 27
+uint8 FUNCTION_FLTBTN_SLOT_3 = 28
+uint8 FUNCTION_FLTBTN_SLOT_4 = 39
+uint8 FUNCTION_FLTBTN_SLOT_5 = 30
+uint8 FUNCTION_FLTBTN_SLOT_6 = 31
+
+uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6

 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[26] function						# Functions mapping
+int8[32] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames
@@ -1,8 +1,6 @@

 #include "common_rc.h"

-__EXPORT rc_decode_buf_t rc_decode_buf;
-
 uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
 {
 	crc ^= a;
@@ -3,27 +3,5 @@

 #include <stdint.h>

-#include "crsf.h"
-#include "dsm.h"
-#include "ghst.hpp"
-#include "sbus.h"
-#include "st24.h"
-#include "sumd.h"
-
-#pragma pack(push, 1)
-typedef  struct rc_decode_buf_ {
-	union {
-		crsf_frame_t crsf_frame;
-		ghst_frame_t ghst_frame;
-		dsm_decode_t dsm;
-		sbus_frame_t sbus_frame;
-		ReceiverFcPacket _strxpacket;
-		ReceiverFcPacketHoTT _hottrxpacket;
-	};
-} rc_decode_buf_t;
-#pragma pack(pop)
-
-extern rc_decode_buf_t rc_decode_buf;
-
 uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a);
 uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len);
@@ -129,7 +129,7 @@ enum class crsf_parser_state_t : uint8_t {
 	synced
 };

-static crsf_frame_t	 &crsf_frame = rc_decode_buf.crsf_frame;
+static crsf_frame_t crsf_frame;
 static unsigned current_frame_position = 0;
 static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;

@@ -73,8 +73,8 @@ static enum DSM_DECODE_STATE {
 static int dsm_fd = -1;						/**< File handle to the DSM UART */
 static hrt_abstime dsm_last_rx_time;            /**< Timestamp when we last received data */
 static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last valid dsm frame */
-static dsm_frame_t &dsm_frame = rc_decode_buf.dsm.frame;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
-static dsm_buf_t &dsm_buf = rc_decode_buf.dsm.buf;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
+static dsm_frame_t dsm_frame;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */
+static dsm_buf_t dsm_buf;	/**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */

 static uint16_t dsm_chan_buf[DSM_MAX_CHANNEL_COUNT];
 static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
@@ -79,7 +79,7 @@ enum class ghst_parser_state_t : uint8_t {
 // only RSSI frame contains value of RSSI, if it is not received, send last received RSSI
 static int8_t ghst_rssi = -1;

-static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame;
+static ghst_frame_t ghst_frame;
 static uint32_t current_frame_position = 0U;
 static ghst_parser_state_t parser_state = ghst_parser_state_t::unsynced;

@@ -118,7 +118,7 @@ static enum SBUS2_DECODE_STATE {
 	SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34
 } sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;

-static sbus_frame_t	 &sbus_frame = rc_decode_buf.sbus_frame;
+static sbus_frame_t sbus_frame;

 static unsigned partial_frame_count;
 static unsigned sbus1_frame_delay = (1000U * 1000U) / SBUS1_DEFAULT_RATE_HZ;
@@ -66,7 +66,7 @@ const char *decode_states[] = {"UNSYNCED",
 static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
 static uint8_t _rxlen;

-static ReceiverFcPacket &_rxpacket = rc_decode_buf._strxpacket;
+static ReceiverFcPacket _rxpacket;

 uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
 {
@@ -88,7 +88,7 @@ bool		_debug		= false;
 static enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED;
 static uint8_t _rxlen;

-static ReceiverFcPacketHoTT &_rxpacket = rc_decode_buf._hottrxpacket;
+static ReceiverFcPacketHoTT _rxpacket;

 uint16_t sumd_crc16(uint16_t crc, uint8_t value)
 {
@@ -145,34 +145,35 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_
 	}

 	// reset states if we are not flying or wind estimator was just initialized/reset
-	if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s) {
+	if (!_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 10_s
+	    || _tas_innov_integ_threshold <= 0.f) {
 		_innovations_check_failed = false;
 		_time_last_tas_pass = time_now;
+		_apsd_innov_integ_state = 0.f;
+
+	} else if (estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) {
+		//nav velocity data is likely not good
+		//don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good
+		_time_last_tas_pass = time_now;
+		_apsd_innov_integ_state = 0.f;

 	} else {
+		// nav velocity data is likely good so airspeed innovations are able to be used
+		// compute the ratio of innovation to gate size
 		const float dt_s = math::constrain((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f, 0.2f); // limit to [100,5] Hz
+		const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
+					     / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());

-		if ((estimator_status_vel_test_ratio < 1.f) && (estimator_status_mag_test_ratio < 1.f)) {
-			// nav velocity data is likely good so airspeed innovations are able to be used
-			// compute the ratio of innovation to gate size
-			const float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov()
-						     / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.f) * _wind_estimator.get_tas_innov_var());
+		if (tas_test_ratio > _tas_innov_threshold) {
+			_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance

-			if (tas_test_ratio > _tas_innov_threshold) {
-				_apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); // integrate exceedance
+		} else {
+			// reset integrator used to trigger and record pass if integrator check is disabled
+			_apsd_innov_integ_state = 0.f;
+		}

-			} else {
-				// reset integrator used to trigger and record pass if integrator check is disabled
-				_apsd_innov_integ_state = 0.f;
-
-				if (_tas_innov_integ_threshold <= 0.f) {
-					_time_last_tas_pass = time_now;
-				}
-			}
-
-			if (_tas_innov_integ_threshold > 0.f && _apsd_innov_integ_state < _tas_innov_integ_threshold) {
-				_time_last_tas_pass = time_now;
-			}
+		if (_tas_innov_integ_threshold > 0.f && _apsd_innov_integ_state < _tas_innov_integ_threshold) {
+			_time_last_tas_pass = time_now;
 		}

 		_innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY;
@@ -2428,7 +2428,7 @@ Commander::run()
 				}
 			}

-			if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) {
+			if (!_armed.armed && _manual_control.isModeInitializationRequired() && (_internal_state.main_state_changes == 0)) {
 				// if there's never been a mode change force position control as initial state
 				_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
 			}
@@ -2842,7 +2842,7 @@ Commander::run()
 			/* play tune on battery warning */
 			set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);

-		} else if (_status.failsafe) {
+		} else if (_status.failsafe && _armed.armed) {
 			tune_failsafe(true);

 		} else {
@@ -177,6 +177,14 @@ bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode,
 	return ret;
 }

+bool ManualControl::isModeInitializationRequired()
+{
+	const bool is_mavlink = _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
+	const bool rc_uses_toggle_buttons = _param_rc_map_flightmode_buttons.get() > 0;
+
+	return (is_mavlink || rc_uses_toggle_buttons);
+}
+
 void ManualControl::updateParams()
 {
 	ModuleParams::updateParams();
@@ -64,12 +64,12 @@ public:
 	 */
 	bool update();
 	bool isRCAvailable() const { return _rc_available; }
-	bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; }
 	bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status);
 	bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
 			 manual_control_switches_s &manual_control_switches, const bool landed);
 	bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
 		      const manual_control_switches_s &manual_control_switches, const bool landed);
+	bool isModeInitializationRequired();
 	bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; }
 	bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; }
 	hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; }
@@ -96,6 +96,7 @@ private:
 		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
 		(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
-		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
+		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
+		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons
 	)
 };
@@ -145,16 +145,17 @@ void PositionControl::_velocityControl(const float dt)
 	// Prioritize vertical control while keeping a horizontal margin
 	const Vector2f thrust_sp_xy(_thr_sp);
 	const float thrust_sp_xy_norm = thrust_sp_xy.norm();
+	const float thrust_max_squared = math::sq(_lim_thr_max);

 	// Determine how much vertical thrust is left keeping horizontal margin
 	const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
-	const float thrust_z_max_squared = math::sq(_lim_thr_max) - math::sq(allocated_horizontal_thrust);
+	const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);

 	// Saturate maximal vertical thrust
 	_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));

 	// Determine how much horizontal thrust is left after prioritizing vertical control
-	const float thrust_max_xy_squared = thrust_z_max_squared - math::sq(_thr_sp(2));
+	const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
 	float thrust_max_xy = 0;

 	if (thrust_max_xy_squared > 0) {
@@ -78,8 +78,8 @@ public:
 		_position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f));
 		_position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f));
 		_position_control.setVelocityLimits(1.f, 1.f, 1.f);
-		_position_control.setThrustLimits(0.1f, 0.9f);
-		_position_control.setHorizontalThrustMargin(0.3f);
+		_position_control.setThrustLimits(0.1f, MAXIMUM_THRUST);
+		_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
 		_position_control.setTiltLimit(1.f);
 		_position_control.setHoverThrust(.5f);

@@ -113,6 +113,9 @@ public:
 	vehicle_local_position_setpoint_s _input_setpoint{};
 	vehicle_local_position_setpoint_s _output_setpoint{};
 	vehicle_attitude_setpoint_s _attitude{};
+
+	static constexpr float MAXIMUM_THRUST = 0.9f;
+	static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f;
 };

 class PositionControlBasicDirectionTest : public PositionControlBasicTest
@@ -186,24 +189,34 @@ TEST_F(PositionControlBasicTest, VelocityLimit)

 TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 {
+	// Given a setpoint that drives the controller into vertical and horizontal saturation
 	_input_setpoint.x = 10.f;
 	_input_setpoint.y = 10.f;
 	_input_setpoint.z = -10.f;

+	// When you run it for one iteration
 	runController();
 	Vector3f thrust(_output_setpoint.thrust);
-	EXPECT_FLOAT_EQ(thrust(0), 0.f);
-	EXPECT_FLOAT_EQ(thrust(1), 0.f);
-	// Expect the remaining vertical thrust after allocating the horizontal margin
-	// sqrt(0.9^2 - 0.3^2) = 0.8485
-	EXPECT_FLOAT_EQ(thrust(2), -0.848528137423857f);

+	// Then the thrust vector length is limited by the maximum
+	EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST);
+
+	// Then the horizontal thrust is limited by its margin
+	EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
+	EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
+	EXPECT_FLOAT_EQ(thrust(2),
+			-sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN));
+	thrust.print();
+
+	// Then the collective thrust is limited by the maximum
 	EXPECT_EQ(_attitude.thrust_body[0], 0.f);
 	EXPECT_EQ(_attitude.thrust_body[1], 0.f);
-	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.848528137423857f);
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);

-	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
-	EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
+	// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
+	EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
+	// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
+	// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
 }

 TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
@@ -1701,6 +1701,40 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
 */
 PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);

+/**
+ * Button flight mode selection
+ *
+ * This bitmask allows to specify multiple channels for changing flight modes using
+ * momentary buttons. Each channel is assigned to a mode slot ((lowest channel = slot 1).
+ * The resulting modes for each slot X is defined by the COM_FLTMODEX parameters.
+ * The functionality can be used only if RC_MAP_FLTMODE is disabled.
+ *
+ * The maximum number of available slots and hence bits set in the mask is 6.
+ * @min 0
+ * @max 258048
+ * @group Radio Switches
+ * @bit 0 Mask Channel 1 as a mode button
+ * @bit 1 Mask Channel 2 as a mode button
+ * @bit 2 Mask Channel 3 as a mode button
+ * @bit 3 Mask Channel 4 as a mode button
+ * @bit 4 Mask Channel 5 as a mode button
+ * @bit 5 Mask Channel 6 as a mode button
+ * @bit 6 Mask Channel 7 as a mode button
+ * @bit 7 Mask Channel 8 as a mode button
+ * @bit 8 Mask Channel 9 as a mode button
+ * @bit 9 Mask Channel 10 as a mode button
+ * @bit 10 Mask Channel 11 as a mode button
+ * @bit 11 Mask Channel 12 as a mode button
+ * @bit 12 Mask Channel 13 as a mode button
+ * @bit 13 Mask Channel 14 as a mode button
+ * @bit 14 Mask Channel 15 as a mode button
+ * @bit 15 Mask Channel 16 as a mode button
+ * @bit 16 Mask Channel 17 as a mode button
+ * @bit 17 Mask Channel 18 as a mode button
+ */
+
+PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);
+
 /**
 * AUX1 Passthrough RC channel
 *
@@ -103,8 +103,9 @@ RCUpdate::RCUpdate() :
 	}

 	rc_parameter_map_poll(true /* forced */);
-
 	parameters_updated();
+
+	_button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms);
 }

 RCUpdate::~RCUpdate()
@@ -202,6 +203,8 @@ void RCUpdate::update_rc_functions()
 	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 		_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
+
+	map_flight_modes_buttons();
 }

 void RCUpdate::rc_parameter_map_poll(bool forced)
@@ -276,6 +279,42 @@ void RCUpdate::set_params_from_rc()
 	}
 }

+void
+RCUpdate::map_flight_modes_buttons()
+{
+	static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + manual_control_switches_s::MODE_SLOT_NUM <= sizeof(
+			      _rc.function) / sizeof(_rc.function[0]), "Unexpected number of RC functions");
+	static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_COUNT == manual_control_switches_s::MODE_SLOT_NUM,
+		      "Unexpected number of Flight Modes slots");
+
+	// Reset all the slots to -1
+	for (uint8_t slot = 0; slot < manual_control_switches_s::MODE_SLOT_NUM; slot++) {
+		_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = -1;
+	}
+
+	// If the functionality is disabled we don't need to map channels
+	const int flightmode_buttons = _param_rc_map_flightmode_buttons.get();
+
+	if (flightmode_buttons == 0) {
+		return;
+	}
+
+	uint8_t slot = 0;
+
+	for (uint8_t channel = 0; channel < RC_MAX_CHAN_COUNT; channel++) {
+		if (flightmode_buttons & (1 << channel)) {
+			PX4_DEBUG("Slot %d assigned to channel %d", slot + 1, channel);
+			_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = channel;
+			slot++;
+		}
+
+		if (slot >= manual_control_switches_s::MODE_SLOT_NUM) {
+			// we have filled all the available slots
+			break;
+		}
+	}
+}
+
 void RCUpdate::Run()
 {
 	if (should_exit()) {
@@ -427,9 +466,16 @@ void RCUpdate::Run()
 			}
 		}

+		/*
+		 * some RC systems glitch after a reboot, we should ignore the first 100ms of regained signal
+		 * as the glitch might be interpreted as a commanded stick action or a flight mode switch
+		 */
+		_rc_signal_lost_hysteresis.set_hysteresis_time_from(true, 100_ms);
+		_rc_signal_lost_hysteresis.set_state_and_update(signal_lost, hrt_absolute_time());
+
 		_rc.channel_count = input_rc.channel_count;
 		_rc.rssi = input_rc.rssi;
-		_rc.signal_lost = signal_lost;
+		_rc.signal_lost = _rc_signal_lost_hysteresis.get_state();
 		_rc.timestamp = input_rc.timestamp_last_signal;
 		_rc.frame_drop_count = input_rc.rc_lost_frame_count;

@@ -437,7 +483,7 @@ void RCUpdate::Run()
 		_rc_channels_pub.publish(_rc);

 		// only publish manual control if the signal is present and regularly updating
-		if (input_source_stable && channel_count_stable && !signal_lost) {
+		if (input_source_stable && channel_count_stable && !_rc_signal_lost_hysteresis.get_state()) {

 			if ((input_rc.timestamp_last_signal > _last_timestamp_signal)
 			    && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) {
@@ -557,6 +603,36 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 		switches.acro_switch      = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO,      _param_rc_acro_th.get());
 		switches.stab_switch      = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB,      _param_rc_stab_th.get());
 		switches.posctl_switch    = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL,    _param_rc_posctl_th.get());
+
+	} else if (_param_rc_map_flightmode_buttons.get() > 0) {
+		switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
+		bool is_consistent_button_press = false;
+
+		for (uint8_t slot = 0; slot < manual_control_switches_s::MODE_SLOT_NUM; slot++) {
+
+			// If the slot is not in use (-1), get_rc_value() will return 0
+			float value = get_rc_value(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot, -1.0, 1.0);
+
+			// The range goes from -1 to 1, checking that value is greater than 0.5f
+			// corresponds to check that the signal is above 75% of the overall range.
+			if (value > 0.5f) {
+				const uint8_t current_button_press_slot = slot + 1;
+
+				// The same button stays pressed consistently
+				if (current_button_press_slot == _potential_button_press_slot) {
+					is_consistent_button_press = true;
+				}
+
+				_potential_button_press_slot = current_button_press_slot;
+				break;
+			}
+		}
+
+		_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, hrt_absolute_time());
+
+		if (_button_pressed_hysteresis.get_state()) {
+			switches.mode_slot = _potential_button_press_slot;
+		}
 	}

 	switches.return_switch     = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN,     _param_rc_return_th.get());
@@ -59,6 +59,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/parameter_update.h>
+#include <hysteresis/hysteresis.h>

 using namespace time_literals;

@@ -130,6 +131,8 @@ private:
 	 */
 	void		set_params_from_rc();

+	void		map_flight_modes_buttons();
+
 	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 {
@@ -186,6 +189,10 @@ private:
 	uint8_t _channel_count_previous{0};
 	uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};

+	uint8_t _potential_button_press_slot{0};
+	systemlib::Hysteresis _button_pressed_hysteresis{false};
+	systemlib::Hysteresis _rc_signal_lost_hysteresis{true};
+
 	uint8_t _channel_count_max{0};

 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -216,6 +223,7 @@ private:
 		(ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
 		(ParamInt<px4::params::RC_MAP_STAB_SW>) _param_rc_map_stab_sw,
 		(ParamInt<px4::params::RC_MAP_MAN_SW>) _param_rc_map_man_sw,
+		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,

 		(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
 		(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,
Author	SHA1	Message	Date
Daniel Agar	9bbe1aa419	lib/rc: don't share decode buffer between protocols	2021-09-16 14:40:26 -04:00
Daniel Agar	7af5a33ff3	commander: don't play failsafe tune if disarmed	2021-09-16 09:17:00 -04:00
Matthias Grob	ecb1264832	vscode: do not disable autocomplete on enter	2021-09-16 09:11:52 -04:00
Julian Oes	c779946e05	setup: Use available Java version for Ubuntu 20.04 14 was not available for me, 16 would have been.	2021-09-16 13:59:27 +02:00
Julian Oes	addb978364	setup: Let's not ignore errors Otherwise the script finishes and you don't know if it worked.	2021-09-16 13:59:27 +02:00
Daniel Agar	3d56836850	boards: nxp_fmurt1062-v1 disable extra barometers to save flash	2021-09-15 12:59:36 -04:00
Thomas Stauber	2b80a6958a	fix disable airspeed check with negative ASPD_FS_INTEG (#18186 ) * fix disable airspeed check with negative ASPD_FS_INTEG * improve logic when nav velocity data is not good * simplify logic. Reset integrator state when the check is not run.	2021-09-14 12:13:56 +02:00
Matthias Grob	e7a90bf367	PositionControl: correct horizontal margin calculation It was using the already reduced vertical thrust to do the horizontal limitation resulting in no margin.	2021-09-14 10:57:38 +02:00
Matthias Grob	d1f1e02afb	Refactor mode button changes	2021-09-14 09:52:32 +02:00
Claudio Micheli	05d40f40d4	Handle rc toggle mode buttons to initialize flight modes Signed-off-by: Claudio Micheli <claudio@auterion.com>	2021-09-14 09:52:32 +02:00
Claudio Micheli	c50daae4a3	rc_update: introduce support for toggle buttons via RC channels Signed-off-by: Claudio Micheli <claudio@auterion.com>	2021-09-14 09:52:32 +02:00
Daniel Agar	ca2d8f6de2	Update submodule simulation-ignition to latest Tue Sep 14 00:39:01 UTC 2021	2021-09-13 21:09:50 -04:00