FWPosCtrl: Rearrange VTOL transition code into eparate functions.

Signed-off-by: Julian Oes <julian@oes.ch>

.github/workflows/stale.yml

@@ -0,0 +1,20 @@
+name: 'Close stale issues and PRs'
+on:
+  schedule:
+    - cron: '30 1 * * *'
+
+jobs:
+  stale:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/stale@v4.1.1
+        with:
+          stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.'
+          stale-pr-message: 'This PR is stale because it has been open 45 days with no activity. Remove stale label or comment or this will be closed in 10 days.'
+          close-issue-message: 'This issue was closed because it has been stalled for 5 days with no activity.'
+          close-pr-message: 'This PR was closed because it has been stalled for 10 days with no activity.'
+          days-before-issue-stale: 30
+          days-before-pr-stale: 45
+          days-before-issue-close: 5
+          days-before-pr-close: 10
+          debug-only: true

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -17,6 +17,7 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -48,35 +48,32 @@ param set-default PWM_MAIN_REV 96 # invert both elevons
 param set-default EKF2_MULTI_IMU 0
 param set-default SENS_IMU_MODE 1
 
-param set-default NPFG_PERIOD 12
-param set-default FW_PR_I 0.2
-param set-default FW_PR_P 0.2
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_FF 0.1
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_P 0.2
-param set-default FW_THR_TRIM 0.33
-param set-default FW_THR_MAX 0.6
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.2
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
 param set-default FW_THR_MIN 0.05
-param set-default FW_T_ALT_TC 2
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-param set-default FW_T_TAS_TC 2
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
 
 param set-default MC_AIRMODE 1
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
 param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
 
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-
+param set-default VT_ARSP_TRANS 10
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.5
+param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
-param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
 
 param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter

@@ -0,0 +1,74 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 0
+
+parm set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -60,6 +60,7 @@ px4_add_romfs_files(
 	1042_gazebo-classic_tiltrotor
 	1043_gazebo-classic_standard_vtol_drop
 	1044_gazebo-classic_plane_lidar
+	1045_gazebo-classic_quadtailsitter
 	1060_gazebo-classic_rover
 	1061_gazebo-classic_r1_rover
 	1062_flightgear_tf-r1

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -20,6 +20,7 @@
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
 
 param set UAVCAN_ENABLE 0
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -31,3 +31,4 @@ param set-default CA_SV_CS1_TYPE 6
 param set-default MAV_TYPE 19
 param set-default VT_TYPE 0
 param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_B_TRANS_DUR 5

Tools/simulation/sitl_multiple_run.sh

@@ -10,7 +10,7 @@ sitl_num=2
 [ -n "$1" ] && sitl_num="$1"
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-src_path="$SCRIPT_DIR/.."
+src_path="$SCRIPT_DIR/../../"
 
 build_path=${src_path}/build/px4_sitl_default
 

boards/ark/cannode/default.px4board

@@ -20,6 +20,7 @@ CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_COMMON_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y

boards/px4/fmu-v6x/nuttx-config/nsh/defconfig

@@ -291,8 +291,9 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=1500
 CONFIG_UART5_RXDMA=y
-CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y

platforms/common/px4_log.cpp

@@ -103,7 +103,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
+			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -138,12 +138,12 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1);
-			pos += sprintf(buf + sz, "%s\n", PX4_ANSI_COLOR_RESET);
+			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET);
 
 		} else
 #endif // PX4_LOG_COLORIZED_OUTPUT
 		{
-			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
+			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
 		}
 
 		// ensure NULL termination (buffer is max_length + 1)
@@ -162,7 +162,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 			va_start(argptr, fmt);
 			pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr);
 			va_end(argptr);
-			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
+			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
 			buf[max_length] = 0; // ensure NULL termination
 		}
 
@@ -220,7 +220,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
+			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -235,7 +235,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET));
-			pos += sprintf(buf + sz, "%s", PX4_ANSI_COLOR_RESET);
+			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s", PX4_ANSI_COLOR_RESET);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -61,12 +61,13 @@ static BlockingList<WorkQueue *> *_wq_manager_wqs_list{nullptr};
 static BlockingQueue<const wq_config_t *, 1> *_wq_manager_create_queue{nullptr};
 
 static px4::atomic_bool _wq_manager_should_exit{true};
+static px4::atomic_bool _wq_manager_running{false};
 
 
 static WorkQueue *
 FindWorkQueueByName(const char *name)
 {
-	if (_wq_manager_wqs_list == nullptr) {
+	if (!_wq_manager_running.load()) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -86,7 +87,7 @@ FindWorkQueueByName(const char *name)
 WorkQueue *
 WorkQueueFindOrCreate(const wq_config_t &new_wq)
 {
-	if (_wq_manager_create_queue == nullptr) {
+	if (!_wq_manager_running.load()) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -258,6 +259,7 @@ WorkQueueManagerRun(int, char **)
 {
 	_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
 	_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
+	_wq_manager_running.store(true);
 
 	while (!_wq_manager_should_exit.load()) {
 		// create new work queues as needed
@@ -361,13 +363,15 @@ WorkQueueManagerRun(int, char **)
 		}
 	}
 
+	_wq_manager_running.store(false);
+
 	return 0;
 }
 
 int
 WorkQueueManagerStart()
 {
-	if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) {
+	if (_wq_manager_should_exit.load() && !_wq_manager_running.load()) {
 
 		_wq_manager_should_exit.store(false);
 
@@ -384,6 +388,18 @@ WorkQueueManagerStart()
 			return -errno;
 		}
 
+		// Wait until initialized
+		int max_tries = 1000;
+
+		while (!_wq_manager_running.load() && --max_tries > 0) {
+			px4_usleep(1000);
+		}
+
+		if (max_tries <= 0) {
+			PX4_ERR("failed to wait for task to start");
+			return PX4_ERROR;
+		}
+
 	} else {
 		PX4_WARN("already running");
 		return PX4_ERROR;
@@ -398,7 +414,7 @@ WorkQueueManagerStop()
 	if (!_wq_manager_should_exit.load()) {
 
 		// error can't shutdown until all WorkItems are removed/stopped
-		if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) {
+		if (_wq_manager_running.load() && (_wq_manager_wqs_list->size() > 0)) {
 			PX4_ERR("can't shutdown with active WQs");
 			WorkQueueManagerStatus();
 			return PX4_ERROR;
@@ -422,6 +438,7 @@ WorkQueueManagerStop()
 			}
 
 			delete _wq_manager_wqs_list;
+			_wq_manager_wqs_list = nullptr;
 		}
 
 		_wq_manager_should_exit.store(true);
@@ -433,6 +450,7 @@ WorkQueueManagerStop()
 			px4_usleep(10000);
 
 			delete _wq_manager_create_queue;
+			_wq_manager_create_queue = nullptr;
 		}
 
 	} else {
@@ -446,7 +464,7 @@ WorkQueueManagerStop()
 int
 WorkQueueManagerStatus()
 {
-	if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {
+	if (!_wq_manager_should_exit.load() && _wq_manager_running.load()) {
 
 		const size_t num_wqs = _wq_manager_wqs_list->size();
 		PX4_INFO_RAW("\nWork Queue: %-2zu threads                          RATE        INTERVAL\n", num_wqs);

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -111,7 +111,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 
 	if (pubsubtest_print && timings) {
 		char fname[32] {};
-		sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
+		snprintf(fname, sizeof(fname), PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
 		FILE *f = fopen(fname, "w");
 
 		if (f == nullptr) {

platforms/nuttx/src/px4/common/px4_init.cpp

@@ -127,8 +127,6 @@ int px4_platform_init()
 	hrt_ioctl_init();
 #endif
 
-	param_init();
-
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
@@ -180,9 +178,10 @@ int px4_platform_init()
 
 #endif // CONFIG_FS_BINFS
 
-
 	px4::WorkQueueManagerStart();
 
+	param_init();
+
 	uorb_start();
 
 	px4_log_initialize();

platforms/posix/src/px4/common/px4_init.cpp

@@ -44,10 +44,10 @@ int px4_platform_init(void)
 {
 	hrt_init();
 
-	param_init();
-
 	px4::WorkQueueManagerStart();
 
+	param_init();
+
 	uorb_start();
 
 	px4_log_initialize();

src/drivers/px4io/px4io.cpp

@@ -209,7 +209,7 @@ private:
 	bool			_timer_rates_configured{false};
 
 	/* advertised topics */
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<input_rc_s>	_input_rc_pub{ORB_ID(input_rc)};
 	uORB::Publication<px4io_status_s>	_px4io_status_pub{ORB_ID(px4io_status)};
 
 	ButtonPublisher	_button_publisher;
@@ -1144,7 +1144,7 @@ int PX4IO::io_publish_raw_rc()
 		input_rc.link_quality = -1;
 		input_rc.rssi_dbm = NAN;
 
-		_to_input_rc.publish(input_rc);
+		_input_rc_pub.publish(input_rc);
 	}
 
 	return ret;

src/drivers/rc_input/RCInput.cpp

@@ -192,16 +192,16 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 		    unsigned frame_drops, int rssi = -1)
 {
 	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
+	_input_rc.channel_count = raw_rc_count_local;
 
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	if (_input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
 
 	unsigned valid_chans = 0;
 
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
+	for (unsigned i = 0; i < _input_rc.channel_count; i++) {
+		_input_rc.values[i] = raw_rc_values_local[i];
 
 		if (raw_rc_values_local[i] != UINT16_MAX) {
 			valid_chans++;
@@ -211,20 +211,20 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 		_raw_rc_values[i] = UINT16_MAX;
 	}
 
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
+	_input_rc.timestamp = now;
+	_input_rc.timestamp_last_signal = _input_rc.timestamp;
+	_input_rc.rc_ppm_frame_length = 0;
 
 	/* fake rssi if no value was provided */
 	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
 
 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((_input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+			_input_rc.rssi = math::constrain(rc_rssi, 0, 100);
 
 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
@@ -238,24 +238,24 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 				rssi_analog = 0.0f;
 			}
 
-			_rc_in.rssi = rssi_analog;
+			_input_rc.rssi = rssi_analog;
 
 		} else {
-			_rc_in.rssi = 255;
+			_input_rc.rssi = 255;
 		}
 
 	} else {
-		_rc_in.rssi = rssi;
+		_input_rc.rssi = rssi;
 	}
 
 	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
+		_input_rc.rssi = 0;
 	}
 
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
+	_input_rc.rc_failsafe = failsafe;
+	_input_rc.rc_lost = (valid_chans == 0);
+	_input_rc.rc_lost_frame_count = frame_drops;
+	_input_rc.rc_total_frame_count = 0;
 }
 
 void RCInput::set_rc_scan_state(RC_SCAN newState)
@@ -468,7 +468,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   sbus_frame_drop, sbus_failsafe, frame_drops);
 						_rc_scan_locked = true;
@@ -506,7 +506,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   false, false, frame_drops, dsm_rssi);
 						_rc_scan_locked = true;
@@ -552,14 +552,14 @@ void RCInput::Run()
 					if (rc_updated) {
 						if (lost_count == 0) {
 							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
+							_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
 							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 								   false, false, frame_drops, st24_rssi);
 							_rc_scan_locked = true;
 
 						} else {
 							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
+							_input_rc.rc_lost = true;
 						}
 					}
 				}
@@ -600,7 +600,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   false, sumd_failsafe, frame_drops, sumd_rssi);
 						_rc_scan_locked = true;
@@ -625,14 +625,14 @@ void RCInput::Run()
 			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
 				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
+				if ((ppm_last_valid_decode != _input_rc.timestamp_last_signal) && ppm_decoded_channels > 3) {
 					// we have a new PPM frame. Publish it.
 					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
 					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
 					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
+					_input_rc.rc_ppm_frame_length = ppm_frame_length;
+					_input_rc.timestamp_last_signal = ppm_last_valid_decode;
 				}
 
 			} else {
@@ -669,7 +669,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
 
 						// on Pixhawk (-related) boards we cannot write to the RC UART
@@ -718,7 +718,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
 
 						// ghst telemetry works on fmu-v5
@@ -753,12 +753,12 @@ void RCInput::Run()
 		if (rc_updated) {
 			perf_count(_publish_interval_perf);
 
-			_rc_in.link_quality = -1;
-			_rc_in.rssi_dbm = NAN;
+			_input_rc.link_quality = -1;
+			_input_rc.rssi_dbm = NAN;
 
-			_to_input_rc.publish(_rc_in);
+			_input_rc_pub.publish(_input_rc);
 
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_input_rc.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -907,8 +907,8 @@ int RCInput::print_status()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_publish_interval_perf);
 
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
+	if (hrt_elapsed_time(&_input_rc.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _input_rc);
 	}
 
 	return 0;

src/drivers/rc_input/RCInput.hpp

@@ -127,6 +127,7 @@ private:
 
 	void rc_io_invert(bool invert);
 
+	input_rc_s _input_rc{};
 	hrt_abstime _rc_scan_begin{0};
 
 	bool _initialized{false};
@@ -140,16 +141,13 @@ private:
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	input_rc_s	_rc_in{};
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
 
 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};
 
 	bool _armed{false};
 
-
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
-
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
 

src/drivers/uavcan/arming_status.cpp

@@ -69,7 +69,7 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
 		if (actuator_armed.lockdown || actuator_armed.manual_lockdown) {
 			cmd.status = cmd.STATUS_DISARMED;
 
-		} else if (actuator_armed.armed || actuator_armed.prearmed) {
+		} else if (actuator_armed.armed) {
 			cmd.status = cmd.STATUS_FULLY_ARMED;
 
 		} else {

src/drivers/uavcannode/Publishers/GnssAuxiliary.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+#include "UavcanPublisherBase.hpp"
+
+#include <uavcan/equipment/gnss/Auxiliary.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+namespace uavcannode
+{
+
+class GnssAuxiliary :
+	public UavcanPublisherBase,
+	public uORB::SubscriptionCallbackWorkItem,
+	private uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>
+{
+public:
+	GnssAuxiliary(px4::WorkItem *work_item, uavcan::INode &node) :
+		UavcanPublisherBase(uavcan::equipment::gnss::Auxiliary::DefaultDataTypeID),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)),
+		uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>(node)
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
+
+	void PrintInfo() override
+	{
+		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       uavcan::equipment::gnss::Auxiliary::getDataTypeFullName(),
+			       id());
+		}
+	}
+
+	void BroadcastAnyUpdates() override
+	{
+		using uavcan::equipment::gnss::Auxiliary;
+
+		// sensor_gps -> uavcan::equipment::gnss::Auxiliary
+		sensor_gps_s gps;
+
+		if (uORB::SubscriptionCallbackWorkItem::update(&gps)) {
+			uavcan::equipment::gnss::Auxiliary auxiliary{};
+
+			//auxiliary.gdop = gps.gdop;
+			//auxiliary.pdop = gps.pdop;
+			auxiliary.hdop = gps.hdop;
+			auxiliary.vdop = gps.vdop;
+			//auxiliary.tdop = gps.tdop;
+			//auxiliary.ndop = gps.ndop;
+			//auxiliary.edop = gps.edop;
+
+			auxiliary.sats_visible = gps.satellites_used;
+			auxiliary.sats_used = gps.satellites_used;
+
+			uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>::broadcast(auxiliary);
+
+			// ensure callback is registered
+			uORB::SubscriptionCallbackWorkItem::registerCallback();
+		}
+	}
+};
+} // namespace uavcannode

