fmu-v6x: add crossfire UART driver

[AUTO COMMIT] update change indication
matrix: fix slice to slice assignment to do deep copy
2026-06-20 16:30:35 +08:00 · 2024-01-11 11:04:51 -05:00 · 2023-12-22 09:46:09 +01:00 · 2023-12-22 09:46:09 +01:00 · 2023-12-12 20:48:28 -05:00 · 2023-12-12 20:48:28 -05:00
63 changed files with 1281 additions and 320 deletions
@@ -266,12 +266,9 @@ else
 		. $FCONFIG
 	fi

-	#
-	# Start IO for PWM output or RC input if enabled
-	#
-	if param compare -s SYS_USE_IO 1
+	if px4io supported
 	then
-		# Check if PX4IO present and update firmware if needed.
+	# Check if PX4IO present and update firmware if needed.
 		if [ -f $IOFW ]
 		then
 			if ! px4io checkcrc ${IOFW}
@@ -293,12 +290,12 @@ else
 					tune_control stop
 				fi
 			fi
-		fi

-		if ! px4io start
-		then
-			echo "PX4IO start failed"
-			set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+			if ! px4io start
+			then
+				echo "PX4IO start failed"
+				set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
+			fi
 		fi
 	fi

@@ -32,11 +32,4 @@ then
 	param set-default SENS_TEMP_ID 3014666
 fi

-if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
-then
-	param set-default SYS_USE_IO 0
-else
-	param set-default SYS_USE_IO 1
-fi
-
 safety_button start
@@ -247,7 +247,6 @@
 /* PWM
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8
-#define BOARD_PWM_FREQ	             1024000

 #define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
 #define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
@@ -82,9 +82,7 @@
 #define rDMAR(_tmr)     REG(_tmr, STM32_GTIM_DMAR_OFFSET)
 #define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)

-#if !defined(BOARD_PWM_FREQ)
-#define BOARD_PWM_FREQ 1000000
-#endif
+#define BOARD_SPIX_SYNC_PWM_FREQ 1024000

 unsigned
 spix_sync_timer_get_period(unsigned timer)
@@ -129,11 +127,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate)
 		 * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
 		 */

-		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
+		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1;

 		/* configure the timer to update at the desired rate */

-		rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
+		rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1;

 		/* generate an update event; reloads the counter and all registers */
 		rEGR(timer) = GTIM_EGR_UG;
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -11,5 +11,3 @@ param set-default BAT2_A_PER_V 36.367515152

 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
-
-param set-default SYS_USE_IO 1
@@ -9,7 +9,5 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152

-param set-default SYS_USE_IO 1
-
 rgbled_pwm start
 safety_button start
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 26.4
 param set-default EKF2_MULTI_IMU 2
 param set-default SENS_IMU_MODE 0

-param set-default SYS_USE_IO 1
-
 set LOGGER_BUF 64
@@ -9,13 +9,6 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152

-if ver hwtypecmp V5004000 V5006000
-then
-	param set-default SYS_USE_IO 0
-else
-	param set-default SYS_USE_IO 1
-fi
-
 if ver hwtypecmp V5005000 V5005002 V5006000 V5006002
 then
 	# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs
@@ -16,6 +16,4 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-param set-default SYS_USE_IO 1
-
 safety_button start
@@ -9,5 +9,3 @@ param set-default BAT2_V_DIV 18.1

 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -36,6 +36,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -16,6 +16,5 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-param set-default SYS_USE_IO 1

 safety_button start
@@ -25,8 +25,6 @@ param set-default BAT_V_OFFS_CURR 0.413
 # Disable safety switch
 param set-default CBRK_IO_SAFETY 22027

-param set-default SYS_USE_IO 1
-
 safety_button start

 set LOGGER_BUF 32
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -522,7 +522,7 @@ void ICM45686::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
 			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);

 		} else {
-			accel.dt = FIFO_TIMESTAMP_SCALING;
+			accel.dt = FIFO_SAMPLE_DT;
 		}

 		// 20 bit hires mode
@@ -634,7 +634,7 @@ void ICM45686::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
 			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);

 		} else {
-			gyro.dt = FIFO_TIMESTAMP_SCALING;
+			gyro.dt = FIFO_SAMPLE_DT;
 		}

 		// 20 bit hires mode
@@ -70,7 +70,7 @@ private:
 	void exit_and_cleanup() override;

 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 6400.f};     // 6400 Hz accel & gyro ODR configured
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};

@@ -38,7 +38,7 @@

 void ICM45686::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
+	PRINT_MODULE_USAGE_NAME("icm45686", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
@@ -336,8 +336,7 @@ private:
 		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
 		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal,
-		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
-		(ParamInt<px4::params::SYS_USE_IO>) _param_sys_use_io
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
 	)
 };

@@ -468,7 +467,7 @@ int PX4IO::init()
 	}

 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
+	if (_param_sys_hitl.get() <= 0) {
 		_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
 	}

@@ -1557,6 +1556,10 @@ int PX4IO::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];