src/drivers/uavcannode/Subscribers/ArmingStatus.hpp

@@ -89,6 +89,7 @@ private:
 			actuator_armed.armed = false;
 		}
 
+		actuator_armed.prearmed = true;
 		actuator_armed.timestamp = hrt_absolute_time();
 		_actuator_armed_pub.publish(actuator_armed);
 	}

src/drivers/uavcannode/UavcanNode.cpp

@@ -58,6 +58,7 @@
 
 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 #include "Publishers/GnssFix2.hpp"
+#include "Publishers/GnssAuxiliary.hpp"
 #endif // CONFIG_UAVCANNODE_GNSS_FIX
 
 #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)
@@ -366,6 +367,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 
 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 	_publisher_list.add(new GnssFix2(this, _node));
+	_publisher_list.add(new GnssAuxiliary(this, _node));
 #endif // CONFIG_UAVCANNODE_GNSS_FIX
 
 #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)

src/lib/events/enums.json

@@ -240,6 +240,10 @@
                         "536870912": {
                             "name": "gyro",
                             "description": "Gyroscope"
+                        },
+                        "1073741824": {
+                            "name": "open_drone_id",
+                            "description": "Open Drone ID system"
                         }
                     }
                 },

src/lib/parameters/CMakeLists.txt

@@ -156,7 +156,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
 
 set(SRCS)
 
-list(APPEND SRCS parameters.cpp)
+list(APPEND SRCS parameters.cpp atomic_transaction.cpp autosave.cpp)
 
 if(BUILD_TESTING)
 	list(APPEND SRCS param_translation_unit_tests.cpp)

src/lib/parameters/ConstLayer.h

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ParamLayer.h"
+
+class ConstLayer : public ParamLayer
+{
+public:
+
+	ConstLayer() = default;
+
+	bool store(param_t param, param_value_u value) override
+	{
+		return false;
+	}
+
+	bool contains(param_t param) const override
+	{
+		return param < PARAM_COUNT;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+
+		for (int i = 0; i < PARAM_COUNT; i++) {
+			set.set(i);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		if (param >= PARAM_COUNT) {
+			return {0};
+		}
+
+		return px4::parameters[param].val;
+	}
+
+	void reset(param_t param) override
+	{
+		// Do nothing
+	}
+
+	void refresh(param_t param) override
+	{
+		// Do nothing
+	}
+
+	int size() const override
+	{
+		return PARAM_COUNT;
+	}
+
+	int byteSize() const override
+	{
+		return PARAM_COUNT * sizeof(param_info_s);
+	}
+};

src/lib/parameters/DynamicSparseLayer.h

@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ParamLayer.h"
+
+#include <px4_platform_common/atomic.h>
+
+class DynamicSparseLayer : public ParamLayer
+{
+public:
+	DynamicSparseLayer(ParamLayer *parent, int n_prealloc = 32, int n_grow = 4) : ParamLayer(parent),
+		_n_slots(n_prealloc), _n_grow(n_grow)
+	{
+		Slot *slots = (Slot *)malloc(sizeof(Slot) * n_prealloc);
+
+		if (slots == nullptr) {
+			PX4_ERR("Failed to allocate memory for dynamic sparse layer");
+			_n_slots = 0;
+			return;
+		}
+
+		for (int i = 0; i < _n_slots; i++) {
+			slots[i] = {UINT16_MAX, param_value_u{}};
+		}
+
+		_slots.store(slots);
+	}
+
+	virtual ~DynamicSparseLayer()
+	{
+		if (_slots.load()) {
+			free(_slots.load());
+		}
+	}
+
+	bool store(param_t param, param_value_u value) override
+	{
+		AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // already exists
+			slots[index].value = value;
+
+		} else if (_next_slot < _n_slots) {
+			slots[_next_slot++] = {param, value};
+			_sort();
+
+		} else {
+			if (!_grow(transaction)) {
+				return false;
+			}
+
+			_slots.load()[_next_slot++] = {param, value};
+			_sort();
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		return _getIndex(param) < _next_slot;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+		const AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		for (int i = 0; i < _next_slot; i++) {
+			set.set(slots[i].param);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // exists in our data structure
+			return slots[index].value;
+		}
+
+		return _parent->get(param);
+	}
+
+	void reset(param_t param) override
+	{
+		const AtomicTransaction transaction;
+		int index = _getIndex(param);
+		Slot *slots = _slots.load();
+
+		if (index < _next_slot) {
+			slots[index] = {UINT16_MAX, param_value_u{}};
+			_sort();
+			_next_slot--;
+		}
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+	}
+
+	int size() const override
+	{
+		return _next_slot;
+	}
+
+	int byteSize() const override
+	{
+		return _n_slots * sizeof(Slot);
+	}
+
+private:
+	struct Slot {
+		param_t param;
+		param_value_u value;
+	};
+
+	static int _slotCompare(const void *a, const void *b)
+	{
+		return ((int)((Slot *)a)->param) - ((int)((Slot *)b)->param);
+	}
+
+	void _sort()
+	{
+		qsort(_slots.load(), _n_slots, sizeof(Slot), _slotCompare);
+	}
+
+	int _getIndex(param_t param) const
+	{
+		int left = 0;
+		int right = _next_slot - 1;
+		Slot *slots = _slots.load();
+
+		while (left <= right) {
+			int mid = (left + right) / 2;
+
+			if (slots[mid].param == param) {
+				return mid;
+
+			} else if (slots[mid].param < param) {
+				left = mid + 1;
+
+			} else {
+				right = mid - 1;
+			}
+		}
+
+		return _next_slot;
+	}
+
+	bool _grow(AtomicTransaction &transaction)
+	{
+		if (_n_slots == 0) {
+			return false;
+		}
+
+		int max_retries = 5;
+
+		// As malloc uses locking, so we need to re-enable IRQ's during malloc/free and
+		// then atomically exchange the buffer
+		while (_next_slot >= _n_slots && max_retries-- > 0) {
+			Slot *previous_slots = nullptr;
+			Slot *new_slots = nullptr;
+
+			do {
+				previous_slots = _slots.load();
+				transaction.unlock();
+
+				if (new_slots) {
+					free(new_slots);
+				}
+
+				new_slots = (Slot *) malloc(sizeof(Slot) * (_n_slots + _n_grow));
+				transaction.lock();
+
+				if (new_slots == nullptr) {
+					return false;
+				}
+
+			} while (!_slots.compare_exchange(&previous_slots, new_slots));
+
+			memcpy(new_slots, previous_slots, sizeof(Slot) * _n_slots);
+
+			for (int i = _n_slots; i < _n_slots + _n_grow; i++) {
+				new_slots[i] = {UINT16_MAX, param_value_u{}};
+			}
+
+			_n_slots += _n_grow;
+
+			transaction.unlock();
+			free(previous_slots);
+			transaction.lock();
+		}
+
+		return _next_slot < _n_slots;
+	}
+
+	int _next_slot = 0;
+	int _n_slots = 0;
+	const int _n_grow;
+	px4::atomic<Slot *> _slots{nullptr};
+};

src/lib/parameters/ExhaustiveLayer.h

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/atomic_bitset.h>
+#include "ParamLayer.h"
+
+class ExhaustiveLayer : public ParamLayer
+{
+public:
+	ExhaustiveLayer(ParamLayer *parent) : ParamLayer(parent)
+	{
+		// refresh all values
+		for (param_t i = 0; i < PARAM_COUNT; i++) {
+			_values[i] = parent->get(i);
+		}
+	}
+
+	bool store(param_t param, param_value_u value) override
+	{
+		if (param >= PARAM_COUNT) {
+			return false;
+		}
+
+		{
+			const AtomicTransaction transaction;
+			_values[param] = value;
+			_ownership_set.set(param);
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		return param < PARAM_COUNT && _ownership_set[param];
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		return _ownership_set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		if (param >= PARAM_COUNT) {
+			return {0};
+		}
+
+		const AtomicTransaction transaction;
+		// We assume to have the correct values for all params, even without ownership.
+		// We expect that refresh was called when underlying defaults changed
+		return _values[param];
+	}
+
+	void reset(param_t param) override
+	{
+		if (param >= PARAM_COUNT) {
+			return;
+		}
+
+		const AtomicTransaction transaction;
+		_values[param] = _parent->get(param);
+		_ownership_set.set(param, false);
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+		// in case we don't have ownership, and it changed below, we have to refresh our cache.
+		{
+			const AtomicTransaction transaction;
+
+			if (!contains(param)) {
+				_values[param] = _parent->get(param);
+			}
+		}
+	}
+
+	int size() const override
+	{
+		return _ownership_set.count();
+	}
+
+	int byteSize() const override
+	{
+		return PARAM_COUNT * sizeof(param_value_u);
+	}
+
+private:
+	param_value_u _values[PARAM_COUNT];
+	px4::AtomicBitset<PARAM_COUNT> _ownership_set;
+};

src/lib/parameters/ParamLayer.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/atomic_bitset.h>
+#include "atomic_transaction.h"
+#include "param.h"
+#include <stdlib.h>
+
+class ParamLayer
+{
+public:
+	static constexpr param_t PARAM_COUNT = sizeof(px4::parameters) / sizeof(param_info_s);
+
+	ParamLayer(ParamLayer *parent = nullptr) : _parent(parent) {}
+
+	ParamLayer(const ParamLayer &) = delete;
+	ParamLayer &operator=(const ParamLayer &) = delete;
+	ParamLayer(ParamLayer &&) = delete;
+	ParamLayer &operator=(ParamLayer &&) = delete;
+
+
+	virtual bool store(param_t param, param_value_u value) = 0;
+
+	virtual bool contains(param_t param) const = 0;
+
+	virtual px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const = 0;
+
+	virtual param_value_u get(param_t param) const = 0;
+
+	virtual void reset(param_t param) = 0;
+
+	virtual void refresh(param_t param) = 0;
+
+	virtual int size() const = 0;
+
+	virtual int byteSize() const = 0;
+
+protected:
+	ParamLayer *const _parent;
+};

src/lib/parameters/StaticSparseLayer.h

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdlib.h>
+#include "ParamLayer.h"
+
+template <int N_SLOTS>
+class StaticSparseLayer : public ParamLayer
+{
+public:
+	StaticSparseLayer(ParamLayer *parent) : ParamLayer(parent)
+	{
+		for (int i = 0; i < N_SLOTS; i++) {
+			_slots[i] = {UINT16_MAX, param_value_u{}};
+		}
+	}
+
+	virtual ~StaticSparseLayer() = default;
+
+	bool store(param_t param, param_value_u value) override
+	{
+		const AtomicTransaction transaction;
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // already exists
+			_slots[index].value = value;
+
+		} else if (_next_slot < N_SLOTS) {
+			_slots[_next_slot++] = {param, value};
+			_sort();
+
+		} else {
+			return false;
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		return _getIndex(param) < _next_slot;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+		const AtomicTransaction transaction;
+
+		for (int i = 0; i < _next_slot; i++) {
+			set.set(_slots[i].param);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // exists in this layer
+			return _slots[index].value;
+		}
+
+		return _parent->get(param);
+	}
+
+	void reset(param_t param) override
+	{
+		const AtomicTransaction transaction;
+		int index = _getIndex(param);
+
+		if (index < _next_slot) {
+			_slots[index] = {UINT16_MAX, param_value_u{}};
+			_sort();
+			_next_slot--;
+		}
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+	}
+
+	int size() const override
+	{
+		return _next_slot;
+	}
+
+	int byteSize() const override
+	{
+		return N_SLOTS * sizeof(Slot);
+	}
+
+private:
+	struct Slot {
+		param_t param;
+		param_value_u value;
+	};
+
+	static int _slotCompare(const void *a, const void *b)
+	{
+		return ((int)((Slot *)a)->param) - ((int)((Slot *)b)->param);
+	}
+
+	void _sort()
+	{
+		qsort(_slots, N_SLOTS, sizeof(Slot), &_slotCompare);
+	}
+
+	int _getIndex(param_t param) const
+	{
+		int left = 0;
+		int right = _next_slot - 1;
+
+		while (left <= right) {
+			int mid = (left + right) / 2;
+
+			if (_slots[mid].param == param) {
+				return mid;
+
+			} else if (_slots[mid].param < param) {
+				left = mid + 1;
+
+			} else {
+				right = mid - 1;
+			}
+		}
+
+		return _next_slot;
+	}
+
+	Slot _slots[N_SLOTS];
+	int _next_slot = 0;
+};

src/lib/parameters/atomic_transaction.cpp

@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "atomic_transaction.h"
+
+#ifdef __PX4_POSIX
+_MutexHolder  AtomicTransaction::_mutex_holder = _MutexHolder {};
+#endif

src/lib/parameters/atomic_transaction.h

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#ifdef __PX4_NUTTX
+#include "px4_platform_common/micro_hal.h"
+#endif
+
+#ifdef __PX4_POSIX
+#include <pthread.h>
+
+class _MutexHolder
+{
+public:
+	pthread_mutex_t _mutex;
+	pthread_mutexattr_t _mutex_attr;
+
+	_MutexHolder()
+	{
+		pthread_mutexattr_init(&_mutex_attr);
+		pthread_mutexattr_settype(&_mutex_attr, PTHREAD_MUTEX_RECURSIVE);
+		pthread_mutex_init(&_mutex, &_mutex_attr);
+	}
+
+	~_MutexHolder()
+	{
+		pthread_mutex_destroy(&_mutex);
+	}
+};
+#endif
+
+
+class AtomicTransaction
+{
+private:
+#ifdef __PX4_NUTTX
+	irqstate_t _irq_state;
+#endif
+
+#ifdef __PX4_POSIX
+	static _MutexHolder _mutex_holder;
+#endif
+
+public:
+	AtomicTransaction()
+	{
+		lock();
+	}
+
+	~AtomicTransaction()
+	{
+		unlock();
+	}
+
+	void lock()
+	{
+#ifdef __PX4_NUTTX
+		_irq_state = px4_enter_critical_section();
+#endif
+#ifdef __PX4_POSIX
+		pthread_mutex_lock(&_mutex_holder._mutex);
+#endif
+	}
+
+	void unlock()
+	{
+#ifdef __PX4_NUTTX
+		px4_leave_critical_section(_irq_state);
+#endif
+#ifdef __PX4_POSIX
+		pthread_mutex_unlock(&_mutex_holder._mutex);
+#endif
+	}
+};

src/lib/parameters/autosave.cpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "autosave.h"
+
+#include <px4_platform_common/log.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/Subscription.hpp>
+
+#include "param.h"
+#include "atomic_transaction.h"
+
+using namespace time_literals;
+
+ParamAutosave::ParamAutosave()
+	: ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+}
+
+void ParamAutosave::request()
+{
+	if (_scheduled.load() || _disabled) {
+		return;
+	}
+
+	// wait at least 300ms before saving, because:
+	// - tasks often call param_set() for multiple params, so this avoids unnecessary save calls
+	// - the logger stores changed params. He gets notified on a param change via uORB and then
+	//   looks at all unsaved params.
+	hrt_abstime delay = 300_ms;
+
+	static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds
+	const hrt_abstime last_save_elapsed = hrt_elapsed_time(&_last_timestamp);
+
+	if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) {
+		delay = rate_limit - last_save_elapsed;
+	}
+
+	_scheduled.store(true);
+	ScheduleDelayed(delay);
+}
+
+void ParamAutosave::enable(bool enable)
+{
+	AtomicTransaction transaction;
+	_disabled = !enable;
+
+	if (!enable && _scheduled.load()) {
+		_scheduled.store(false);
+		px4::ScheduledWorkItem::ScheduleClear();
+	}
+}
+
+void ParamAutosave::Run()
+{
+	bool disabled = false;
+
+	if (!param_get_default_file()) {
+		// In case we save to FLASH, defer param writes until disarmed,
+		// as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7)
+		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
+
+		if (armed_sub.get().armed) {
+			ScheduleDelayed(1_s);
+			return;
+		}
+	}
+
+	{
+		const AtomicTransaction transaction;
+		_last_timestamp = hrt_absolute_time();
+		// Note that the order is important here: we first clear _scheduled, then save the parameters, as during export,
+		// more parameter changes could happen.
+		_scheduled.store(false);
+		disabled = _disabled;
+	}
+
+	if (disabled) {
+		return;
+	}
+
+	PX4_DEBUG("Autosaving params");
+	int ret = param_save_default();
+
+	if (ret != 0) {
+		PX4_ERR("param auto save failed (%i)", ret);
+	}
+}
+