+	if (!strcmp(verb, "supported")) {
+		return 0;
+	}
+
 	if (!strcmp(verb, "checkcrc")) {
 		if (is_running()) {
 			PX4_ERR("io must be stopped");
@@ -1762,7 +1765,7 @@ Output driver communicating with the IO co-processor.
 extern "C" __EXPORT int px4io_main(int argc, char *argv[])
 {
 	if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
-		PX4_ERR("PX4IO Not Supported");
+		PX4_INFO("PX4IO Not Supported");
 		return -1;
 	}
 	return PX4IO::main(argc, argv);
@@ -42,18 +42,6 @@
 #include <px4_platform_common/px4_config.h>
 #include <parameters/param.h>

-/**
- * Set usage of IO board
- *
- * Can be used to use a configure the use of the IO board.
- *
- * @value 0 IO PWM disabled (RC only)
- * @value 1 IO enabled (RC & PWM)
- * @reboot_required true
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_USE_IO, 0);
-
 /**
 * S.BUS out
 *
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017-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
@@ -61,6 +61,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL)
 add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
+add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
 add_subdirectory(systemlib EXCLUDE_FROM_ALL)
@@ -70,6 +71,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL)
 add_subdirectory(timesync EXCLUDE_FROM_ALL)
 add_subdirectory(tinybson EXCLUDE_FROM_ALL)
 add_subdirectory(tunes EXCLUDE_FROM_ALL)
+add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(version EXCLUDE_FROM_ALL)
 add_subdirectory(weather_vane EXCLUDE_FROM_ALL)
 add_subdirectory(wind_estimator EXCLUDE_FROM_ALL)
@@ -35,6 +35,8 @@ public:
 		assert(y0 + Q <= N);
 	}

+	Slice(const Slice<Type, P, Q, M, N> &other) = default;
+
 	const Type &operator()(size_t i, size_t j) const
 	{
 		assert(i < P);
@@ -52,6 +54,12 @@ public:
 		return (*_data)(_x0 + i, _y0 + j);
 	}

+	// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
+	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
+	{
+		return this->operator=<M, N>(other);
+	}
+
 	template<size_t MM, size_t NN>
 	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
 	{
@@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+
+TEST(MatrixSliceTest, XYAssignmentTest)
+{
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	// Assign first two elements from b to first two slot of a
+	a.xy() = b.xy();
+	EXPECT_EQ(a, Vector3f(4, 5, 3));
+}
@@ -134,21 +134,20 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co
 	}

 	// Get the crossing point using L1-style guidance
-	auto l1_point = _getL1Point(position, waypoints);
-	return {l1_point(0), l1_point(1), target(2)};
+	return _getL1Point(position, waypoints);
 }

-const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
+const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
 {
-	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
-				_trajectory[1].getCurrentPosition());
-	const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero();
-	const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0]));
-	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
-	const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest;
+	const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(),
+				_trajectory[2].getCurrentPosition());
+	const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero();
+	const Vector3f prev_to_pos(pos_traj - waypoints[0]);
+	const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector3f closest_pt = waypoints[0] + prev_to_closest;

 	// Compute along-track error using L1 distance and cross-track error
-	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
+	const float crosstrack_error = (closest_pt - pos_traj).length();

 	const float l1 = math::max(_target_acceptance_radius, 5.f);
 	float alongtrack_error = 0.f;
@@ -159,9 +158,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 	}

 	// Position of the point on the line where L1 intersect the line between the two waypoints
-	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
-
-	return l1_point;
+	return closest_pt + alongtrack_error * u_prev_to_target;
 }

 const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
@@ -438,7 +438,7 @@ private:
 	const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
 			bool is_single_waypoint,
 			const Vector3f &feedforward_velocity_setpoint);
-	const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
+	const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const;
 	float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const;
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(ringbuffer
+	Ringbuffer.cpp
+)
+
+target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer)
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   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 "Ringbuffer.hpp"
+
+#include <mathlib/mathlib.h>
+#include <assert.h>
+#include <string.h>
+
+
+Ringbuffer::~Ringbuffer()
+{
+	deallocate();
+}
+
+bool Ringbuffer::allocate(size_t buffer_size)
+{
+	assert(_ringbuffer == nullptr);
+
+	_size = buffer_size;
+	_ringbuffer = new uint8_t[_size];
+	return _ringbuffer != nullptr;
+}
+
+void Ringbuffer::deallocate()
+{
+	delete[] _ringbuffer;
+	_ringbuffer = nullptr;
+	_size = 0;
+}
+
+size_t Ringbuffer::space_available() const
+{
+	if (_start > _end) {
+		return _start - _end - 1;
+
+	} else {
+		return _start - _end - 1 + _size;
+	}
+}
+
+size_t Ringbuffer::space_used() const
+{
+	if (_start <= _end) {
+		return _end - _start;
+
+	} else {
+		// Potential wrap around.
+		return _end - _start + _size;
+	}
+}
+
+
+bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len)
+{
+	if (buf_len == 0 || buf == nullptr) {
+		// Nothing to add, we better don't try.
+		return false;
+	}
+
+	if (_start > _end) {
+		// Add after end up to start, no wrap around.
+
+		// Leave one byte free so that start don't end up the same
+		// which signals empty.
+		const size_t available = _start - _end - 1;
+
+		if (available < buf_len) {
+			return false;
+		}
+
+		memcpy(&_ringbuffer[_end], buf, buf_len);
+		_end += buf_len;
+
+	} else {
+		// Add after end, maybe wrap around.
+		const size_t available = _start - _end - 1 + _size;
+
+		if (available < buf_len) {
+			return false;
+		}
+
+		const size_t remaining_packet_len = _size - _end;
+
+		if (buf_len > remaining_packet_len) {
+			memcpy(&_ringbuffer[_end], buf, remaining_packet_len);
+			_end = 0;
+
+			memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len);
+			_end += buf_len - remaining_packet_len;
+
+		} else {
+			memcpy(&_ringbuffer[_end], buf, buf_len);
+			_end += buf_len;
+		}
+	}
+
+	return true;
+}
+
+size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
+{
+	if (buf == nullptr) {
+		// User needs to supply a valid pointer.
+		return 0;
+	}
+
+	if (_start == _end) {
+		// Empty
+		return 0;
+	}
+
+	if (_start < _end) {
+
+		// No wrap around.
+		size_t to_copy_len = math::min(_end - _start, buf_max_len);
+
+		memcpy(buf, &_ringbuffer[_start], to_copy_len);
+		_start += to_copy_len;
+
+		return to_copy_len;
+
+	} else {
+		// Potential wrap around.
+		size_t to_copy_len = _end - _start + _size;
+
+		if (to_copy_len > buf_max_len) {
+			to_copy_len = buf_max_len;
+		}
+
+		const size_t remaining_buf_len = _size - _start;
+
+		if (to_copy_len > remaining_buf_len) {
+
+			memcpy(buf, &_ringbuffer[_start], remaining_buf_len);
+			_start = 0;
+			memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len);
+			_start += to_copy_len - remaining_buf_len;
+
+		} else {
+			memcpy(buf, &_ringbuffer[_start], to_copy_len);
+			_start += to_copy_len;
+		}
+
+		return to_copy_len;
+	}
+}
@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   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 <stdint.h>
+#include <pthread.h>
+
+
+// FIFO ringbuffer implementation.
+//
+// The ringbuffer can store up 1 byte less than allocated as
+// start and end marker need to be one byte apart when the buffer
+// is full, otherwise it would suddenly be empty.
+//
+// The buffer is not thread-safe.
+
+class Ringbuffer
+{
+public:
+	/* @brief Constructor
+	 *
+	 * @note Does not allocate automatically.
+	 */
+	Ringbuffer() = default;
+
+	/*
+	 * @brief Destructor
+	 *
+	 * Automatically calls deallocate.
+	 */
+	~Ringbuffer();
+
+	/* @brief Allocate ringbuffer
+	 *
+	 * @param buffer_size Number of bytes to allocate on heap.
+	 *
+	 * @returns false if allocation fails.
+	 */
+	bool allocate(size_t buffer_size);
+
+	/*
+	 * @brief Deallocate ringbuffer
+	 *
+	 * @note only required to deallocate and reallocate again.
+	 */
+	void deallocate();
+
+	/*
+	 * @brief Space available to copy bytes into
+	 *
+	 * @returns number of free bytes.
+	 */
+	size_t space_available() const;
+
+	/*
+	 * @brief Space used to copy data from
+	 *
+	 * @returns number of used bytes.
+	 */
+	size_t space_used() const;
+
+	/*
+	 * @brief Copy data into ringbuffer
+	 *
+	 * @param buf Pointer to buffer to copy from.
+	 * @param buf_len Number of bytes to copy.
+	 *
+	 * @returns true if packet could be copied into buffer.
+	 */
+	bool push_back(const uint8_t *buf, size_t buf_len);
+
+	/*
+	 * @brief Get data from ringbuffer
+	 *
+	 * @param buf Pointer to buffer where data can be copied into.
+	 * @param max_buf_len Max number of bytes to copy.
+	 *
+	 * @returns 0 if buffer is empty.
+	 */
+	size_t pop_front(uint8_t *buf, size_t max_buf_len);
+
+private:
+	size_t _size {0};
+	uint8_t *_ringbuffer {nullptr};
+	size_t _start{0};
+	size_t _end{0};
+};
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <stdint.h>
+#include <string.h>
+
+
+#include "Ringbuffer.hpp"
+
+class TempData
+{
+public:
+	TempData(size_t len)
+	{
+		_size = len;
+		_buf = new uint8_t[_size];
+	}
+
+	~TempData()
+	{
+		delete[] _buf;
+		_buf = nullptr;
+	}
+
+	uint8_t *buf() const
+	{
+		return _buf;
+	}
+
+	size_t size() const
+	{
+		return _size;
+	}
+
+	void paint(unsigned offset = 0)
+	{
+		for (size_t i = 0; i < _size; ++i) {
+			_buf[i] = (uint8_t)((i + offset) % UINT8_MAX);
+		}
+	}
+
+private:
+	uint8_t *_buf {nullptr};
+	size_t _size{0};
+
+};
+
+bool operator==(const TempData &lhs, const TempData &rhs)
+{
+	if (lhs.size() != rhs.size()) {
+		return false;
+	}
+
+	return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0;
+}
+
+
+TEST(Ringbuffer, AllocateAndDeallocate)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+	buf.deallocate();
+
+	ASSERT_TRUE(buf.allocate(1000));
+	// The second time we forget to clean up, but we expect no leak.
+}
+
+TEST(Ringbuffer, PushATooBigMessage)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{200};
+
+	// A message that doesn't fit should get rejected.
+	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
+}
+
+TEST(Ringbuffer, PushAndPopOne)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{20};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+	EXPECT_EQ(buf.space_used(), 20);
+	EXPECT_EQ(buf.space_available(), 79);
+
+	// Get everything
+	TempData out{20};
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20);
+	EXPECT_EQ(data, out);
+
+	// Nothing remaining
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0);
+}
+
+TEST(Ringbuffer, PushAndPopSeveral)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{90};
+	data.paint();
+
+	// 9 little chunks in
+	for (unsigned i = 0; i < 9; ++i) {
+		EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10));
+	}
+
+	// 10 won't because of overhead inside the buffer
+	EXPECT_FALSE(buf.push_back(data.buf(), 10));
+
+	TempData out{90};
+	// Take it back out in 2 big steps
+	EXPECT_EQ(buf.pop_front(out.buf(), 50), 50);
+	EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40);
+	EXPECT_EQ(data, out);
+}
+
+TEST(Ringbuffer, PushAndTryToPopMore)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data1{50};
+	data1.paint();
+	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
+
+	TempData out1{80};
+	EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size());
+}
+
+TEST(Ringbuffer, PushAndPopSeveralInterleaved)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data1{50};
+	data1.paint();
+	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
+
+	TempData data2{30};
+	data2.paint(50);
+	EXPECT_TRUE(buf.push_back(data2.buf(), data2.size()));
+
+	TempData out12{80};
+	EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size());
+
+	TempData out12_ref{80};
+	out12_ref.paint();
+	EXPECT_EQ(out12_ref, out12);
+
+	TempData data3{50};
+	data3.paint(33);
+	EXPECT_TRUE(buf.push_back(data3.buf(), data3.size()));
+
+	TempData out3{50};
+	EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size());
+	EXPECT_EQ(data3, out3);
+}
+
+TEST(Ringbuffer, PushEmpty)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+}
+
+TEST(Ringbuffer, PopWithoutBuffer)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+
+	TempData data{50};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+
+	EXPECT_EQ(buf.pop_front(nullptr, 50), 0);
+}
+
+TEST(Ringbuffer, EmptyAndNoSpaceForHeader)
+{
+	// Addressing a corner case where start and end are at the end
+	// and the same.
+
+	Ringbuffer buf;
+	// Allocate 1 bytes more than the packet, 1 for the start/end logic.
+	ASSERT_TRUE(buf.allocate(21));
+
+	{
+		TempData data{20};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{20};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+
+	{
+		TempData data{10};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{10};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+}
@@ -52,10 +52,17 @@ using namespace time_literals;

 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}

-void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
+void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim,
+				    const bool airspeed_sensor_available)
 {
-	_airspeed_state.speed = equivalent_airspeed;
-	_airspeed_state.speed_rate = 0.0f;
+	if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) {
+		_airspeed_state.speed = equivalent_airspeed;
+
+	} else {
+		_airspeed_state.speed = equivalent_airspeed_trim;
+	}
+
+	_airspeed_state.speed_rate = 0.f;
 }

 void TECSAirspeedFilter::update(const float dt, const Input &input, const Param &param,
@@ -353,7 +360,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt

 void TECSControl::_detectUnderspeed(const Input &input, const Param &param, const Flag &flag)
 {
-	if (!flag.detect_underspeed_enabled) {
+	if (!flag.detect_underspeed_enabled || !flag.airspeed_enabled) {
 		_ratio_undersped = 0.0f;
 		return;
 	}
@@ -412,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
 	const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
 	_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
 				    _pitch_setpoint + pitch_increment);
+	_pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max);

 	//Debug Output
 	_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
@@ -654,7 +662,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 	TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
 			.alt_rate = altitude_rate};
 	_altitude_reference_model.initialize(current_state);