src/lib/parameters/autosave.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/atomic.h>
+#include <drivers/drv_hrt.h>
+
+class ParamAutosave : public px4::ScheduledWorkItem
+{
+public:
+
+	ParamAutosave();
+
+	/**
+	 * Automatically save the parameters after a timeout and at limited rate.
+	 */
+	void request();
+
+	void enable(bool enable);
+
+	bool enabled() const { return !_disabled; }
+
+	hrt_abstime lastAutosave() const { return _last_timestamp; }
+
+	void Run() override;
+
+private:
+	hrt_abstime _last_timestamp{0};
+	px4::atomic_bool _scheduled{false};
+	bool _disabled{false};
+};

src/lib/parameters/flashparams/CMakeLists.txt

@@ -42,7 +42,7 @@ add_library(flashparams
 	flashparams.cpp
 )
 
-add_dependencies(flashparams prebuild_targets)
+add_dependencies(flashparams prebuild_targets parameters_header)
 target_compile_definitions(flashparams PRIVATE -DMODULE_NAME="flashparams")
 target_compile_options(flashparams
 	PRIVATE

src/lib/parameters/flashparams/flashparams.cpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/shutdown.h>
+#include <parameters/px4_parameters.hpp>
 
 #include <string.h>
 #include <stdbool.h>
@@ -54,7 +55,6 @@
 
 #include <parameters/param.h>
 
-#include "../uthash/utarray.h"
 #include <lib/tinybson/tinybson.h>
 #include "flashparams.h"
 #include "flashfs.h"
@@ -78,36 +78,30 @@ struct param_wbuf_s {
 static int
 param_export_internal(param_filter_func filter)
 {
-	struct param_wbuf_s *s = nullptr;
 	bson_encoder_s encoder{};
 	int     result = -1;
 
 	/* Use realloc */
 
 	bson_encoder_init_buf(&encoder, nullptr, 0);
+	auto changed_params = user_config.containedAsBitset();
 
-	/* no modified parameters -> we are done */
-	if (param_values == nullptr) {
-		result = 0;
-		goto out;
-	}
+	for (param_t param = 0; param < user_config.PARAM_COUNT; param++) {
 
-	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
+		if (!changed_params[param] || (filter && !filter(param))) {
+			continue;
+		}
 
 		int32_t i;
 		float   f;
 
-		if (filter && !filter(s->param)) {
-			continue;
-		}
-
 		/* append the appropriate BSON type object */
 
-		switch (param_type(s->param)) {
+		switch (param_type(param)) {
 		case PARAM_TYPE_INT32:
-			i = s->val.i;
+			i = user_config.get(param).i;
 
-			if (bson_encoder_append_int32(&encoder, param_name(s->param), i)) {
+			if (bson_encoder_append_int32(&encoder, param_name(param), i)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}
@@ -115,9 +109,9 @@ param_export_internal(param_filter_func filter)
 			break;
 
 		case PARAM_TYPE_FLOAT:
-			f = s->val.f;
+			f = user_config.get(param).f;
 
-			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+			if (bson_encoder_append_double(&encoder, param_name(param), (double)f)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}

src/lib/parameters/flashparams/flashparams.h

@@ -49,6 +49,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <sys/types.h>
+#include "../DynamicSparseLayer.h"
 
 __BEGIN_DECLS
 
@@ -57,9 +58,9 @@ __BEGIN_DECLS
  * the param_values and 2 functions to be global
  */
 
-__EXPORT extern UT_array        *param_values;
+__EXPORT extern DynamicSparseLayer user_config;
 __EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes);
-__EXPORT const void *param_get_value_ptr_external(param_t param);
+__EXPORT void param_get_external(param_t param, void *val);
 
 /* The interface hooks to the Flash based storage. The caller is responsible for locking */
 __EXPORT int flash_param_save(param_filter_func filter);

src/lib/parameters/uthash/doc/utarray.txt

@@ -1,376 +0,0 @@
-utarray: dynamic array macros for C 
-===================================
-Troy D. Hanson <thanson@users.sourceforge.net>
-v1.9.5, November 2011
-
-include::sflogo.txt[]
-include::topnav_utarray.txt[]
-
-Introduction
-------------
-include::toc.txt[]
-
-A set of general-purpose dynamic array macros for C structures are included with
-uthash in `utarray.h`.  To use these macros in your own C program, just
-copy `utarray.h` into your source directory and use it in your programs.
-
-  #include "utarray.h"
-
-The dynamic array supports basic operations such as push, pop, and erase on the
-array elements. These array elements can be any simple datatype or structure.
-The array <<operations,operations>> are based loosely on the C++ STL vector methods.
-
-Internally the dynamic array contains a contiguous memory region into which
-the elements are copied. This buffer is grown as needed using `realloc` to
-accomodate all the data that is pushed into it.
-
-Download
-~~~~~~~~
-To download the `utarray.h` header file, follow the link on the
-http://uthash.sourceforge.net[uthash home page]. 
-
-BSD licensed
-~~~~~~~~~~~~
-This software is made available under the 
-link:license.html[revised BSD license]. 
-It is free and open source. 
-
-Platforms
-~~~~~~~~~
-The 'utarray' macros have been tested on:
-
- * Linux, 
- * Mac OS X, 
- * Windows, using Visual Studio 2008 and Visual Studio 2010
-
-Usage 
------
-
-Declaration
-~~~~~~~~~~~
-
-The array itself has the data type `UT_array`, regardless of the type of
-elements to be stored in it. It is declared like,
-
-  UT_array *nums;
-
-New and free
-~~~~~~~~~~~~
-The next step is to create the array using `utarray_new`. Later when you're
-done with the array, `utarray_free` will free it and all its elements. 
-
-Push, pop, etc
-~~~~~~~~~~~~~~
-The central features of the utarray involve putting elements into it, taking
-them out, and iterating over them. There are several <<operations,operations>>
-to pick from that deal with either single elements or ranges of elements at a
-time. In the examples below we will use only the push operation to insert
-elements. 
-
-Elements
---------
-
-Support for dynamic arrays of integers or strings is especially easy. These are
-best shown by example:
-
-Integers
-~~~~~~~~
-This example makes a utarray of integers, pushes 0-9 into it, then prints it.
-Lastly it frees it.
-
-.Integer elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-int main() {
-  UT_array *nums;
-  int i, *p;
-
-  utarray_new(nums,&ut_int_icd);  
-  for(i=0; i < 10; i++) utarray_push_back(nums,&i);
-
-  for(p=(int*)utarray_front(nums); 
-      p!=NULL; 
-      p=(int*)utarray_next(nums,p)) {
-    printf("%d\n",*p);
-  }
-
-  utarray_free(nums);
-
-  return 0;
-}
--------------------------------------------------------------------------------
-
-The second argument to `utarray_push_back` is always a 'pointer' to the type
-(so a literal cannot be used). So for integers, it is an `int*`.
-
-Strings
-~~~~~~~
-In this example we make a utarray of strings, push two strings into it, print
-it and free it.
-
-.String elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-int main() {
-  UT_array *strs;
-  char *s, **p;
-
-  utarray_new(strs,&ut_str_icd);
-
-  s = "hello"; utarray_push_back(strs, &s);
-  s = "world"; utarray_push_back(strs, &s);
-  p = NULL;
-  while ( (p=(char**)utarray_next(strs,p))) {
-    printf("%s\n",*p);
-  }
-
-  utarray_free(strs);
-
-  return 0;
-}
--------------------------------------------------------------------------------
-
-In this example, since the element is a `char*`, we pass a pointer to it
-(`char**`) as the second argument to `utarray_push_back`. Note that "push" makes
-a copy of the source string and pushes that copy into the array.
-
-About UT_icd
-~~~~~~~~~~~~
-
-Arrays be made of any type of element, not just integers and strings.  The
-elements can be basic types or structures. Unless you're dealing with integers
-and strings (which use pre-defined `ut_int_icd` and `ut_str_icd`), you'll need
-to define a `UT_icd` helper structure. This structure contains everything that
-utarray needs to initialize, copy or destruct elements.
-
-  typedef struct {
-      size_t sz;
-      init_f *init;
-      ctor_f *copy;
-      dtor_f *dtor;
-  } UT_icd;
-
-The three function pointers `init`, `copy`, and `dtor` have these prototypes:
-
-  typedef void (ctor_f)(void *dst, const void *src);
-  typedef void (dtor_f)(void *elt);
-  typedef void (init_f)(void *elt);
-
-The `sz` is just the size of the element being stored in the array.
-
-The `init` function will be invoked whenever utarray needs to initialize an
-empty element. This only happens as a byproduct of `utarray_resize` or
-`utarray_extend_back`. If `init` is `NULL`, it defaults to zero filling the
-new element using memset.
-
-The `copy` function is used whenever an element is copied into the array.
-It is invoked during `utarray_push_back`, `utarray_insert`, `utarray_inserta`,
-or `utarray_concat`. If `copy` is `NULL`, it defaults to a bitwise copy using
-memcpy.
-
-The `dtor` function is used to clean up an element that is being removed from
-the array. It may be invoked due to `utarray_resize`, `utarray_pop_back`,
-`utarray_erase`, `utarray_clear`, `utarray_done` or `utarray_free`. If the
-elements need no cleanup upon destruction, `dtor` may be `NULL`.
-
-Scalar types
-~~~~~~~~~~~~
-
-The next example uses `UT_icd` with all its defaults to make a utarray of
-`long` elements. This example pushes two longs, prints them, and frees the
-array.
-
-.long elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-UT_icd long_icd = {sizeof(long), NULL, NULL, NULL };
-
-int main() {
-  UT_array *nums;
-  long l, *p;
-  utarray_new(nums, &long_icd);
-
-  l=1; utarray_push_back(nums, &l);
-  l=2; utarray_push_back(nums, &l);
-
-  p=NULL;
-  while( (p=(long*)utarray_next(nums,p))) printf("%ld\n", *p);
-
-  utarray_free(nums);
-  return 0;
-}
--------------------------------------------------------------------------------
-
-Structures
-~~~~~~~~~~
-
-Structures can be used as utarray elements. If the structure requires no
-special effort to initialize, copy or destruct, we can use `UT_icd` with all
-its defaults. This example shows a structure that consists of two integers. Here
-we push two values, print them and free the array.
-
-.Structure (simple)
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-typedef struct {
-    int a;
-    int b;
-} intpair_t;
-
-UT_icd intpair_icd = {sizeof(intpair_t), NULL, NULL, NULL};
-
-int main() {
-
-  UT_array *pairs;
-  intpair_t ip, *p;
-  utarray_new(pairs,&intpair_icd);
-
-  ip.a=1;  ip.b=2;  utarray_push_back(pairs, &ip);
-  ip.a=10; ip.b=20; utarray_push_back(pairs, &ip);
-
-  for(p=(intpair_t*)utarray_front(pairs);
-      p!=NULL;
-      p=(intpair_t*)utarray_next(pairs,p)) {
-    printf("%d %d\n", p->a, p->b);
-  }
-
-  utarray_free(pairs);
-  return 0;
-}
--------------------------------------------------------------------------------
-
-The real utility of `UT_icd` is apparent when the elements of the utarray are
-structures that require special work to initialize, copy or destruct. 
-
-For example, when a structure contains pointers to related memory areas that
-need to be copied when the structure is copied (and freed when the structure is
-freed), we can use custom `init`, `copy`, and `dtor` members in the `UT_icd`.
-
-Here we take an example of a structure that contains an integer and a string.
-When this element is copied (such as when an element is pushed into the array),
-we want to "deep copy" the `s` pointer (so the original element and the new
-element point to their own copies of `s`). When an element is destructed, we
-want to "deep free" its copy of `s`. Lastly, this example is written to work
-even if `s` has the value `NULL`.
-
-.Structure (complex)
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include <stdlib.h>
-#include "utarray.h"
-
-typedef struct {
-    int a;
-    char *s;
-} intchar_t;
-
-void intchar_copy(void *_dst, const void *_src) {
-  intchar_t *dst = (intchar_t*)_dst, *src = (intchar_t*)_src;
-  dst->a = src->a;
-  dst->s = src->s ? strdup(src->s) : NULL;
-}
-
-void intchar_dtor(void *_elt) {
-  intchar_t *elt = (intchar_t*)_elt;
-  if (elt->s) free(elt->s);
-}
-
-UT_icd intchar_icd = {sizeof(intchar_t), NULL, intchar_copy, intchar_dtor};
-
-int main() {
-  UT_array *intchars;
-  intchar_t ic, *p;
-  utarray_new(intchars, &intchar_icd);
-
-  ic.a=1; ic.s="hello"; utarray_push_back(intchars, &ic);
-  ic.a=2; ic.s="world"; utarray_push_back(intchars, &ic);
-
-  p=NULL;
-  while( (p=(intchar_t*)utarray_next(intchars,p))) {
-    printf("%d %s\n", p->a, (p->s ? p->s : "null"));
-  }
-
-  utarray_free(intchars);
-  return 0;
-}
-
--------------------------------------------------------------------------------
-
-[[operations]]
-Reference
----------
-This table lists all the utarray operations. These are loosely based on the C++
-vector class.
-
-Operations
-~~~~~~~~~~
-
-[width="100%",cols="50<m,40<",grid="none",options="none"]
-|===============================================================================
-| utarray_new(UT_array *a, UT_icd *icd)| allocate a new array 
-| utarray_free(UT_array *a)            | free an allocated array
-| utarray_init(UT_array *a,UT_icd *icd)| init an array (non-alloc)
-| utarray_done(UT_array *a)            | dispose of an array (non-allocd)
-| utarray_reserve(UT_array *a,int n)  | ensure space available for 'n' more elements
-| utarray_push_back(UT_array *a,void *p) | push element p onto a
-| utarray_pop_back(UT_array *a)        | pop last element from a
-| utarray_extend_back(UT_array *a)     | push empty element onto a
-| utarray_len(UT_array *a)             | get length of a 
-| utarray_eltptr(UT_array *a,int j)    | get pointer of element from index 
-| utarray_eltidx(UT_array *a,void *e)  | get index of element from pointer
-| utarray_insert(UT_array *a,void *p, int j) | insert element p to index j
-| utarray_inserta(UT_array *a,UT_array *w, int j) | insert array w into array a at index j
-| utarray_resize(UT_array *dst,int num)  | extend or shrink array to num elements
-| utarray_concat(UT_array *dst,UT_array *src) | copy src to end of dst array
-| utarray_erase(UT_array *a,int pos,int len) | remove len elements from a[pos]..a[pos+len-1]
-| utarray_clear(UT_array *a) | clear all elements from a, setting its length to zero
-| utarray_sort(UT_array *a,cmpfcn *cmp) | sort elements of a using comparison function 
-| utarray_find(UT_array *a,void *v, cmpfcn *cmp) | find element v in utarray (must be sorted)
-| utarray_front(UT_array *a) | get first element of a
-| utarray_next(UT_array *a,void *e) | get element of a following e (front if e is NULL)
-| utarray_prev(UT_array *a,void *e) | get element of a before e (back if e is NULL)
-| utarray_back(UT_array *a) | get last element of a
-|===============================================================================
-
-Notes
-~~~~~
-
-1. `utarray_new` and `utarray_free` are used to allocate a new array and free it,
-   while `utarray_init` and `utarray_done` can be used if the UT_array is already
-   allocated and just needs to be initialized or have its internal resources
-   freed.
-2. `utarray_reserve` takes the "delta" of elements to reserve (not the total 
-   desired capacity of the array-- this differs from the C++ STL "reserve" notion)
-3. `utarray_sort` expects a comparison function having the usual `strcmp` -like
-   convention where it accepts two elements (a and b) and returns a negative 
-   value if a precedes b, 0 if a and b sort equally, and positive if b precedes a.
-   This is an example of a comparison function:
-
-  int intsort(const void *a,const void*b) {
-      int _a = *(int*)a;
-      int _b = *(int*)b;
-      return _a - _b;
-  }
-
-4. `utarray_find` uses a binary search to locate an element having a certain value
-   according to the given comparison function. The utarray must be first sorted
-   using the same comparison function. An example of using `utarray_find` with
-   a utarray of strings is included in `tests/test61.c`.
-
-5. A 'pointer' to a particular element (obtained using `utarray_eltptr` or
-   `utarray_front`, `utarray_next`, `utarray_prev`, `utarray_back`) becomes invalid whenever
-   another element is inserted into the utarray. This is because the internal
-   memory management may need to `realloc` the element storage to a new address.
-   For this reason, it's usually better to refer to an element by its integer
-   'index' in code whose duration may include element insertion.
-
-// vim: set nowrap syntax=asciidoc: 
-

src/lib/parameters/uthash/utarray.h

@@ -1,234 +0,0 @@
-/*
-Copyright (c) 2008-2012, Troy D. Hanson   http://uthash.sourceforge.net
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
-IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
-TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
-PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
-OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
-EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
-PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
-PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
-LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
-NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-/* a dynamic array implementation using macros 
- * see http://uthash.sourceforge.net/utarray
- */
-#ifndef UTARRAY_H
-#define UTARRAY_H
-
-#define UTARRAY_VERSION 1.9.6
-
-#ifdef __GNUC__
-#define _UNUSED_ __attribute__ ((__unused__)) 
-#else
-#define _UNUSED_ 
-#endif
-
-#include <stddef.h>  /* size_t */
-#include <string.h>  /* memset, etc */
-#include <stdlib.h>  /* exit */
-
-// FIXME: this needs to be checked: we need to handle OOM properly instead of just exiting
-#define oom() system_exit(-1)
-
-typedef void (ctor_f)(void *dst, const void *src);
-typedef void (dtor_f)(void *elt);
-typedef void (init_f)(void *elt);
-typedef struct {
-    size_t sz;
-    init_f *init;
-    ctor_f *copy;
-    dtor_f *dtor;
-} UT_icd;
-
-typedef struct {
-    unsigned i,n;/* i: index of next available slot, n: num slots */
-    UT_icd icd;  /* initializer, copy and destructor functions */
-    char *d;     /* n slots of size icd->sz*/
-} UT_array;
-
-#define utarray_init(a,_icd) do {                                             \
-  memset(a,0,sizeof(UT_array));                                               \
-  (a)->icd=*_icd;                                                             \
-} while(0)
-
-#define utarray_done(a) do {                                                  \
-  if ((a)->n) {                                                               \
-    if ((a)->icd.dtor) {                                                      \
-      size_t _ut_i;                                                           \
-      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
-        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
-      }                                                                       \
-    }                                                                         \
-    free((a)->d);                                                             \
-  }                                                                           \
-  (a)->n=0;                                                                   \
-} while(0)
-
-#define utarray_new(a,_icd) do {                                              \
-  a=(UT_array*)malloc(sizeof(UT_array));                                      \
-  utarray_init(a,_icd);                                                       \
-} while(0)
-
-#define utarray_free(a) do {                                                  \
-  utarray_done(a);                                                            \
-  free(a);                                                                    \
-} while(0)
-
-#define utarray_reserve(a,by) do {                                            \
-  if (((a)->i+by) > ((a)->n)) {                                               \
-    while(((a)->i+by) > ((a)->n)) { (a)->n = ((a)->n ? (2*(a)->n) : 8); }     \
-    if ( ((a)->d=(char*)realloc((a)->d, (a)->n*(a)->icd.sz)) == NULL) oom();  \
-  }                                                                           \
-} while(0)
-
-#define utarray_push_back(a,p) do {                                           \
-  utarray_reserve(a,1);                                                       \
-  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,(a)->i++), p); }      \
-  else { memcpy(_utarray_eltptr(a,(a)->i++), p, (a)->icd.sz); };              \
-} while(0)
-
-#define utarray_pop_back(a) do {                                              \
-  if ((a)->icd.dtor) { (a)->icd.dtor( _utarray_eltptr(a,--((a)->i))); }       \
-  else { (a)->i--; }                                                          \
-} while(0)
-
-#define utarray_extend_back(a) do {                                           \
-  utarray_reserve(a,1);                                                       \
-  if ((a)->icd.init) { (a)->icd.init(_utarray_eltptr(a,(a)->i)); }            \
-  else { memset(_utarray_eltptr(a,(a)->i),0,(a)->icd.sz); }                   \
-  (a)->i++;                                                                   \
-} while(0)
-
-#define utarray_len(a) ((a)->i)
-
-#define utarray_eltptr(a,j) (((j) < (a)->i) ? _utarray_eltptr(a,j) : NULL)
-#define _utarray_eltptr(a,j) ((char*)((a)->d + ((a)->icd.sz*(j) )))
-
-#define utarray_insert(a,p,j) do {                                            \
-  utarray_reserve(a,1);                                                       \
-  if (j > (a)->i) break;                                                      \
-  if ((j) < (a)->i) {                                                         \
-    memmove( _utarray_eltptr(a,(j)+1), _utarray_eltptr(a,j),                  \
-             ((a)->i - (j))*((a)->icd.sz));                                   \
-  }                                                                           \
-  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,j), p); }             \
-  else { memcpy(_utarray_eltptr(a,j), p, (a)->icd.sz); };                     \
-  (a)->i++;                                                                   \
-} while(0)
-
-#define utarray_inserta(a,w,j) do {                                           \
-  if (utarray_len(w) == 0) break;                                             \
-  if (j > (a)->i) break;                                                      \
-  utarray_reserve(a,utarray_len(w));                                          \
-  if ((j) < (a)->i) {                                                         \
-    memmove(_utarray_eltptr(a,(j)+utarray_len(w)),                            \
-            _utarray_eltptr(a,j),                                             \
-            ((a)->i - (j))*((a)->icd.sz));                                    \
-  }                                                                           \
-  if ((a)->icd.copy) {                                                        \
-    size_t _ut_i;                                                             \
-    for(_ut_i=0;_ut_i<(w)->i;_ut_i++) {                                       \
-      (a)->icd.copy(_utarray_eltptr(a,j+_ut_i), _utarray_eltptr(w,_ut_i));    \
-    }                                                                         \
-  } else {                                                                    \
-    memcpy(_utarray_eltptr(a,j), _utarray_eltptr(w,0),                        \
-           utarray_len(w)*((a)->icd.sz));                                     \
-  }                                                                           \
-  (a)->i += utarray_len(w);                                                   \
-} while(0)
-
-#define utarray_resize(dst,num) do {                                          \
-  size_t _ut_i;                                                               \
-  if (dst->i > (size_t)(num)) {                                               \
-    if ((dst)->icd.dtor) {                                                    \
-      for(_ut_i=num; _ut_i < dst->i; _ut_i++) {                               \
-        (dst)->icd.dtor(utarray_eltptr(dst,_ut_i));                           \
-      }                                                                       \
-    }                                                                         \
-  } else if (dst->i < (size_t)(num)) {                                        \
-    utarray_reserve(dst,num-dst->i);                                          \
-    if ((dst)->icd.init) {                                                    \
-      for(_ut_i=dst->i; _ut_i < num; _ut_i++) {                               \
-        (dst)->icd.init(utarray_eltptr(dst,_ut_i));                           \
-      }                                                                       \
-    } else {                                                                  \
-      memset(_utarray_eltptr(dst,dst->i),0,(dst)->icd.sz*(num-dst->i));       \
-    }                                                                         \
-  }                                                                           \
-  dst->i = num;                                                               \
-} while(0)
-
-#define utarray_concat(dst,src) do {                                          \
-  utarray_inserta((dst),(src),utarray_len(dst));                              \
-} while(0)
-
-#define utarray_erase(a,pos,len) do {                                         \
-  if ((a)->icd.dtor) {                                                        \
-    size_t _ut_i;                                                             \
-    for(_ut_i=0; _ut_i < len; _ut_i++) {                                      \
-      (a)->icd.dtor(utarray_eltptr((a),pos+_ut_i));                           \
-    }                                                                         \
-  }                                                                           \
-  if ((a)->i > (pos+len)) {                                                   \
-    memmove( _utarray_eltptr((a),pos), _utarray_eltptr((a),pos+len),          \
-            (((a)->i)-(pos+len))*((a)->icd.sz));                              \
-  }                                                                           \
-  (a)->i -= (len);                                                            \
-} while(0)
-
-#define utarray_renew(a,u) do {                                               \
-  if (a) utarray_clear(a); \
-  else utarray_new((a),(u));   \
-} while(0) 
-
-#define utarray_clear(a) do {                                                 \
-  if ((a)->i > 0) {                                                           \
-    if ((a)->icd.dtor) {                                                      \
-      size_t _ut_i;                                                           \
-      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
-        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
-      }                                                                       \
-    }                                                                         \
-    (a)->i = 0;                                                               \
-  }                                                                           \
-} while(0)
-
-#define utarray_sort(a,cmp) do {                                              \
-  qsort((a)->d, (a)->i, (a)->icd.sz, cmp);                                    \
-} while(0)
-
-#define utarray_find(a,v,cmp) bsearch((v),(a)->d,(a)->i,(a)->icd.sz,cmp)
-
-#define utarray_front(a) (((a)->i) ? (_utarray_eltptr(a,0)) : NULL)
-#define utarray_next(a,e) (((e)==NULL) ? utarray_front(a) : ((((a)->i) > (utarray_eltidx(a,e)+1)) ? _utarray_eltptr(a,utarray_eltidx(a,e)+1) : NULL))
-#define utarray_prev(a,e) (((e)==NULL) ? utarray_back(a) : ((utarray_eltidx(a,e) > 0) ? _utarray_eltptr(a,utarray_eltidx(a,e)-1) : NULL))
-#define utarray_back(a) (((a)->i) ? (_utarray_eltptr(a,(a)->i-1)) : NULL)
-#define utarray_eltidx(a,e) (((char*)(e) >= (char*)((a)->d)) ? (((char*)(e) - (char*)((a)->d))/(a)->icd.sz) : -1)
-
-/* last we pre-define a few icd for common utarrays of ints and strings */
-static void utarray_str_cpy(void *dst, const void *src) {
-  char **_src = (char**)src, **_dst = (char**)dst;
-  *_dst = (*_src == NULL) ? NULL : strdup(*_src);
-}
-static void utarray_str_dtor(void *elt) {
-  char **eltc = (char**)elt;
-  if (*eltc) free(*eltc);
-}
-static const UT_icd ut_str_icd _UNUSED_ = {sizeof(char*),NULL,utarray_str_cpy,utarray_str_dtor};
-static const UT_icd ut_int_icd _UNUSED_ = {sizeof(int),NULL,NULL,NULL};
-static const UT_icd ut_ptr_icd _UNUSED_ = {sizeof(void*),NULL,NULL,NULL};
-
-
-#endif /* UTARRAY_H */

src/lib/rate_control/rate_control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -40,7 +40,7 @@
 
 using namespace matrix;
 
-void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+void RateControl::setPidGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
 {
 	_gain_p = P;
 	_gain_i = I;

src/lib/rate_control/rate_control.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -51,12 +51,12 @@ public:
 	~RateControl() = default;
 
 	/**
-	 * Set the rate control gains
+	 * Set the rate control PID gains
 	 * @param P 3D vector of proportional gains for body x,y,z axis
 	 * @param I 3D vector of integral gains
 	 * @param D 3D vector of derivative gains
 	 */
-	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+	void setPidGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
 
 	/**
 	 * Set the mximum absolute value of the integrator for all axes
@@ -94,6 +94,18 @@ public:
 	 */
 	void resetIntegral() { _rate_int.zero(); }
 
+	/**
+	 * Set the integral term to 0 for specific axes
+	 * @param  axis roll 0 / pitch 1 / yaw 2
+	 * @see _rate_int
+	 */
+	void resetIntegral(size_t axis)
+	{
+		if (axis < 3) {
+			_rate_int(axis) = 0.f;
+		}
+	}
+
 	/**
 	 * Get status message of controller for logging/debugging
 	 * @param rate_ctrl_status status message to fill with internal states

src/lib/sensor_calibration/Utilities.cpp

@@ -64,7 +64,7 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id)
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
 		char str[20] {};
-		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 
 		int32_t device_id_val = 0;
 
@@ -103,7 +103,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
 		char str[20] {};
-		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 		int32_t device_id_val = 0;
 
 		if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
@@ -138,7 +138,7 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
 {
 	// eg CAL_MAGn_ID/CAL_MAGn_ROT
 	char str[20] {};
-	sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	int32_t value = 0;
 
@@ -153,7 +153,7 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
 {
 	// eg CAL_BAROn_OFF
 	char str[20] {};
-	sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	float value = NAN;
 
@@ -174,7 +174,7 @@ Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_t
 		char axis_char = 'X' + axis;
 
 		// eg CAL_MAGn_{X,Y,Z}OFF
-		sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
+		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
 
 		if (param_get(param_find(str), &values(axis)) != 0) {
 			PX4_ERR("failed to get %s", str);
@@ -193,7 +193,7 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type,
 		char axis_char = 'X' + axis;
 
 		// eg CAL_MAGn_{X,Y,Z}OFF
-		sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
+		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
 
 		if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
 			PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));

src/lib/sensor_calibration/Utilities.hpp

@@ -88,7 +88,7 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t
 	char str[20] {};
 
 	// eg CAL_MAGn_ID/CAL_MAGn_ROT
-	sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type);
 
 	int ret = param_set_no_notification(param_find(str), &value);
 

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -61,6 +61,7 @@ px4_add_library(health_and_arming_checks
 	checks/rcAndDataLinkCheck.cpp
 	checks/vtolCheck.cpp
 	checks/offboardCheck.cpp
+	checks/openDroneIDCheck.cpp
 )
 add_dependencies(health_and_arming_checks mode_util)
 

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -67,6 +67,7 @@
 #include "checks/rcAndDataLinkCheck.hpp"
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"
+#include "checks/openDroneIDCheck.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -126,6 +127,7 @@ private:
 	ManualControlChecks _manual_control_checks;
 	HomePositionChecks _home_position_checks;
 	ModeChecks _mode_checks;
+	OpenDroneIDChecks _open_drone_id_checks;
 	ParachuteChecks _parachute_checks;
 	PowerChecks _power_checks;
 	RcCalibrationChecks _rc_calibration_checks;
@@ -140,7 +142,7 @@ private:
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
 
-	HealthAndArmingCheckBase *_checks[30] = {
+	HealthAndArmingCheckBase *_checks[31] = {
 		&_accelerometer_checks,
 		&_airspeed_checks,
 		&_baro_checks,
@@ -157,6 +159,7 @@ private:
 		&_mission_checks,
 		&_offboard_checks, // must be after _estimator_checks
 		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
+		&_open_drone_id_checks,
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "openDroneIDCheck.hpp"
+
+
+void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	// Check to see if the check has been disabled
+	if (!_param_com_arm_odid.get()) {
+		return;
+	}
+
+	NavModes affected_modes{NavModes::None};
+
+	if (_param_com_arm_odid.get() == 2) {
+		// disallow arming without the Open Drone ID system
+		affected_modes = NavModes::All;
+	}
+
+	if (!context.status().open_drone_id_system_present) {
+		/* EVENT
+		 * @description
+		 * Open Drone ID system failed to report. Make sure it is setup and installed properly.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+					    events::ID("check_open_drone_id_missing"),
+					    events::Log::Error, "Open Drone ID system missing");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
+		}
+
+	} else if (!context.status().open_drone_id_system_healthy) {
+		/* EVENT
+		 * @description
+		 * Open Drone ID system reported being unhealthy.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+					    events::ID("check_open_drone_id_unhealthy"),
+					    events::Log::Error, "Open Drone ID system not ready");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system not ready");
+		}
+
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+class OpenDroneIDChecks : public HealthAndArmingCheckBase
+{
+public:
+	OpenDroneIDChecks() = default;
+	~OpenDroneIDChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_ARM_ODID>) _param_com_arm_odid
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp

@@ -53,16 +53,16 @@ RcCalibrationChecks::RcCalibrationChecks()
 	char nbuf[20];
 
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		sprintf(nbuf, "RC%d_MIN", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MIN", i + 1);
 		_param_handles[i].min = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_TRIM", i + 1);
 		_param_handles[i].trim = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_MAX", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MAX", i + 1);
 		_param_handles[i].max = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_DZ", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_DZ", i + 1);
 		_param_handles[i].dz = param_find(nbuf);
 	}
 

src/modules/commander/commander_params.c

@@ -50,8 +50,8 @@
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -66,8 +66,8 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -82,8 +82,8 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -1003,6 +1003,20 @@ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
 
+/**
+ * Enable Drone ID system detection and health check
+ *
+ * This check detects if the Open Drone ID system is missing.
+ * Depending on the value of the parameter, the check can be
+ * disabled, warn only or deny arming.
+ *
+ * @group Commander
+ * @value 0 Disabled
+ * @value 1 Warning only
+ * @value 2 Enforce Open Drone ID system presence
+ */
+PARAM_DEFINE_INT32(COM_ARM_ODID, 0);
+
 /**
  * Enforced delay between arming and further navigation
  *

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -167,6 +167,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
 			if (_control_status.flags.synthetic_mag_z) {
+				fused[2] = true;
 				continue;
 			}
 
@@ -216,6 +217,11 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	if (fused[0] && fused[1] && fused[2]) {
 		aid_src_mag.fused = true;
 		aid_src_mag.time_last_fuse = _time_delayed_us;
+
+		if (update_all_states) {
+			_time_last_heading_fuse = _time_delayed_us;
+		}
+
 		return true;
 	}
 

src/modules/fw_att_control/fw_att_control_params.c

@@ -40,11 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-
 /**
  * Attitude Roll Time Constant
  *
@@ -86,7 +81,7 @@ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -98,7 +93,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -110,7 +105,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -122,7 +117,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -140,7 +135,6 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f);
  */
 PARAM_DEFINE_INT32(FW_W_EN, 0);
 
-
 /**
  * Wheel steering rate proportional gain
  *
@@ -149,7 +143,7 @@ PARAM_DEFINE_INT32(FW_W_EN, 0);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Attitude Control
@@ -164,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.5
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Attitude Control
@@ -207,7 +201,7 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 2
  * @increment 0.05
  * @group FW Attitude Control

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -707,6 +707,22 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 		return; // do not publish the setpoint
 	}
 
+	if (_control_mode.flag_control_auto_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
+		if (_control_mode.flag_control_position_enabled) {
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION;
+
+		} else {
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED;
+		}
+
+		return;
+	}
+
+	if (_control_mode.flag_control_manual_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
+		_control_mode_current = FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION;
+		return;
+	}
+
 	FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current;
 
 	_skipping_takeoff_detection = false;
@@ -716,40 +732,24 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
-			if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
-				_control_mode_current = FW_POSCTRL_MODE_AUTO;
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
 
-				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
-				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-
-			} else {
-				_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
-
-				if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
-					// skip takeoff detection when switching from any other mode, auto or manual,
-					// while already in air.
-					// TODO: find a better place for / way of doing this
-					_skipping_takeoff_detection = true;
-				}
+			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
+				// skip takeoff detection when switching from any other mode, auto or manual,
+				// while already in air.
+				// TODO: find a better place for / way of doing this
+				_skipping_takeoff_detection = true;
 			}
 
 		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-
-			if (!_vehicle_status.in_transition_mode) {
-
-				// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
-				// Straight landings are currently only possible in Missions, and there the previous WP
-				// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
-				if (_position_setpoint_previous_valid) {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
-
-				} else {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
-				}
+			// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
+			// Straight landings are currently only possible in Missions, and there the previous WP
+			// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
+			if (_position_setpoint_previous_valid) {
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
 
 			} else {
-				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
-				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
 			}
 
 		} else {
@@ -769,8 +769,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 			_time_in_fixed_bank_loiter = now;
 		}
 
-		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
-		    && !_vehicle_status.in_transition_mode) {
+		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)) {
 			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
 				// Need to init because last loop iteration was in a different mode
 				events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
@@ -833,36 +832,24 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 }
 
 void
-FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
+FixedwingPositionControl::set_position_setpoint_for_vtol_transition(const position_setpoint_s &current_sp)
 {
 	// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
 	// shifting hacks
 
-	if (_vehicle_status.in_transition_to_fw) {
+	if (!PX4_ISFINITE(_transition_waypoint(0))) {
+		double lat_transition, lon_transition;
 
-		if (!PX4_ISFINITE(_transition_waypoint(0))) {
-			double lat_transition, lon_transition;
+		// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
+		// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
+		// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
+		const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
+		waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
+						   &lat_transition,
+						   &lon_transition);
 
-			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
-			// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
-			// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
-			const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
-			waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
-							   &lat_transition,
-							   &lon_transition);
-
-			_transition_waypoint(0) = lat_transition;
-			_transition_waypoint(1) = lon_transition;
-		}
-
-
-		current_sp.lat = _transition_waypoint(0);
-		current_sp.lon = _transition_waypoint(1);
-
-	} else {
-		/* reset transition waypoint, will be set upon entering front transition */
-		_transition_waypoint(0) = static_cast<double>(NAN);
-		_transition_waypoint(1) = static_cast<double>(NAN);
+		_transition_waypoint(0) = lat_transition;
+		_transition_waypoint(1) = lon_transition;
 	}
 }
 