-	_airspeed_filter.initialize(equivalent_airspeed);
+	_airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim,
+				    _control_flag.airspeed_enabled);

 	TECSControl::Setpoint control_setpoint;
 	control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
@@ -88,8 +88,11 @@ public:
 	 * @brief Initialize filter
 	 *
 	 * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
+	 * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s].
+	 * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
 	 */
-	void initialize(float equivalent_airspeed);
+	void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim,
+			const bool airspeed_sensor_available);

 	/**
 	 * @brief Update filter
@@ -696,9 +699,9 @@ private:
 		.max_climb_rate = 5.0f,
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
-		.tas_min = 3.0f,
-		.pitch_max = 5.0f,
-		.pitch_min = -5.0f,
+		.tas_min = 10.0f,
+		.pitch_max = 0.5f,
+		.pitch_min = -0.5f,
 		.throttle_trim = 0.0f,
 		.throttle_trim_adjusted = 0.f,
 		.throttle_max = 1.0f,
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(variable_length_ringbuffer
+	VariableLengthRingbuffer.cpp
+)
+
+target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer)
+
+target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer)
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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 "VariableLengthRingbuffer.hpp"
+
+#include <assert.h>
+#include <string.h>
+
+
+VariableLengthRingbuffer::~VariableLengthRingbuffer()
+{
+	deallocate();
+}
+
+bool VariableLengthRingbuffer::allocate(size_t buffer_size)
+{
+	return _ringbuffer.allocate(buffer_size);
+}
+
+void VariableLengthRingbuffer::deallocate()
+{
+	_ringbuffer.deallocate();
+}
+
+bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len)
+{
+	if (packet_len == 0 || packet == nullptr) {
+		// Nothing to add, we better don't try.
+		return false;
+	}
+
+	size_t space_required = packet_len + sizeof(Header);
+
+	if (space_required > _ringbuffer.space_available()) {
+		return false;
+	}
+
+	Header header{static_cast<uint32_t>(packet_len)};
+	bool result = _ringbuffer.push_back(reinterpret_cast<const uint8_t * >(&header), sizeof(header));
+	assert(result);
+
+	result = _ringbuffer.push_back(packet, packet_len);
+	assert(result);
+
+	// In case asserts are commented out to prevent unused warnings.
+	(void)result;
+
+	return true;
+}
+
+size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
+{
+	if (buf == nullptr) {
+		// User needs to supply a valid pointer.
+		return 0;
+	}
+
+	// Check next header
+	Header header;
+
+	if (_ringbuffer.pop_front(reinterpret_cast<uint8_t *>(&header), sizeof(header)) < sizeof(header)) {
+		return 0;
+	}
+
+	// We can't fit the packet into the user supplied buffer.
+	// This should never happen as the user has to supply a big // enough buffer.
+	assert(static_cast<uint32_t>(header.len) <= buf_max_len);
+
+	size_t bytes_read = _ringbuffer.pop_front(buf, header.len);
+	assert(bytes_read == header.len);
+
+	return bytes_read;
+}
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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 <stdint.h>
+#include <lib/ringbuffer/Ringbuffer.hpp>
+
+
+// FIFO ringbuffer implementation for packets of variable length.
+//
+// The variable length is implemented using a 4 byte header
+// containing a the length.
+//
+// The buffer is not thread-safe.
+
+class VariableLengthRingbuffer
+{
+public:
+	/* @brief Constructor
+	 *
+	 * @note Does not allocate automatically.
+	 */
+	VariableLengthRingbuffer() = default;
+
+	/*
+	 * @brief Destructor
+	 *
+	 * Automatically calls deallocate.
+	 */
+	~VariableLengthRingbuffer();
+
+	/* @brief Allocate ringbuffer
+	 *
+	 * @note The variable length requires 4 bytes
+	 * of overhead per packet.
+	 *
+	 * @param buffer_size Number of bytes to allocate on heap.
+	 *
+	 * @returns false if allocation fails.
+	 */
+	bool allocate(size_t buffer_size);
+
+	/*
+	 * @brief Deallocate ringbuffer
+	 *
+	 * @note only required to deallocate and reallocate again.
+	 */
+	void deallocate();
+
+	/*
+	 * @brief Copy packet into ringbuffer
+	 *
+	 * @param packet Pointer to packet to copy from.
+	 * @param packet_len Length of packet.
+	 *
+	 * @returns true if packet could be copied into buffer.
+	 */
+	bool push_back(const uint8_t *packet, size_t packet_len);
+
+	/*
+	 * @brief Get packet from ringbuffer
+	 *
+	 * @note max_buf_len needs to be bigger equal to any pushed packet.
+	 *
+	 * @param buf Pointer to where next packet can be copied into.
+	 * @param max_buf_len Max size of buf
+	 *
+	 * @returns 0 if packet is bigger than max_len or buffer is empty.
+	 */
+	size_t pop_front(uint8_t *buf, size_t max_buf_len);
+
+private:
+	struct Header {
+		uint32_t len;
+	};
+
+	Ringbuffer _ringbuffer {};
+};
@@ -0,0 +1,257 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <stdint.h>
+#include <string.h>
+
+
+#include "VariableLengthRingbuffer.hpp"
+
+class TempData
+{
+public:
+	TempData(size_t len)
+	{
+		_size = len;
+		_buf = new uint8_t[_size];
+	}
+
+	~TempData()
+	{
+		delete[] _buf;
+		_buf = nullptr;
+	}
+
+	uint8_t *buf() const
+	{
+		return _buf;
+	}
+
+	size_t size() const
+	{
+		return _size;
+	}
+
+	void paint(unsigned offset = 0)
+	{
+		for (size_t i = 0; i < _size; ++i) {
+			_buf[i] = (uint8_t)((i + offset) % UINT8_MAX);
+		}
+	}
+
+private:
+	uint8_t *_buf {nullptr};
+	size_t _size{0};
+
+};
+
+bool operator==(const TempData &lhs, const TempData &rhs)
+{
+	if (lhs.size() != rhs.size()) {
+		return false;
+	}
+
+	return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0;
+}
+
+
+TEST(VariableLengthRingbuffer, AllocateAndDeallocate)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+	buf.deallocate();
+
+	ASSERT_TRUE(buf.allocate(1000));
+	// The second time we forget to clean up, but we expect no leak.
+}
+
+TEST(VariableLengthRingbuffer, PushATooBigMessage)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{200};
+
+	// A message that doesn't fit should get rejected.
+	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
+}
+
+TEST(VariableLengthRingbuffer, PushAndPopOne)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{20};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+
+	// Out buffer is the same size
+	TempData out{20};
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20);
+	EXPECT_EQ(data, out);
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+	// Out buffer is supposedly bigger
+	TempData out2{20};
+	EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20);
+	EXPECT_EQ(data, out2);
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+	// Disabled because it doesn't work reliably.
+	// For some reason the abort works when tests are filtered using TESTFILTER
+	// but not when all tests are run.
+	//
+	// Out buffer is too small
+	// Asserts are disabled in release build
+	//TempData out3{19};
+	//EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*");
+}
+
+TEST(VariableLengthRingbuffer, PushAndPopSeveral)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{20};
+	data.paint();
+
+	// 4 should fit
+	for (unsigned i = 0; i < 4; ++i) {
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+	}
+
+	// 5 won't because of overhead
+	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
+
+	// Take 4 back out
+	for (unsigned i = 0; i < 4; ++i) {
+		TempData out{20};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size());
+		EXPECT_EQ(data, out);
+	}
+
+	TempData out{20};
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0);
+}
+
+TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data1{50};
+	data1.paint();
+	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
+
+	TempData data2{30};
+	data2.paint(42);
+	EXPECT_TRUE(buf.push_back(data2.buf(), data2.size()));
+
+	// Supposedly more space
+	TempData out1{50};
+	EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size());
+	EXPECT_EQ(data1, out1);
+
+	TempData data3{50};
+	data3.paint(33);
+	EXPECT_TRUE(buf.push_back(data3.buf(), data3.size()));
+
+	// Supposedly more space
+	TempData out2{30};
+	EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size());
+	EXPECT_EQ(data2, out2);
+
+	// Supposedly more space
+	TempData out3{50};
+	EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size());
+	EXPECT_EQ(data3, out3);
+
+	TempData out4{100};
+	EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0);
+}
+
+TEST(VariableLengthRingbuffer, PushEmpty)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+}
+
+TEST(VariableLengthRingbuffer, PopWithoutBuffer)
+{
+	VariableLengthRingbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+
+	TempData data{50};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+
+	EXPECT_EQ(buf.pop_front(nullptr, 50), 0);
+}
+
+TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader)
+{
+	// Addressing a corner case where start and end are at the end
+	// and the same.
+
+	VariableLengthRingbuffer buf;
+	// Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic.
+	ASSERT_TRUE(buf.allocate(25));
+
+	{
+		TempData data{20};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{20};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+
+	{
+		TempData data{10};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{10};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+}
@@ -229,6 +229,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co

 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
+
+	} else {
+		status.unallocated_torque[0] = 0.f;
 	}

 	if (_saturation_flags.pitch_pos) {
@@ -236,6 +239,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co

 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
+
+	} else {
+		status.unallocated_torque[1] = 0.f;
 	}

 	if (_saturation_flags.yaw_pos) {
@@ -243,6 +249,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co

 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
 	}

 	if (_saturation_flags.thrust_pos) {
@@ -250,5 +259,8 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co

 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
+
+	} else {
+		status.unallocated_thrust[2] = 0.f;
 	}
 }