@@ -871,16 +858,13 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
 				       const position_setpoint_s &pos_sp_next)
 {
-	position_setpoint_s current_sp = pos_sp_curr;
-	move_position_setpoint_for_vtol_transition(current_sp);
-
-	const uint8_t position_sp_type = handle_setpoint_type(current_sp);
+	const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr);
 
 	_position_sp_type = position_sp_type;
 
 	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
-	    || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-		publishOrbitStatus(current_sp);
+	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+		publishOrbitStatus(pos_sp_curr);
 	}
 
 	switch (position_sp_type) {
@@ -891,15 +875,15 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
+		control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_curr);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr, pos_sp_next);
 		break;
 	}
 
@@ -917,10 +901,60 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 	_att_sp.pitch_body = get_tecs_pitch();
 
 	if (!_vehicle_status.in_transition_to_fw) {
-		publishLocalPositionSetpoint(current_sp);
+		publishLocalPositionSetpoint(pos_sp_curr);
 	}
 }
 
+void FixedwingPositionControl::control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed,
+		const position_setpoint_s &pos_sp_curr)
+{
+	set_position_setpoint_for_vtol_transition(pos_sp_curr);
+
+	Vector2d curr_wp = Vector2d(_transition_waypoint(0), _transition_waypoint(1));
+	Vector2d prev_wp{curr_pos};
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
+
+	_npfg.setAirspeedNom(_airspeed); // We stop the motor, so use the current airspeed as estimation.
+	_npfg.setAirspeedMax(_airspeed);
+
+	navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+
+	_att_sp.roll_body = _npfg.getRollSetpoint();
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
+
+	_att_sp.thrust_body[0] = 0.0f;
+
+
+}
+
+void FixedwingPositionControl::control_auto_transition_fixed()
+{
+	_att_sp.roll_body = radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
+
+	_att_sp.thrust_body[0] = 0.0f;
+}
+
+void FixedwingPositionControl::control_manual_transition()
+{
+	_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	_att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle
+
+	_att_sp.thrust_body[0] = 0.0f;
+}
+
 void
 FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
 {
@@ -2092,23 +2126,14 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 float
 FixedwingPositionControl::get_tecs_pitch()
 {
-	if (_tecs_is_running) {
-		return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
-	}
+	return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
 
-	// return level flight pitch offset to prevent stale tecs state when it's not running
-	return radians(_param_fw_psp_off.get());
 }
 
 float
 FixedwingPositionControl::get_tecs_thrust()
 {
-	if (_tecs_is_running) {
-		return min(_tecs.get_throttle_setpoint(), 1.f);
-	}
-
-	// return 0 to prevent stale tecs state when it's not running
-	return 0.0f;
+	return min(_tecs.get_throttle_setpoint(), 1.f);
 }
 
 void
@@ -2301,6 +2326,12 @@ FixedwingPositionControl::Run()
 		_flaps_setpoint = 0.f;
 		_spoilers_setpoint = 0.f;
 
+		/* reset transition waypoint, will be set upon entering transition */
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION) {
+			_transition_waypoint(0) = static_cast<double>(NAN);
+			_transition_waypoint(1) = static_cast<double>(NAN);
+		}
+
 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw_wheel = false;
 
@@ -2352,6 +2383,16 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED: {
+				control_auto_transition_fixed();
+				break;
+			}
+
+		case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION: {
+				control_auto_transition_position(curr_pos, ground_speed, _pos_sp_triplet.current);
+				break;
+			}
+
 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
 				control_manual_position(control_interval, curr_pos, ground_speed);
 				break;
@@ -2362,6 +2403,11 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION: {
+				control_manual_transition();
+				break;
+			}
+
 		case FW_POSCTRL_MODE_OTHER: {
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
 
@@ -2508,15 +2554,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		const float desired_max_sinkrate, const float desired_max_climbrate,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	_tecs_is_running = true;
 
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
 	// (it should also not run during VTOL blending because airspeed is too low still)
 	if (_vehicle_status.is_vtol) {
-		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
-			_tecs_is_running = false;
-		}
-
 		if (_vehicle_status.in_transition_mode) {
 			// we're in transition
 			_was_in_transition = true;
@@ -2547,12 +2588,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		}
 	}
 
-	if (!_tecs_is_running) {
-		// next time we run TECS we should reinitialize states
-		_reinitialize_tecs = true;
-		return;
-	}
-
 	// We need an altitude lock to calculate the TECS control
 	if (_local_pos.timestamp == 0) {
 		_reinitialize_tecs = true;
@@ -2847,31 +2882,36 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, con
 	// BUT no arbitrary max approach angle, approach entirely determined by generated
 	// bearing vectors
 
-	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
-	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
+	const Vector2f vector_A_to_B = waypoint_B - waypoint_A;
+	const Vector2f vector_B_to_A = -vector_A_to_B;
+	const Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
+	const Vector2f vector_B_to_vehicle = vehicle_pos - waypoint_B;
+	Vector2f desired_path = vector_A_to_B; // this is the default path tangent, but is overridden below
 
 	if (vector_A_to_B.norm() < FLT_EPSILON) {
 		// the waypoints are on top of each other and should be considered as a
 		// single waypoint, fly directly to it
 		if (vector_A_to_vehicle.norm() > FLT_EPSILON) {
-			vector_A_to_B = -vector_A_to_vehicle;
+			desired_path = -vector_A_to_vehicle;
 
 		} else {
 			// Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output.
 			return;
 		}
 
-
 	} else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) {
 		// we are in front of waypoint A, fly directly to it until we are within switch distance.
-
 		if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) {
-			vector_A_to_B = -vector_A_to_vehicle;
+			desired_path = -vector_A_to_vehicle;
 		}
+
+	} else if (vector_B_to_A.dot(vector_B_to_vehicle) < -FLT_EPSILON) {
+		// we are behind waypoint B, fly directly to it
+		desired_path = -vector_B_to_vehicle;
 	}
 
 	// track the line segment
-	Vector2f unit_path_tangent{vector_A_to_B.normalized()};
+	Vector2f unit_path_tangent{desired_path.normalized()};
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
 	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -242,8 +242,11 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
+		FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION,
+		FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
+		FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
@@ -390,7 +393,6 @@ private:
 	TECS _tecs;
 
 	bool _reinitialize_tecs{true};
-	bool _tecs_is_running{false};
 	hrt_abstime _time_last_tecs_update{0}; // [us]
 
 	// VTOL / TRANSITION
@@ -511,12 +513,12 @@ private:
 	void update_in_air_states(const hrt_abstime now);
 
 	/**
-	 * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in  a VTOL
+	 * @brief Set the current position setpoint to a value far ahead of the current vehicle yaw when in  a VTOL
 	 * transition.
 	 *
 	 * @param[in,out] current_sp Current position setpoint
 	 */
-	void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
+	void set_position_setpoint_for_vtol_transition(const position_setpoint_s &current_sp);
 
 	/**
 	 * @brief Changes the position setpoint type to achieve the desired behavior in some instances.
@@ -541,6 +543,26 @@ private:
 	void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 			  const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
+	/**
+	 * @brief Automatic position control for VTOL transition
+	 *
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed,
+					      const position_setpoint_s &pos_sp_curr);
+
+	/**
+	 * @brief Automatic fixed bank control for VTOL transition
+	 */
+	void control_auto_transition_fixed();
+
+	/**
+	 * @brief Manual control for for VTOL transition
+	 */
+	void control_manual_transition();
+
 	/**
 	 * @brief Controls altitude and airspeed for a fixed-bank loiter.
 	 *

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f);
  * @increment 0.05
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.1f);
+PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.05f);
 
 /**
  * Integrator gain pitch

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -77,7 +77,7 @@ FixedwingRateControl::parameters_update()
 	const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
 	const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get());
 
-	_rate_control.setGains(rate_p, rate_i, rate_d);
+	_rate_control.setPidGains(rate_p, rate_i, rate_d);
 
 	_rate_control.setIntegratorLimit(
 		Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
@@ -335,27 +335,36 @@ void FixedwingRateControl::Run()
 					body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll);
 				}
 
-				/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+				// Run attitude RATE controllers which need the desired attitudes from above, add trim.
 				const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
 						_landed);
 
-				float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
-				float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
-				_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
-
-				float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
-				float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
-				_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
-
+				const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
+				const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
 				const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
-				float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
 
-				_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw;
+				const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
+				const float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
+
+				// Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw
+				float yaw_u = 0.f;
+
+				if (_vcontrol_mode.flag_control_attitude_enabled || _param_fw_acro_yaw_en.get()) {
+					yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
+
+				} else {
+					yaw_u = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get();
+					_rate_control.resetIntegral(2);
+				}
 
 				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
 					_rate_control.resetIntegral();
 				}
 
+				_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
+				_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
+				_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw;
+
 				/* throttle passed through if it is finite */
 				_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
 

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -148,6 +148,7 @@ private:
 		(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
 		(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
 		(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
+		(ParamInt<px4::params::FW_ACRO_YAW_EN>) _param_fw_acro_yaw_en,
 
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -34,17 +34,12 @@
 /**
  * @file fw_rate_control_params.c
  *
- * Parameters defined by the fixed-wing attitude control task
+ * Parameters defined by the fixed-wing rate control task
  *
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-
 /**
  * Minimum Airspeed (CAS)
  *
@@ -58,7 +53,6 @@
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -72,7 +66,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -87,7 +80,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
  *
  * @value 0 Use airspeed in controller
  * @value 1 Do not use airspeed in controller
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
 
@@ -99,7 +92,6 @@ PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -115,7 +107,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -127,7 +118,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -141,7 +132,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -156,7 +147,7 @@ PARAM_DEFINE_FLOAT(FW_PR_D, 0.f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.5
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -182,7 +173,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -197,7 +188,7 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -212,9 +203,9 @@ PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.2
- * @decimal 3
- * @increment 0.005
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
  * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f);
@@ -237,7 +228,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -252,7 +243,7 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -267,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 50.0
+ * @max 10
  * @decimal 1
  * @increment 0.5
  * @group FW Rate Control
@@ -338,7 +329,7 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
  * This is the rate the controller is trying to achieve if the user applies full roll
  * stick input in acro mode.
  *
- * @min 45
+ * @min 10
  * @max 720
  * @unit deg
  * @group FW Rate Control
@@ -348,7 +339,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
 /**
  * Acro body pitch max rate setpoint.
  *
- * @min 45
+ * @min 10
  * @max 720
  * @unit deg
  * @group FW Rate Control
@@ -359,7 +350,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
  * Acro body yaw max rate setpoint.
  *
  * @min 10
- * @max 180
+ * @max 720
  * @unit deg
  * @group FW Rate Control
  */
@@ -397,8 +388,8 @@ PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
 * This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -410,8 +401,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f);
 * This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -423,8 +414,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f);
 * This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -436,8 +427,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f);
 * This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -449,8 +440,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f);
 * This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -462,8 +453,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
 * This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -480,7 +471,7 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
 
@@ -494,7 +485,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
  * @min 0.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
 
@@ -508,7 +499,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
  * @min 0.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
 
@@ -522,7 +513,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
  * @min 0.0
  * @decimal 1
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
 
@@ -534,6 +525,19 @@ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
  * @value 0 Disabled
  * @value 1 Flaps channel
  * @value 2 Aux1
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);
+
+/**
+ * Enable yaw rate controller in Acro
+ *
+ * If this parameter is set to 1, the yaw rate controller is enabled in Fixed-wing Acro mode.
+ * Otherwise the pilot commands directly the yaw actuator.
+ * It is disabled by default because an active yaw rate controller will fight against the
+ * natural turn coordination of the plane.
+ *
+ * @boolean
+ * @group FW Rate Control
+ */
+PARAM_DEFINE_INT32(FW_ACRO_YAW_EN, 0);

src/modules/manual_control/ManualControlSelector.cpp

@@ -83,7 +83,7 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
 					  (input.data_source == _first_valid_source
 					   || _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
 
-	return sample_from_the_past && sample_newer_than_timeout
+	return sample_from_the_past && sample_newer_than_timeout && input.valid
 	       && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
 }
 

src/modules/manual_control/ManualControlSelectorTest.cpp

@@ -37,7 +37,43 @@
 
 using namespace time_literals;
 