@@ -37,8 +37,12 @@

 #include "range_finder_consistency_check.hpp"

-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
 {
+	if (horizontal_motion) {
+		_time_last_horizontal_motion = time_us;
+	}
+
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;

 	if ((_time_last_update_us == 0)
@@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va

 void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 {
+	if (fabsf(vz) < _min_vz_for_valid_consistency) {
+		// We can only check consistency if there is vertical motion
+		return;
+	}
+
 	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		_is_kinematically_consistent = false;
-		_time_last_inconsistent_us = time_us;
+		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
+			_is_kinematically_consistent = false;
+			_time_last_inconsistent_us = time_us;
+		}

 	} else {
-		if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if (_test_ratio < 1.f
+		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
 			_is_kinematically_consistent = true;
 		}
 	}
@@ -47,7 +47,7 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;

-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);

 	void setGate(float gate) { _gate = gate; }

@@ -71,6 +71,7 @@ private:

 	bool _is_kinematically_consistent{true};
 	uint64_t _time_last_inconsistent_us{};
+	uint64_t _time_last_horizontal_motion{};

 	static constexpr float _signed_test_ratio_tau = 2.f;

@@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());

-			// Run the kinematic consistency check when not moving horizontally
-			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
-			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
+			if (_control_status.flags.in_air) {
+				const bool horizontal_motion = _control_status.flags.fixed_wing
+								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f));

 				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 				const float var = sq(_params.range_noise) + dist_dependant_var;

 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us);
 			}

 		} else {
@@ -199,7 +199,6 @@ void Ekf::resetHaglRng()
 	_terrain_var = getRngVar();
 	_terrain_vpos_reset_counter++;
 	_time_last_hagl_fuse = _time_delayed_us;
-	_time_last_healthy_rng_data = 0;
 }

 void Ekf::stopHaglRngFusion()
@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -132,7 +132,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.9e-05,-2.3e-06,6.8e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-2.8e-06,6.4e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00021,0.00021,9.3e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13190000,0.7,0.00095,-0.014,0.71,0.00044,0.0063,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-2.8e-06,5.1e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
-13290000,0.7,0.00095,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
+13290000,0.7,0.00096,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0
 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0
 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0
@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0
 34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
 34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
-34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
+34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
 34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
@@ -127,7 +127,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
 				// Stop using distance to ground
 				_terrain_hold = false;
-				_terrain_follow = false;

 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
@@ -144,7 +143,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
 				// Start using distance to ground
 				_terrain_hold = true;
-				_terrain_follow = true;

 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_position_setpoint(2))) {
@@ -155,7 +153,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()

 	}

-	if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
 		// respect maximum altitude
@@ -75,6 +75,7 @@ protected:
 	StickYaw _stick_yaw{this};

 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */

 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
@@ -121,8 +122,6 @@ private:
 	void setGearAccordingToSwitch();

 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
-	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
-	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */

 	float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */
 	float _max_distance_to_ground{(float)INFINITY};  /**< max distance to ground constraint */
@@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
-	_position_setpoint(2) = _smoothing.getCurrentPosition();
+
+	if (!_terrain_hold) {
+		if (_terrain_hold_previous) {
+			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			_smoothing.setCurrentPosition(_position(2));
+		}
+
+		_position_setpoint(2) = _smoothing.getCurrentPosition();
+	}
+
+	_terrain_hold_previous = _terrain_hold;
 }