-static constexpr uint64_t some_time = 12345678;
+static constexpr uint64_t SOME_TIME = 12345678;
+static constexpr uint8_t SOURCE_RC = manual_control_setpoint_s::SOURCE_RC;
+static constexpr uint8_t SOURCE_MAVLINK_0 = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+static constexpr uint8_t SOURCE_MAVLINK_3 = manual_control_setpoint_s::SOURCE_MAVLINK_3;
+static constexpr uint8_t SOURCE_MAVLINK_4 = manual_control_setpoint_s::SOURCE_MAVLINK_4;
+
+TEST(ManualControlSelector, RcInputInvalidValid)
+{
+	ManualControlSelector selector;
+	selector.setRcInMode(0);
+	selector.setTimeout(500_ms);
+
+	uint64_t timestamp = SOME_TIME;
+
+	// Now provide input with the correct source flagged invalid
+	manual_control_setpoint_s input {};
+	input.data_source = SOURCE_RC;
+	input.valid = false;
+	input.timestamp_sample = timestamp;
+
+	for (int i = 0; i < 2; i++) {
+		selector.updateWithNewInputSample(timestamp, input, 1);
+		EXPECT_FALSE(selector.setpoint().valid);
+		EXPECT_EQ(selector.setpoint().timestamp_sample, 0);
+		EXPECT_EQ(selector.instance(), -1);
+		EXPECT_EQ(selector.setpoint().data_source, 0);
+		timestamp += 100_ms;
+		input.timestamp_sample = timestamp;
+	}
+
+	input.valid = true;
+	selector.updateWithNewInputSample(timestamp, input, 1);
+	EXPECT_TRUE(selector.setpoint().valid);
+	EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp);
+	EXPECT_EQ(selector.instance(), 1);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
+}
 
 TEST(ManualControlSelector, RcInputContinuous)
 {
@@ -45,11 +81,12 @@ TEST(ManualControlSelector, RcInputContinuous)
 	selector.setRcInMode(0);
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	// Now provide input with the correct source.
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 
 	for (int i = 0; i < 5; i++) {
@@ -57,7 +94,7 @@ TEST(ManualControlSelector, RcInputContinuous)
 		EXPECT_TRUE(selector.setpoint().valid);
 		EXPECT_EQ(selector.setpoint().timestamp_sample, timestamp);
 		EXPECT_EQ(selector.instance(), 1);
-		EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+		EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 		timestamp += 100_ms;
 		input.timestamp_sample = timestamp;
 	}
@@ -66,13 +103,14 @@ TEST(ManualControlSelector, RcInputContinuous)
 TEST(ManualControlSelector, RcInputOnly)
 {
 	ManualControlSelector selector;
-	selector.setRcInMode(0);
+	selector.setRcInMode(0); // Configure RC input only
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
@@ -81,25 +119,26 @@ TEST(ManualControlSelector, RcInputOnly)
 	timestamp += 100_ms;
 
 	// Now provide input with the correct source.
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 1);
 }
 
 TEST(ManualControlSelector, MavlinkInputOnly)
 {
 	ManualControlSelector selector;
-	selector.setRcInMode(1);
+	selector.setRcInMode(1); // Configure MAVLink input only
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
@@ -108,128 +147,131 @@ TEST(ManualControlSelector, MavlinkInputOnly)
 	timestamp += 100_ms;
 
 	// Now provide input with the correct source.
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_3;
+	input.data_source = SOURCE_MAVLINK_3;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_3);
 	EXPECT_EQ(selector.instance(), 1);
 
 	timestamp += 100_ms;
 
 	// But only the first MAVLink source wins, others are too late.
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_4;
+	input.data_source = SOURCE_MAVLINK_4;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_3);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_3);
 	EXPECT_EQ(selector.instance(), 1);
 }
 
-TEST(ManualControlSelector, AutoInput)
+TEST(ManualControlSelector, RcMavlinkInputFallback)
 {
 	ManualControlSelector selector;
-	selector.setRcInMode(2);
+	selector.setRcInMode(2); // Configure fallback
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 100_ms;
 
 	// Now provide input from MAVLink as well which should get ignored.
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 500_ms;
 
 	// Now we'll let RC time out, so it should switch to MAVLINK.
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_MAVLINK_0);
 	EXPECT_EQ(selector.instance(), 1);
 }
 
-TEST(ManualControlSelector, FirstInput)
+TEST(ManualControlSelector, RcMavlinkInputKeepFirst)
 {
 	ManualControlSelector selector;
-	selector.setRcInMode(3);
+	selector.setRcInMode(3); // Configure keep first input
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 100_ms;
 
 	// Now provide input from MAVLink as well which should get ignored.
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 500_ms;
 
 	// Now we'll let RC time out, but it should NOT switch to MAVLINK because RC was first
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
 	EXPECT_FALSE(selector.setpoint().valid);
-	EXPECT_FALSE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_MAVLINK_0);
+	EXPECT_NE(selector.setpoint().data_source, SOURCE_MAVLINK_0);
 	EXPECT_EQ(selector.instance(), -1);
 
 	timestamp += 100_ms;
 
 	// Provide input from RC again and it should get accepted because it was the first.
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 }
 
 TEST(ManualControlSelector, DisabledInput)
 {
 	ManualControlSelector selector;
-	selector.setRcInMode(4);
+	selector.setRcInMode(4); // Configure disabled stick input
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
 	// Reject MAVLink stick input
-	input.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0;
+	input.data_source = SOURCE_MAVLINK_0;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
@@ -239,7 +281,7 @@ TEST(ManualControlSelector, DisabledInput)
 	timestamp += 100_ms;
 
 	// Reject RC stick input
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 1);
 
@@ -253,15 +295,16 @@ TEST(ManualControlSelector, RcTimeout)
 	selector.setRcInMode(0);
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp;
 	selector.updateWithNewInputSample(timestamp, input, 0);
 
 	EXPECT_TRUE(selector.setpoint().valid);
-	EXPECT_TRUE(selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC);
+	EXPECT_EQ(selector.setpoint().data_source, SOURCE_RC);
 	EXPECT_EQ(selector.instance(), 0);
 
 	timestamp += 600_ms;
@@ -279,10 +322,11 @@ TEST(ManualControlSelector, RcOutdated)
 	selector.setRcInMode(0);
 	selector.setTimeout(500_ms);
 
-	uint64_t timestamp = some_time;
+	uint64_t timestamp = SOME_TIME;
 
 	manual_control_setpoint_s input {};
-	input.data_source = manual_control_setpoint_s::SOURCE_RC;
+	input.data_source = SOURCE_RC;
+	input.valid = true;
 	input.timestamp_sample = timestamp - 600_ms; // First sample is already outdated
 	selector.updateWithNewInputSample(timestamp, input, 0);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -241,9 +241,9 @@ static const StreamListItem streams_list[] = {
 #if defined(COMMAND_LONG_HPP)
 	create_stream_list_item<MavlinkStreamCommandLong>(),
 #endif // COMMAND_LONG_HPP
-#if defined(SYSTEM_TIME_HPP)
+#if defined(SYS_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamSysStatus>(),
-#endif // SYSTEM_TIME_HPP
+#endif // SYS_STATUS_HPP
 	create_stream_list_item<MavlinkStreamBatteryStatus>(),
 #if defined(SMART_BATTERY_INFO_HPP)
 	create_stream_list_item<MavlinkStreamSmartBatteryInfo>(),

src/modules/mavlink/mavlink_receiver.cpp

@@ -78,6 +78,16 @@ MavlinkReceiver::~MavlinkReceiver()
 #if !defined(CONSTRAINED_FLASH)
 	delete[] _received_msg_stats;
 #endif // !CONSTRAINED_FLASH
+
+	_distance_sensor_pub.unadvertise();
+	_gps_inject_data_pub.unadvertise();
+	_rc_pub.unadvertise();
+	_manual_control_input_pub.unadvertise();
+	_ping_pub.unadvertise();
+	_radio_status_pub.unadvertise();
+	_sensor_baro_pub.unadvertise();
+	_sensor_gps_pub.unadvertise();
+	_sensor_optical_flow_pub.unadvertise();
 }
 
 static constexpr vehicle_odometry_s vehicle_odometry_empty {
@@ -2055,6 +2065,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
 	manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
 	manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
+	manual_control_setpoint.valid = true;
 	_manual_control_input_pub.publish(manual_control_setpoint);
 }
 

src/modules/mavlink/mavlink_receiver.h

@@ -335,7 +335,7 @@ private:
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
 	uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
 	uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
-	uORB::PublicationMulti<sensor_optical_flow_s>         _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s>           _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	// ORB publications (queue length > 1)
 	uORB::Publication<transponder_report_s>  _transponder_report_pub{ORB_ID(transponder_report)};

src/modules/mavlink/mavlink_shell.cpp

@@ -121,8 +121,8 @@ int MavlinkShell::start()
 
 	char r_in[32];
 	char r_out[32];
-	sprintf(r_in, "%d", remote_in_fd);
-	sprintf(r_out, "%d", remote_out_fd);
+	snprintf(r_in, sizeof(r_in), "%d", remote_in_fd);
+	snprintf(r_out, sizeof(r_out), "%d", remote_out_fd);
 	char *const argv[3] = {r_in, r_out, nullptr};
 
 #else

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -80,7 +80,7 @@ MulticopterRateControl::parameters_updated()
 	// to the ideal (K * [1 + 1/sTi + sTd]) form
 	const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
 
-	_rate_control.setGains(
+	_rate_control.setPidGains(
 		rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
 		rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
 		rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -206,6 +206,10 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
 	if (!_takeoff_failed) {
 		_takeoff_failed = !checkTakeoff(mission_item);
 	}
+
+	if (!_items_fit_to_vehicle_type_failed) {
+		_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
+	}
 }
 
 void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
@@ -686,8 +690,6 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_
 
 	if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) {
 
-
-
 		mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1);
 		events::send<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
 				      "Waypoint {1} below home", current_index + 1);
@@ -695,3 +697,19 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_
 
 	return true;
 }
+
+bool FeasibilityChecker::checkItemsFitToVehicleType(const mission_item_s &mission_item)
+{
+	if (_vehicle_type != VehicleType::Vtol &&
+	    (mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+	     || mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION)) {
+
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Mission contains VTOL items but vehicle is not a VTOL\t");
+		events::send(events::ID("navigator_mis_vtol_items"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Mission contains VTOL items but vehicle is not a VTOL");
+
+		return false;
+	}
+
+	return true;
+}

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -120,6 +120,7 @@ private:
 	bool _below_home_alt_failed{false};
 	bool _fixed_wing_land_approach_failed{false};
 	bool _takeoff_land_available_failed{false};
+	bool _items_fit_to_vehicle_type_failed{false};
 
 	// internal checkTakeoff related variables
 	bool _found_item_with_position{false};
@@ -163,6 +164,14 @@ private:
 	*/
 	bool checkTakeoff(mission_item_s &mission_item);
 
+	/**
+	 * @brief Check if the mission items fit to the vehicle type
+	 *
+	 * @param mission_item The current mission item
+	 * @return False if the check failed.
+	*/
+	bool checkItemsFitToVehicleType(const mission_item_s &mission_item);
+
 	/**
 	 * @brief Check validity of landing pattern (fixed wing & vtol)
 	 *

src/modules/navigator/mission.cpp

@@ -982,7 +982,10 @@ Mission::set_mission_items()
 					_mission_item.altitude_is_relative = false;
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					_mission_item.autocontinue = true;
-					_mission_item.time_inside = 0.0f;
+
+					// have to reset here because these field were used in set_vtol_transition_item
+					_mission_item.time_inside = 0.f;
+					_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 
 					// make previous setpoint invalid, such that there will be no prev-current line following.
 					// if the vehicle drifted off the path during back-transition it should just go straight to the landing point

src/modules/navigator/rtl.cpp

@@ -544,6 +544,10 @@ void RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 
+			// have to reset here because these field were used in set_vtol_transition_item
+			_mission_item.time_inside = 0.f;
+			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+
 			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
 				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
 
@@ -554,7 +558,6 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _navigator->get_local_position()->heading;
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.origin = ORIGIN_ONBOARD;
 			break;
 		}

src/modules/navigator/vtol_takeoff.cpp

@@ -117,8 +117,9 @@ VtolTakeoff::on_active()
 				// we need the vehicle to loiter indefinitely but also we want this mission item to be reached as soon
 				// as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly,
 				// however it will just continue loitering as there is no next mission item
-				_mission_item.time_inside = 1;
+				_mission_item.time_inside = 1.f;
 				_mission_item.loiter_radius = _navigator->get_loiter_radius();
+				_mission_item.acceptance_radius  = _navigator->get_acceptance_radius();
 				_mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get();
 
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

src/modules/rc_update/rc_update.cpp

@@ -72,23 +72,23 @@ RCUpdate::RCUpdate() :
 		char nbuf[16];
 
 		/* min values */
-		sprintf(nbuf, "RC%d_MIN", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MIN", i + 1);
 		_parameter_handles.min[i] = param_find(nbuf);
 
 		/* trim values */
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_TRIM", i + 1);
 		_parameter_handles.trim[i] = param_find(nbuf);
 
 		/* max values */
-		sprintf(nbuf, "RC%d_MAX", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MAX", i + 1);
 		_parameter_handles.max[i] = param_find(nbuf);
 
 		/* channel reverse */
-		sprintf(nbuf, "RC%d_REV", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_REV", i + 1);
 		_parameter_handles.rev[i] = param_find(nbuf);
 
 		/* channel deadzone */
-		sprintf(nbuf, "RC%d_DZ", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_DZ", i + 1);
 		_parameter_handles.dz[i] = param_find(nbuf);
 	}
 
@@ -127,7 +127,6 @@ void RCUpdate::parameters_updated()
 {
 	// rc values
 	for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
-
 		float min = 0.f;
 		param_get(_parameter_handles.min[i], &min);
 		_parameters.min[i] = min;
@@ -155,6 +154,12 @@ void RCUpdate::parameters_updated()
 
 	update_rc_functions();
 
+	_rc_calibrated = _param_rc_chan_cnt.get() > 0
+			 && (_param_rc_map_throttle.get() > 0
+			     || _param_rc_map_roll.get() > 0
+			     || _param_rc_map_pitch.get() > 0
+			     || _param_rc_map_yaw.get() > 0);
+
 	// deprecated parameters, will be removed post v1.12 once QGC is updated
 	{
 		int32_t rc_map_value = 0;
@@ -687,6 +692,7 @@ void RCUpdate::UpdateManualControlInput(const hrt_abstime &timestamp_sample)
 	manual_control_input.aux4  = get_rc_value(rc_channels_s::FUNCTION_AUX_4,   -1.f, 1.f);
 	manual_control_input.aux5  = get_rc_value(rc_channels_s::FUNCTION_AUX_5,   -1.f, 1.f);
 	manual_control_input.aux6  = get_rc_value(rc_channels_s::FUNCTION_AUX_6,   -1.f, 1.f);
+	manual_control_input.valid = _rc_calibrated;
 
 	// publish manual_control_input topic
 	manual_control_input.timestamp = hrt_absolute_time();

src/modules/rc_update/rc_update.h

@@ -170,6 +170,7 @@ public:
 	manual_control_switches_s _manual_switches_previous{};
 	manual_control_switches_s _manual_switches_last_publish{};
 	rc_channels_s _rc{};
+	bool _rc_calibrated{false};
 
 	rc_parameter_map_s _rc_parameter_map {};
 	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {};	/**< parameter values for RC control */

src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake

@@ -91,6 +91,7 @@ if(gazebo_FOUND)
 		plane_catapult
 		plane_lidar
 		px4vision
+		quadtailsitter
 		r1_rover
 		rover
 		standard_vtol

src/modules/temperature_compensation/TemperatureCompensation.cpp

@@ -59,25 +59,25 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 
 	if (ret == PX4_OK && gyro_tc_enabled) {
 		for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) {
-			sprintf(nbuf, "TC_G%d_ID", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_G%d_ID", j);
 			parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf);
 
 			for (unsigned i = 0; i < 3; i++) {
-				sprintf(nbuf, "TC_G%d_X3_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_G%d_X3_%d", j, i);
 				parameter_handles.gyro_cal_handles[j].x3[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_G%d_X2_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_G%d_X2_%d", j, i);
 				parameter_handles.gyro_cal_handles[j].x2[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_G%d_X1_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_G%d_X1_%d", j, i);
 				parameter_handles.gyro_cal_handles[j].x1[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_G%d_X0_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_G%d_X0_%d", j, i);
 				parameter_handles.gyro_cal_handles[j].x0[i] = param_find(nbuf);
 			}
 
-			sprintf(nbuf, "TC_G%d_TREF", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_G%d_TREF", j);
 			parameter_handles.gyro_cal_handles[j].ref_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_G%d_TMIN", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_G%d_TMIN", j);
 			parameter_handles.gyro_cal_handles[j].min_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_G%d_TMAX", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_G%d_TMAX", j);
 			parameter_handles.gyro_cal_handles[j].max_temp = param_find(nbuf);
 		}
 	}
@@ -89,25 +89,25 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 
 	if (ret == PX4_OK && accel_tc_enabled) {
 		for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
-			sprintf(nbuf, "TC_A%d_ID", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_ID", j);
 			parameter_handles.accel_cal_handles[j].ID = param_find(nbuf);
 
 			for (unsigned i = 0; i < 3; i++) {
-				sprintf(nbuf, "TC_A%d_X3_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X3_%d", j, i);
 				parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_A%d_X2_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X2_%d", j, i);
 				parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_A%d_X1_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X1_%d", j, i);
 				parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_A%d_X0_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X0_%d", j, i);
 				parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf);
 			}
 
-			sprintf(nbuf, "TC_A%d_TREF", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TREF", j);
 			parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_A%d_TMIN", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMIN", j);
 			parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_A%d_TMAX", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMAX", j);
 			parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf);
 		}
 	}
@@ -119,25 +119,25 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 
 	if (ret == PX4_OK && baro_tc_enabled) {
 		for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
-			sprintf(nbuf, "TC_B%d_ID", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_ID", j);
 			parameter_handles.baro_cal_handles[j].ID = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X5", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X5", j);
 			parameter_handles.baro_cal_handles[j].x5 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X4", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X4", j);
 			parameter_handles.baro_cal_handles[j].x4 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X3", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X3", j);
 			parameter_handles.baro_cal_handles[j].x3 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X2", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X2", j);
 			parameter_handles.baro_cal_handles[j].x2 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X1", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X1", j);
 			parameter_handles.baro_cal_handles[j].x1 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_X0", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_X0", j);
 			parameter_handles.baro_cal_handles[j].x0 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_TREF", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_TREF", j);
 			parameter_handles.baro_cal_handles[j].ref_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_TMIN", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_TMIN", j);
 			parameter_handles.baro_cal_handles[j].min_temp = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_TMAX", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_B%d_TMAX", j);
 			parameter_handles.baro_cal_handles[j].max_temp = param_find(nbuf);
 		}
 	}