@@ -67,4 +67,7 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
+
+private:
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };
@@ -171,5 +171,15 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
 	_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
-	_position_setpoint(2) = _smoothing_z.getCurrentPosition();
+
+	if (!_terrain_hold) {
+		if (_terrain_hold_previous) {
+			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			_smoothing_z.setCurrentPosition(_position(2));
+		}
+
+		_position_setpoint(2) = _smoothing_z.getCurrentPosition();
+	}
+
+	_terrain_hold_previous = _terrain_hold;
 }
@@ -87,4 +87,6 @@ private:

 	ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
 	ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
+
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };
@@ -225,7 +225,7 @@ void FwAutotuneAttitudeControl::checkFilters()
 			reset_filters = true;
 		}

-		if (reset_filters) {
+		if (reset_filters || !_are_filters_initialized) {
 			_are_filters_initialized = true;
 			_filter_sample_rate = update_rate_hz;
 			_sys_id.setLpfCutoffFrequency(_filter_sample_rate, _param_imu_gyro_cutoff.get());
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -140,6 +140,7 @@ px4_add_module(
 		mavlink_c
 		timesync
 		tunes
+		variable_length_ringbuffer
 		version
 	UNITY_BUILD
 	)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -178,6 +178,7 @@ Mavlink::~Mavlink()
 	perf_free(_loop_perf);
 	perf_free(_loop_interval_perf);
 	perf_free(_send_byte_error_perf);
+	perf_free(_forwarding_error_perf);
 }

 void
@@ -1218,117 +1219,16 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 	}
 }

-int
-Mavlink::message_buffer_init(int size)
-{
-	_message_buffer.size = size;
-	_message_buffer.write_ptr = 0;
-	_message_buffer.read_ptr = 0;
-	_message_buffer.data = (char *)malloc(_message_buffer.size);
-
-	int ret;
-
-	if (_message_buffer.data == nullptr) {
-		ret = PX4_ERROR;
-		_message_buffer.size = 0;
-
-	} else {
-		ret = OK;
-	}
-
-	return ret;
-}
-
-void
-Mavlink::message_buffer_destroy()
-{
-	_message_buffer.size = 0;
-	_message_buffer.write_ptr = 0;
-	_message_buffer.read_ptr = 0;
-	free(_message_buffer.data);
-}
-
-int
-Mavlink::message_buffer_count()
-{
-	int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
-
-	if (n < 0) {
-		n += _message_buffer.size;
-	}
-
-	return n;
-}
-
-bool
-Mavlink::message_buffer_write(const void *ptr, int size)
-{
-	// bytes available to write
-	int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
-
-	if (available < 0) {
-		available += _message_buffer.size;
-	}
-
-	if (size > available) {
-		// buffer overflow
-		return false;
-	}
-
-	char *c = (char *) ptr;
-	int n = _message_buffer.size - _message_buffer.write_ptr;	// bytes to end of the buffer
-
-	if (n < size) {
-		// message goes over end of the buffer
-		memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
-		_message_buffer.write_ptr = 0;
-
-	} else {
-		n = 0;
-	}
-
-	// now: n = bytes already written
-	int p = size - n;	// number of bytes to write
-	memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
-	_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
-	return true;
-}
-
-int
-Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
-{
-	// bytes available to read
-	int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
-
-	if (available == 0) {
-		return 0;	// buffer is empty
-	}
-
-	int n = 0;
-
-	if (available > 0) {
-		// read pointer is before write pointer, all available bytes can be read
-		n = available;
-		*is_part = false;
-
-	} else {
-		// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
-		n = _message_buffer.size - _message_buffer.read_ptr;
-		*is_part = _message_buffer.write_ptr > 0;
-	}
-
-	*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
-	return n;
-}
-
 void
 Mavlink::pass_message(const mavlink_message_t *msg)
 {
-	/* size is 8 bytes plus variable payload */
+	/* size is 12 bytes plus variable payload */
 	int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
-	pthread_mutex_lock(&_message_buffer_mutex);
-	message_buffer_write(msg, size);
-	pthread_mutex_unlock(&_message_buffer_mutex);
+	LockGuard lg{_message_buffer_mutex};
+
+	if (!_message_buffer.push_back(reinterpret_cast<const uint8_t *>(msg), size)) {
+		perf_count(_forwarding_error_perf);
+	}
 }

 MavlinkShell *
@@ -2211,7 +2111,7 @@ Mavlink::task_main(int argc, char *argv[])
 		return PX4_ERROR;
 	}

-	/* initialize send mutex */
+	pthread_mutex_init(&_message_buffer_mutex, nullptr);
 	pthread_mutex_init(&_send_mutex, nullptr);
 	pthread_mutex_init(&_radio_status_mutex, nullptr);

@@ -2221,13 +2121,12 @@ Mavlink::task_main(int argc, char *argv[])
 		 * make space for two messages plus off-by-one space as we use the empty element
 		 * marker ring buffer approach.
 		 */
-		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
+		LockGuard lg{_message_buffer_mutex};
+
+		if (!_message_buffer.allocate(2 * sizeof(mavlink_message_t) + 1)) {
 			PX4_ERR("msg buf alloc fail");
 			return PX4_ERROR;
 		}
-
-		/* initialize message buffer mutex */
-		pthread_mutex_init(&_message_buffer_mutex, nullptr);
 	}

 	/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
@@ -2569,50 +2468,21 @@ Mavlink::task_main(int argc, char *argv[])

 		_events.update(t);

-		/* pass messages from other UARTs */
+		/* pass messages from other instances */
 		if (get_forwarding_on()) {

-			bool is_part;
-			uint8_t *read_ptr;
-			uint8_t *write_ptr;
-
-			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
-			pthread_mutex_unlock(&_message_buffer_mutex);
-
-			if (available > 0) {
-				// Reconstruct message from buffer
-
-				mavlink_message_t msg;
-				write_ptr = (uint8_t *)&msg;
-
-				// Pull a single message from the buffer
-				size_t read_count = available;
-
-				if (read_count > sizeof(mavlink_message_t)) {
-					read_count = sizeof(mavlink_message_t);
-				}
-
-				memcpy(write_ptr, read_ptr, read_count);
-
-				// We hold the mutex until after we complete the second part of the buffer. If we don't
-				// we may end up breaking the empty slot overflow detection semantics when we mark the
-				// possibly partial read below.
-				pthread_mutex_lock(&_message_buffer_mutex);
-
-				message_buffer_mark_read(read_count);
-
-				/* write second part of buffer if there is some */
-				if (is_part && read_count < sizeof(mavlink_message_t)) {
-					write_ptr += read_count;
-					available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
-					read_count = sizeof(mavlink_message_t) - read_count;
-					memcpy(write_ptr, read_ptr, read_count);
-					message_buffer_mark_read(available);
-				}
-
-				pthread_mutex_unlock(&_message_buffer_mutex);
+			mavlink_message_t msg;
+			size_t available_bytes;
+			{
+				// We only send one message at a time, not to put too much strain on a
+				// link from forwarded messages.
+				LockGuard lg{_message_buffer_mutex};
+				available_bytes = _message_buffer.pop_front(reinterpret_cast<uint8_t *>(&msg), sizeof(msg));
+				// We need to make sure to release the lock here before sending the
+				// bytes out via IP or UART which could potentially take longer.
+			}

+			if (available_bytes > 0) {
 				resend_message(&msg);
 			}
 		}
@@ -2662,11 +2532,6 @@ Mavlink::task_main(int argc, char *argv[])
 		_socket_fd = -1;
 	}

-	if (get_forwarding_on()) {
-		message_buffer_destroy();
-		pthread_mutex_destroy(&_message_buffer_mutex);
-	}
-
 	if (_mavlink_ulog) {
 		_mavlink_ulog->stop();
 		_mavlink_ulog = nullptr;
@@ -2674,6 +2539,7 @@ Mavlink::task_main(int argc, char *argv[])

 	pthread_mutex_destroy(&_send_mutex);
 	pthread_mutex_destroy(&_radio_status_mutex);
+	pthread_mutex_destroy(&_message_buffer_mutex);

 	PX4_INFO("exiting channel %i", (int)_channel);

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -60,6 +60,7 @@

 #include <containers/List.hpp>
 #include <parameters/param.h>
+#include <lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp>
 #include <perf/perf_counter.h>
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/px4_config.h>
@@ -419,11 +420,6 @@ public:
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
 	bool			should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }

-	bool			message_buffer_write(const void *ptr, int size);
-
-	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
-	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
-
 	/**
 	 * Count transmitted bytes
 	 */
@@ -651,16 +647,9 @@ private:

 	ping_statistics_s	_ping_stats {};

-	struct mavlink_message_buffer {
-		int write_ptr;
-		int read_ptr;
-		int size;
-		char *data;
-	};
+	pthread_mutex_t		_message_buffer_mutex{};
+	VariableLengthRingbuffer _message_buffer{};

-	mavlink_message_buffer	_message_buffer {};
-
-	pthread_mutex_t		_message_buffer_mutex {};
 	pthread_mutex_t		_send_mutex {};
 	pthread_mutex_t         _radio_status_mutex {};

@@ -682,6 +671,7 @@ private:
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")};                      /**< loop performance counter */
 	perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")};           /**< loop interval performance counter */
 	perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")};           /**< send bytes error count */
+	perf_counter_t _forwarding_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": forwarding error")};           /**< forwarding messages error count */

 	void			mavlink_update_parameters();

@@ -711,18 +701,6 @@ private:
 	 */
 	int configure_streams_to_default(const char *configure_single_stream = nullptr);