src/modules/temperature_compensation/temperature_calibration/accel.cpp

@@ -212,7 +212,7 @@ int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int
 
 	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
 		for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
-			sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
+			snprintf(str, sizeof(str), "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
 			param = (float)res[axis_index][coef_index];
 			result = param_set_no_notification(param_find(str), &param);
 

src/modules/temperature_compensation/temperature_calibration/baro.cpp

@@ -197,7 +197,7 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
 
 	for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
-		sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
+		snprintf(str, sizeof(str), "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
 		param = (float)res[coef_index];
 		result = param_set_no_notification(param_find(str), &param);
 

src/modules/temperature_compensation/temperature_calibration/common.h

@@ -97,7 +97,7 @@ protected:
 int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
 {
 	char param_str[30] {};
-	(void)sprintf(param_str, format_str, index);
+	(void)snprintf(param_str, sizeof(param_str), format_str, index);
 	int result = param_set_no_notification(param_find(param_str), value);
 
 	if (result != 0) {

src/modules/temperature_compensation/temperature_calibration/gyro.cpp

@@ -197,7 +197,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
 
 	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
 		for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
-			sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
+			snprintf(str, sizeof(str), "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
 			param = (float)res[axis_index][coef_index];
 			result = param_set_no_notification(param_find(str), &param);
 

src/modules/vtol_att_control/standard_params.c

@@ -71,6 +71,8 @@ PARAM_DEFINE_INT32(VT_FWD_THRUST_EN, 0);
  *
  * @min 0.0
  * @max 2.0
+ * @increment 0.01
+ * @decimal 2
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.7f);
@@ -83,6 +85,8 @@ PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.7f);
  * @unit s
  * @min 0.0
  * @max 20.0
+ * @increment 0.1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_B_TRANS_RAMP, 3.0f);

src/modules/vtol_att_control/tiltrotor_params.c

@@ -39,7 +39,7 @@
  */
 
 /**
- * Position of tilt servo in mc mode
+ * Normalized tilt in Hover
  *
  * @min 0.0
  * @max 1.0
@@ -50,7 +50,7 @@
 PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
 
 /**
- * Position of tilt servo in transition mode
+ * Normalized tilt in transition to FW
  *
  * @min 0.0
  * @max 1.0
@@ -58,10 +58,10 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
  * @decimal 3
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
+PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.4f);
 
 /**
- * Position of tilt servo in fw mode
+ * Normalized tilt in FW
  *
  * @min 0.0
  * @max 1.0

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -197,12 +197,12 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
 /**
  * Quad-chute uncommanded descent threshold
  *
- * Threshold for integrated height rate error to trigger a uncommanded-descent quad-chute.
- * Only checked in altitude-controlled fixed-wing flight.
- * Additional conditions that have to be met for uncommanded descent detection are a positive (climbing) height
- * rate setpoint and a negative (sinking) current height rate estimate.
+ * Altitude error threshold for quad-chute triggering during fixed-wing flight.
+ * The check is only active if altitude is controlled and the vehicle is below the current altitude reference.
+ * The altitude error is relative to the highest altitude the vehicle has achieved since it has flown below the current
+ * altitude reference.
  *
- * Set to 0 do disable this threshold.
+ * Set to 0 do disable.
  *
  * @unit m
  * @min 0.0
@@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
  * @decimal 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
+PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f);
 
 /**
  * Quad-chute transition altitude loss threshold

src/modules/vtol_att_control/vtol_type.cpp

@@ -272,26 +272,21 @@ bool VtolType::isMinAltBreached()
 
 bool VtolType::isUncommandedDescent()
 {
-	if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled
+	const float current_altitude = -_local_pos->z + _local_pos->ref_alt;
+
+	if (_param_vt_qc_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _local_pos->z_global
+	    && _v_control_mode->flag_control_altitude_enabled
+	    && PX4_ISFINITE(_tecs_status->altitude_reference)
+	    && (current_altitude < _tecs_status->altitude_reference)
 	    && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
 
-		// TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here
+		_quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude),
+					       _tecs_status->altitude_reference);
 
-		if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) {
-			// vehicle is currently in uncommended descend, start integrating error
+		return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get();
 
-			const hrt_abstime now = hrt_absolute_time();
-			float dt = static_cast<float>(now - _last_loop_quadchute_timestamp) / 1e6f;
-			dt = math::constrain(dt, 0.0001f, 0.1f);
-			_last_loop_quadchute_timestamp = now;
-
-			_height_rate_error_integral += (_tecs_status->height_rate_setpoint - _tecs_status->height_rate) * dt;
-
-		} else {
-			_height_rate_error_integral = 0.f; // reset
-		}
-
-		return (_height_rate_error_integral > _param_vt_qc_hr_error_i.get());
+	} else {
+		_quadchute_ref_alt = -MAXFLOAT;
 	}
 
 	return false;

src/modules/vtol_att_control/vtol_type.h

@@ -288,9 +288,6 @@ protected:
 	float _thrust_transition = 0.0f;	// thrust value applied during a front transition (tailsitter & tiltrotor only)
 	float _last_thr_in_fw_mode = 0.0f;
 
-	float _height_rate_error_integral{0.f};
-
-
 	hrt_abstime _trans_finished_ts = 0;
 	hrt_abstime _transition_start_timestamp{0};
 	float _time_since_trans_start{0};
@@ -300,7 +297,8 @@ protected:
 
 	hrt_abstime _last_loop_ts = 0;
 	float _transition_dt = 0;
-	hrt_abstime _last_loop_quadchute_timestamp = 0;
+
+	float _quadchute_ref_alt{-MAXFLOAT};	// altitude (AMSL) reference to compute quad-chute altitude loss condition
 
 	float _accel_to_pitch_integ = 0;
 
@@ -317,7 +315,7 @@ protected:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
 					(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
 					(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
-					(ParamFloat<px4::params::VT_QC_HR_ERROR_I>) _param_vt_qc_hr_error_i,
+					(ParamFloat<px4::params::VT_QC_ALT_LOSS>) _param_vt_qc_alt_loss,
 					(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
 					(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
 					(ParamFloat<px4::params::VT_QC_T_ALT_LOSS>) _param_vt_qc_t_alt_loss,

src/systemcmds/netman/netman.cpp

@@ -405,12 +405,25 @@ static void usage(const char *reason)
   memory. On boot the `update` option will be run. If a network configuration
   does not exist. The default setting will be saved in non-volatile and the
   system rebooted.
-  On Subsequent boots, the `update` option will check for the existence of
-  `net.cfg` in the root of the SD Card.  It will saves the network settings
-  from `net.cfg` in non-volatile memory, delete the file and reboot the system.
 
-  The `save` option will `net.cfg` on the SD Card. Use this to edit the settings.
-  The  `show` option will display the network settings  to the console.
+  #### update
+
+  `netman update` is run automatically by [a startup script](../concept/system_startup.md#system-startup).
+  When run, the `update` option will check for the existence of `net.cfg` in the root of the SD Card.
+  It then saves the network settings from `net.cfg` in non-volatile memory,
+  deletes the file and reboots the system.
+
+  #### save
+
+  The `save` option will save settings from non-volatile memory to a file named
+  `net.cfg` on the SD Card filesystem for editing. Use this to edit the settings.
+  Save does not immediately apply the network settings; the user must reboot the flight stack.
+  By contrast, the `update` command is run by the start-up script, commits the settings to non-volatile memory,
+  and reboots the flight controller (which will then use the new settings).
+
+  #### show
+
+  The `show` option will display the network settings in `net.cfg` to the console.
 
   ### Examples
   $ netman save           # Save the parameters to the SD card.

src/systemcmds/tests/test_float.cpp

@@ -89,7 +89,7 @@ bool FloatTest::singlePrecisionTests()
 		  fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON);
 
 	char sbuf[30];
-	sprintf(sbuf, "%8.4f", (double)0.553415f);
+	snprintf(sbuf, sizeof(sbuf), "%8.4f", (double)0.553415f);
 	ut_compare("sbuf[0]", sbuf[0], ' ');
 	ut_compare("sbuf[1]", sbuf[1], ' ');
 	ut_compare("sbuf[2]", sbuf[2], '0');
@@ -100,7 +100,7 @@ bool FloatTest::singlePrecisionTests()
 	ut_compare("sbuf[7]", sbuf[7], '4');
 	ut_compare("sbuf[8]", sbuf[8], '\0');
 
-	sprintf(sbuf, "%8.4f", (double) - 0.553415f);
+	snprintf(sbuf, sizeof(sbuf), "%8.4f", (double) - 0.553415f);
 	ut_compare("sbuf[0]", sbuf[0], ' ');
 	ut_compare("sbuf[1]", sbuf[1], '-');
 	ut_compare("sbuf[2]", sbuf[2], '0');
@@ -144,7 +144,7 @@ bool FloatTest::doublePrecisionTests()
 
 
 	char sbuf[30];
-	sprintf(sbuf, "%8.4f", 0.553415);
+	snprintf(sbuf, sizeof(sbuf), "%8.4f", 0.553415);
 	ut_compare("sbuf[0]", sbuf[0], ' ');
 	ut_compare("sbuf[1]", sbuf[1], ' ');
 	ut_compare("sbuf[2]", sbuf[2], '0');
@@ -156,7 +156,7 @@ bool FloatTest::doublePrecisionTests()
 	ut_compare("sbuf[8]", sbuf[8], '\0');
 
 
-	sprintf(sbuf, "%8.4f", -0.553415);
+	snprintf(sbuf, sizeof(sbuf), "%8.4f", -0.553415);
 	ut_compare("sbuf[0]", sbuf[0], ' ');
 	ut_compare("sbuf[1]", sbuf[1], '-');
 	ut_compare("sbuf[2]", sbuf[2], '0');