-	int message_buffer_init(int size);
-
-	void message_buffer_destroy();
-
-	int message_buffer_count();
-
-	int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); }
-
-	int message_buffer_get_ptr(void **ptr, bool *is_part);
-
-	void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; }
-
 	void pass_message(const mavlink_message_t *msg);

 	void publish_telemetry_status();
@@ -1587,6 +1587,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 	mavlink_mission_item->frame = mission_item->frame;
 	mavlink_mission_item->command = mission_item->nav_cmd;
 	mavlink_mission_item->autocontinue = mission_item->autocontinue;
+	mavlink_mission_item->mission_type = _mission_type;

 	/* default mappings for generic commands */
 	if (mission_item->frame == MAV_FRAME_MISSION) {
@@ -236,7 +236,7 @@ void McAutotuneAttitudeControl::checkFilters()
 			reset_filters = true;
 		}

-		if (reset_filters && !_are_filters_initialized) {
+		if (reset_filters || !_are_filters_initialized) {
 			_filter_dt = _sample_interval_avg;

 			const float filter_rate_hz = 1.f / _filter_dt;
@@ -694,6 +694,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 		break;

 	case NAV_CMD_TAKEOFF:
+	case NAV_CMD_VTOL_TAKEOFF:

 		// if already flying (armed and !landed) treat TAKEOFF like regular POSITION
 		if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED)
@@ -707,10 +708,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi

 		break;

-	case NAV_CMD_VTOL_TAKEOFF:
-		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
-		break;
-
 	case NAV_CMD_LAND:
 	case NAV_CMD_VTOL_LAND:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
Author	SHA1	Message	Date
Matthias Grob	beb834af2b	fmu-v6x: add crossfire UART driver	2024-01-11 11:04:51 -05:00
MaEtUgR	6eb8d042e1	[AUTO COMMIT] update change indication	2023-12-22 09:46:09 +01:00
Matthias Grob	d09aa8ade5	matrix: fix slice to slice assignment to do deep copy To fix usage of a.xy() = b.xy() which should copy the first two elements over into a and not act on a copy of a.	2023-12-22 09:46:09 +01:00
alexklimaj	b50a9dac84	px4io: change not supported message to INFO instead of ERR	2023-12-12 20:48:28 -05:00
David Sidrane	73fa6e0c52	px4io:Add 'supported' command and uses it in rcS	2023-12-12 20:48:28 -05:00
Matthias Grob	968089bae4	ActuatorEffectivenessHelicopter: explicitly handle unsaturated case This became necessary otherwise the allocation reports saturation all the time and the rate integrator doesn't work.	2023-12-12 14:41:59 -05:00
Silvan Fuhrer	43ba199c37	Navigator: same logic for VTOL_TAKEOFF as for TAKEOFF (#22519 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-11 14:35:56 +01:00
Silvan Fuhrer	a536e3dfe2	TECS: init control params to reasonable values The control params (eg min/max pitch) are used before they are correctly set by TECS::update(). While this is an issue we should fix, it also doesn't hurt to set them to more reasobale values (eg 30° limit). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-08 16:26:10 +01:00
Silvan Fuhrer	5d433ddef7	TECS: make sure to constrain pitch to current min/max pitch Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-08 16:26:10 +01:00
Silvan Fuhrer	5928d7f067	TECS: init to airspeed filter to trim airspeed if airspeed-less Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-08 16:26:10 +01:00
Silvan Fuhrer	740bf63fa7	TECS: set _ratio_underspeed to 0 if airspeed disabled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-08 16:26:10 +01:00
alexklimaj	634ad3893e	flight mode manager: fix terrain hold	2023-11-21 18:12:14 +01:00
bresch	26109a2fe6	ekf2 rng kin: allow check to become true during horizontal motion Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail.	2023-11-21 12:09:11 -05:00
bresch	ff2441d73a	ekf2-terrain: fix validity switching Bug not present after 1.14	2023-11-21 12:09:11 -05:00
Julian Oes	b15e57dd4f	icm45686: fix dt (and usage command) With the wrong dt, the flight behaviour was really poor and noisy, so this fix is absolutely required. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-21 11:08:07 +13:00
bresch	9ab8970206	atune: initialize filter if not already initialized	2023-11-17 09:52:01 +01:00
Julian Oes	c3ed50488f	Remove SYS_USE_IO param The param is not really required anymore with the actuator configuration. Also, when it is set to 0, RC doesn't work for some boards which would be nice to avoid. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-15 06:54:27 +13:00
Matthias Grob	6cdf09644e	PositionSmoothing: fix corner altitude bug During a round corner the L1 distance calculation was only done in 2D and the z-axis coordinate was directly coming from the next waypoint. This lead to an unpredictable altitude profile between two waypoints. Sometimes almost all altitude difference was already covered during the turn instead of going diagonally.	2023-11-10 19:32:21 +01:00
Julian Oes	4138ab0436	gps: update to latest release/1.14 branch This sets the src/drivers/gps/devices submodule to the latest release/1.14 branch. This fixes a potential issue with the Unicore M10 GPS driver, making sure the AGRICA message is requested. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:16:11 +13:00
Julian Oes	e5d92c5195	mavlink: fix MAVLink message forwarding This switches from the horribly intertwined ringbuffer implementation to the new VariableLengthRingbuffer implementation. By ditching the previous implementation, we fix MAVLink message forwarding, which didn't work reliably. The reason it didn't work is that multiple mavlink messages could be added but only one of them was sent out because the buffer didn't keep track of the messages properly and only read the first one. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:15:58 +13:00
Julian Oes	2edc3cf845	lib: add variable length ringbuffer This adds a reusable class for a FIFO ringbuffer that accepts variable length packets. It is using the Ringbuffer class internally. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:15:58 +13:00
Julian Oes	813494bc3d	lib: add FIFO ringbuffer class This adds a reusable class for a simple FIFO ringbuffer. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:15:58 +13:00
Julian Oes	e9a142ac7d	mavlink: properly set mission_type This was defaulted to 0 before which messed with transmitting geofence and rally items. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:15:05 +13:00
alexklimaj	40c9789e7d	boards: arkv6x fix wrong pwm output values	2023-10-31 10:23:57 -04:00