mavlink: rate multiplier and scheduling optimizations

- only update rate multipler if streams, radio status, or tx error rate has changed
 - by default schedule main loop at 3x fastest stream
 - cleanup and optimize mavlink stream update

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -46,7 +46,7 @@ param set-default COM_RC_LOSS_T 3
 
 
 # ekf2
-param set-default EKF2_AID_MASK 35
+param set-default EKF2_AID_MASK 33
 param set-default EKF2_BARO_DELAY 0
 param set-default EKF2_BARO_NOISE 2.0
 

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -149,6 +149,12 @@ then
 	ms5525_airspeed start -X
 fi
 
+# IR-LOCK sensor external I2C
+if param compare -s SENS_EN_IRLOCK 1
+then
+	irlock start -X
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then

Tools/serial/generate_config.py

@@ -268,10 +268,14 @@ for serial_command in serial_commands:
                 default_port_str = port_config['default'][i]
             else:
                 default_port_str = port_config['default']
+
             if default_port_str != "":
                 if default_port_str not in serial_ports:
                     raise Exception("Default Port {:} not found for {:}".format(default_port_str, serial_command['label']))
-                default_port = serial_ports[default_port_str]['index']
+
+                if default_port_str in dict(board_ports).keys():
+                    default_port = serial_ports[default_port_str]['index']
+
 
         commands.append({
             'command': serial_command['command'],

boards/px4/fmu-v5x/default.px4board

@@ -48,6 +48,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -2,7 +2,13 @@
 #
 # PX4 FMUv5X specific board sensors init
 #------------------------------------------------------------------------------
-board_adc start
+
+if param compare -s ADC_ADS1115_EN 1
+then
+	ads1115 start -X
+else
+	board_adc start
+fi
 
 
 if param compare SENS_EN_INA226 1

msg/estimator_status.msg

@@ -45,6 +45,7 @@ uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from ext
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
+uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

msg/estimator_status_flags.msg

@@ -32,6 +32,7 @@ bool cs_ev_vel                # 24 - true when local frame velocity data fusion
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
+bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

platforms/common/include/px4_platform_common/board_common.h

@@ -145,7 +145,10 @@
 #  define BOARD_BATT_I_LIST       {ADC_BATTERY_CURRENT_CHANNEL}
 #  define BOARD_BRICK_VALID_LIST  {BOARD_ADC_BRICK_VALID}
 #elif BOARD_NUMBER_BRICKS == 2
-#  if  !defined(BOARD_NUMBER_DIGITAL_BRICKS)
+#  if  defined(BOARD_NUMBER_DIGITAL_BRICKS)
+#    define BOARD_BATT_V_LIST       {-1, -1}
+#    define BOARD_BATT_I_LIST       {-1, -1}
+#  else
 #    define BOARD_BATT_V_LIST       {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL}
 #    define BOARD_BATT_I_LIST       {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL}
 #  endif

src/drivers/adc/ads1115/ads1115_params.c

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable external ADS1115 ADC
+ *
+ * If enabled, the internal ADC is not used.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(ADC_ADS1115_EN, 0);
+

src/drivers/barometer/ms5837/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2022 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_module(
+	MODULE drivers__barometer__ms5837
+	MAIN ms5837
+	COMPILE_FLAGS
+	SRCS
+		ms5837_main.cpp
+		ms5837_registers.h
+		MS5837.cpp
+		MS5837.hpp
+	DEPENDS
+		drivers_barometer
+		px4_work_queue
+	)

src/drivers/barometer/ms5837/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_MS5837
+	bool "ms5837"
+	default n
+	---help---
+		Enable support for ms5837

src/drivers/barometer/ms5837/MS5837.cpp

@@ -0,0 +1,441 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5837.cpp
+ * Driver for the MS5837 barometric pressure sensor connected via I2C.
+ */
+
+#include "MS5837.hpp"
+
+MS5837::MS5837(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config),
+	_px4_barometer(get_device_id()),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
+{
+}
+
+MS5837::~MS5837()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+}
+
+int MS5837::init()
+{
+
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("I2C::init failed (%i)", ret);
+		return ret;
+	}
+
+	/* do a first measurement cycle to populate reports with valid data */
+	_measure_phase = 0;
+
+	while (true) {
+		/* do temperature first */
+		if (OK != _measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		px4_usleep(MS5837_CONVERSION_INTERVAL);
+
+		if (OK != _collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != _measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		px4_usleep(MS5837_CONVERSION_INTERVAL);
+
+		if (OK != _collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		_px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5837);
+
+		ret = OK;
+
+		break;
+
+	}
+
+	if (ret == 0) {
+		_start();
+	}
+
+	return ret;
+}
+
+int MS5837::_reset()
+{
+	unsigned old_retrycount = _retries;
+	uint8_t	cmd = MS5837_RESET;
+
+	/* bump the retry count */
+	_retries = 3;
+	int result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int MS5837::probe()
+{
+	if ((PX4_OK == _probe_address(MS5837_ADDRESS))) {
+
+		return PX4_OK;
+	}
+
+	_retries = 1;
+
+	return -EIO;
+}
+
+int MS5837::_probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_device_address(address);
+
+	/* send reset command */
+	if (PX4_OK != _reset()) {
+		return -EIO;
+	}
+
+	/* read PROM */
+	if (PX4_OK != _read_prom()) {
+		return -EIO;
+	}
+
+	return PX4_OK;
+}
+
+int MS5837::read(unsigned offset, void *data, unsigned count)
+{
+	union _cvt {
+		uint8_t	b[4];
+		uint32_t w;
+	} *cvt = (_cvt *)data;
+	uint8_t buf[3];
+
+	/* read the most recent measurement */
+	uint8_t cmd = 0;
+	int ret = transfer(&cmd, 1, &buf[0], 3);
+
+	if (ret == PX4_OK) {
+		/* fetch the raw value */
+		cvt->b[0] = buf[2];
+		cvt->b[1] = buf[1];
+		cvt->b[2] = buf[0];
+		cvt->b[3] = 0;
+	}
+
+	return ret;
+}
+
+void MS5837::RunImpl()
+{
+	int ret;
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		ret = _collect();
+
+		if (ret != OK) {
+			if (ret == -6) {
+				/*
+				 * The ms5837 seems to regularly fail to respond to
+				 * its address; this happens often enough that we'd rather not
+				 * spam the console with a message for this.
+				 */
+			} else {
+				//DEVICE_LOG("collection error %d", ret);
+			}
+
+			/* issue a reset command to the sensor */
+			_reset();
+			/* reset the collection state machine and try again - we need
+			 * to wait 2.8 ms after issuing the sensor reset command
+			 * according to the MS5837 datasheet
+			 */
+			ScheduleDelayed(2800);
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+	}
+
+	/* measurement phase */
+	ret = _measure();
+
+	if (ret != OK) {
+		/* issue a reset command to the sensor */
+		_reset();
+		/* reset the collection state machine and try again */
+		_start();
+		return;
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	ScheduleDelayed(MS5837_CONVERSION_INTERVAL);
+}
+
+void MS5837::_start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_measure_phase = 0;
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(MS5837_CONVERSION_INTERVAL);
+}
+
+int MS5837::_measure()
+{
+	perf_begin(_measure_perf);
+
+	/*
+	 * In phase zero, request temperature; in other phases, request pressure.
+	 */
+	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+
+	/*
+	 * Send the command to begin measuring.
+	 */
+	uint8_t cmd = addr;
+	int ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+	}
+
+	_px4_barometer.set_error_count(perf_event_count(_comms_errors));
+
+	perf_end(_measure_perf);
+
+	return ret;
+}
+
+int MS5837::_collect()
+{
+	uint32_t raw;
+
+	perf_begin(_sample_perf);
+
+	/* read the most recent measurement - read offset/size are hardcoded in the interface */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	int ret = read(0, (void *)&raw, 0);
+
+	if (ret < 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	/* handle a measurement */
+	if (_measure_phase == 0) {
+
+		/* temperature offset (in ADC units) */
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+
+		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
+		int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+
+		/* base sensor scale/offset values */
+
+		/* Perform MS5837 Caculation */
+		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
+
+		/* MS5837 temperature compensation */
+		int64_t T2 = 0;
+
+		int64_t f = 0;
+		int64_t OFF2 = 0;
+		int64_t SENS2 = 0;
+
+		if (TEMP < 2000) {
+
+			T2 = 3 * ((int64_t)POW2(dT) >> 33);
+
+			f = POW2((int64_t)TEMP - 2000);
+			OFF2 = 3 * f >> 1;
+			SENS2 = 5 * f >> 3;
+
+			if (TEMP < -1500) {
+
+				int64_t f2 = POW2(TEMP + 1500);
+				OFF2 += 7 * f2;
+				SENS2 += f2 << 2;
+			}
+
+		} else if (TEMP >= 2000) {
+			T2 = 2 * ((int64_t)POW2(dT) >> 37);
+
+			f = POW2((int64_t)TEMP - 2000);
+			OFF2 = 1 * f >> 4;
+			SENS2 = 0;
+		}
+
+		TEMP -= (int32_t)T2;
+		_OFF  -= OFF2;
+		_SENS -= SENS2;
+
+
+		float temperature = TEMP / 100.0f;
+		_px4_barometer.set_temperature(temperature);
+
+	} else {
+		/* pressure calculation, result in Pa */
+		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 13;
+
+		float pressure = P / 10.0f;		/* convert to millibar */
+
+		_px4_barometer.update(timestamp_sample, pressure);
+	}
+
+	/* update the measurement state machine */
+	INCREMENT(_measure_phase, MS5837_MEASUREMENT_RATIO + 1);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+void MS5837::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+
+	printf("pressure:         %f\n", (double)_px4_barometer.get().pressure);
+	printf("temperature:      %f\n", (double)_px4_barometer.get().temperature);
+}
+
+int MS5837::_read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	px4_usleep(3000);
+
+	uint8_t last_val = 0;
+	bool bits_stuck = true;
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 7; i++) {
+		uint8_t cmd = MS5837_PROM_READ + (i * 2);
+
+		if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
+			break;
+		}
+
+		/* check if all bytes are zero */
+		if (i == 0) {
+			/* initialize to first byte read */
+			last_val = prom_buf[0];
+		}
+
+		if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) {
+			bits_stuck = false;
+		}
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return (_crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO;
+}
+
+/**
+ * MS5837 crc4 cribbed from the datasheet
+ */
+bool MS5837::_crc4(uint16_t *n_prom)
+{
+	uint16_t n_rem = 0;
+	uint16_t crcRead = n_prom[0] >> 12;
+	n_prom[0] = ((n_prom[0]) & 0x0FFF);
+	n_prom[7] = 0;
+
+	for (uint8_t i = 0 ; i < 16; i++) {
+		if (i % 2 == 1) {
+			n_rem ^= (uint16_t)((n_prom[i >> 1]) & 0x00FF);
+
+		} else {
+			n_rem ^= (uint16_t)(n_prom[i >> 1] >> 8);
+		}
+
+		for (uint8_t n_bit = 8 ; n_bit > 0 ; n_bit--) {
+			if (n_rem & 0x8000) {
+				n_rem = (n_rem << 1) ^ 0x3000;
+
+			} else {
+				n_rem = (n_rem << 1);
+			}
+		}
+	}
+
+	n_rem = ((n_rem >> 12) & 0x000F);
+
+	return (n_rem ^ 0x00) == crcRead;
+}

src/drivers/barometer/ms5837/MS5837.hpp

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <systemlib/err.h>
+
+#include "ms5837_registers.h"
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
+
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+class MS5837 : public device::I2C, public I2CSPIDriver<MS5837>
+{
+public:
+	MS5837(const I2CSPIDriverConfig &config);
+	~MS5837() override;
+
+	static void 		print_usage();
+
+	int			init();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			RunImpl();
+	void 			print_status() override;
+	int			read(unsigned offset, void *data, unsigned count) override;
+
+private:
+	int			probe() override;
+
+	PX4Barometer		_px4_barometer;
+
+	ms5837::prom_u	   	_prom{};
+
+	bool			_collect_phase{false};
+	unsigned		_measure_phase{false};
+
+	/* intermediate temperature values per MS5611/MS5607 datasheet */
+	int64_t			_OFF{0};
+	int64_t			_SENS{0};
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+
+	/**
+	 * Initialize the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void		_start();
+
+	/**
+	 * Issue a measurement command for the current state.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			_measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			_collect();
+
+	int			_probe_address(uint8_t address);
+
+	/**
+	 * Send a reset command to the MS5837.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int			_reset();
+
+	/**
+	 * Read the MS5837 PROM
+	 *
+	 * @return		PX4_OK if the PROM reads successfully.
+	 */
+	int			_read_prom();
+
+	bool		_crc4(uint16_t *n_prom);
+};

src/drivers/barometer/ms5837/ms5837_main.cpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+
+#include "MS5837.hpp"
+
+void MS5837::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("ms5837", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int ms5837_main(int argc, char *argv[])
+{
+	using ThisDriver = MS5837;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 400000;
+	uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5837;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	cli.i2c_address = MS5837_ADDRESS;
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/barometer/ms5837/ms5837_registers.h

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5837_registers.h
+ *
+ * Shared defines for the ms5837 driver.
+ */
+
+#pragma once
+
+#include <string.h>
+#include "board_config.h"
+
+/* interface ioctls */
+#define IOCTL_RESET		2
+#define IOCTL_MEASURE		3
+
+#define MS5837_ADDRESS		0x76
+
+#define MS5837_RESET  		0x1E
+#define MS5837_ADC_READ  	0x00
+#define MS5837_PROM_READ  	0xA0
+
+#define MS5837_30BA26  		0x1A // Sensor version: From MS5837_30BA datasheet Version PROM Word 0
+
+/*
+ * MS5837 internal constants and data structures.
+ */
+#define ADDR_CMD_CONVERT_D1_OSR256		0x40	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR512		0x42	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR1024		0x44	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR2048		0x46	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR4096		0x48	/* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D2_OSR256		0x50	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR512		0x52	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR1024		0x54	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR2048		0x56	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR4096		0x58	/* write to this address to start temperature conversion */
+
+/*
+  use an OSR of 1024 to reduce the self-heating effect of the
+  sensor. Information from MS tells us that some individual sensors
+  are quite sensitive to this effect and that reducing the OSR can
+  make a big difference
+ */
+#define ADDR_CMD_CONVERT_D1			ADDR_CMD_CONVERT_D1_OSR1024
+#define ADDR_CMD_CONVERT_D2			ADDR_CMD_CONVERT_D2_OSR1024
+
+/*
+ * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update
+ * rate of 100Hz which is be very safe not to read the ADC before the
+ * conversion finished
+ */
+#define MS5837_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5837_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+namespace ms5837
+{
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct prom_s {
+	uint16_t serial_and_crc;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t factory_setup;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union prom_u {
+	uint16_t c[8];
+	prom_s s;
+};
+#pragma pack(pop)
+
+} /* namespace */

src/drivers/drv_sensor.h

@@ -108,6 +108,8 @@
 #define DRV_BARO_DEVTYPE_LPS33HW        0x4C
 #define DRV_BARO_DEVTYPE_TCBP001TA      0x4D
 
+#define DRV_BARO_DEVTYPE_MS5837	0x4E
+
 #define DRV_BARO_DEVTYPE_MPL3115A2	0x51
 #define DRV_ACC_DEVTYPE_FXOS8701C	0x52
 

src/drivers/irlock/parameters.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * IR-LOCK Sensor (external I2C)
+ *
+ * @reboot_required true
+ * @group Sensors
+ * @boolean
+  */
+PARAM_DEFINE_INT32(SENS_EN_IRLOCK, 0);

src/drivers/rc_input/RCInput.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 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
@@ -33,9 +33,9 @@
 
 #include "RCInput.hpp"
 
+#include "crsf_telemetry.h"
 #include <uORB/topics/vehicle_command_ack.h>
 
-#include <poll.h>
 #include <termios.h>
 
 using namespace time_literals;
@@ -44,8 +44,21 @@ constexpr char const *RCInput::RC_SCAN_STRING[];
 
 RCInput::RCInput(const char *device) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
+	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
+	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
 {
+	// rc input, published to ORB
+	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+
+	// initialize it as RC lost
+	_rc_in.rc_lost = true;
+
+	// initialize raw_rc values and count
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		_raw_rc_values[i] = UINT16_MAX;
+	}
+
 	if (device) {
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
@@ -64,7 +77,6 @@ RCInput::~RCInput()
 	delete _ghst_telemetry;
 
 	perf_free(_cycle_perf);
-	perf_free(_cycle_interval_perf);
 	perf_free(_publish_interval_perf);
 }
 
@@ -76,11 +88,6 @@ RCInput::init()
 	RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL
 
-
-#if defined(RC_SERIAL_PORT)
-	_rc_serial_port_output = (strcmp(_device, RC_SERIAL_PORT) != 0);
-#endif // RC_SERIAL_PORT
-
 	// dsm_init sets some file static variables and returns a file descriptor
 	// it also powers on the radio if needed
 	_rcs_fd = dsm_init(_device);
@@ -116,15 +123,15 @@ RCInput::task_spawn(int argc, char *argv[])
 	int myoptind = 1;
 	int ch;
 	const char *myoptarg = nullptr;
-	const char *device = nullptr;
+	const char *device_name = nullptr;
 #if defined(RC_SERIAL_PORT)
-	device = RC_SERIAL_PORT;
+	device_name = RC_SERIAL_PORT;
 #endif // RC_SERIAL_PORT
 
 	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'd':
-			device = myoptarg;
+			device_name = myoptarg;
 			break;
 
 		case '?':
@@ -142,75 +149,111 @@ RCInput::task_spawn(int argc, char *argv[])
 		return -1;
 	}
 
-	if (device == nullptr) {
-		PX4_ERR("valid device required");
-		return PX4_ERROR;
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		RCInput *instance = new RCInput(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleOnInterval(_current_update_interval);
+
+		return PX4_OK;
+
+	} else {
+		if (device_name) {
+			PX4_ERR("invalid device (-d) %s", device_name);
+
+		} else {
+			PX4_INFO("valid device required");
+		}
 	}
 
-	RCInput *instance = new RCInput(device);
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return PX4_ERROR;
-	}
-
-	_object.store(instance);
-	_task_id = task_id_is_work_queue;
-
-	instance->ScheduleOnInterval(_backup_update_interval);
-
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
-void RCInput::FillRssi(input_rc_s &input_rc)
+void
+RCInput::fill_rc_in(uint16_t raw_rc_count_local,
+		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+		    hrt_abstime now, bool frame_drop, bool failsafe,
+		    unsigned frame_drops, int rssi = -1)
 {
-	if (input_rc.rssi < 0 || input_rc.rssi > input_rc_s::RSSI_MAX) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) {
+	// fill rc_in struct for publishing
+	_rc_in.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;
+	}
+
+	unsigned valid_chans = 0;
+
+	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+		_rc_in.values[i] = raw_rc_values_local[i];
+
+		if (raw_rc_values_local[i] != UINT16_MAX) {
+			valid_chans++;
+		}
+
+		// once filled, reset values back to default
+		_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;
+
+	/* 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)) {
 			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 = ((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, (int)input_rc_s::RSSI_MAX);
-
-#if defined(ADC_RC_RSSI_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);
 
 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
 			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
 
-			input_rc.rssi = math::constrain((int)roundf(rssi_analog), 0, (int)input_rc_s::RSSI_MAX);
-#endif // ADC_RC_RSSI_CHANNEL
+			if (rssi_analog > 100.0f) {
+				rssi_analog = 100.0f;
+			}
+
+			if (rssi_analog < 0.0f) {
+				rssi_analog = 0.0f;
+			}
+
+			_rc_in.rssi = rssi_analog;
+
+		} else {
+			_rc_in.rssi = 255;
 		}
-	}
-}
 
-void RCInput::PublishInputRc(input_rc_s &input_rc)
-{
-	FillRssi(input_rc); // requires input_rc.values[]
-
-	input_rc.timestamp = hrt_absolute_time();
-	_input_rc_pub.publish(input_rc);
-
-	perf_count(_publish_interval_perf);
-
-	_last_publish_time = input_rc.timestamp;
-	_rc_scan_locked = true;
-}
-
-void RCInput::set_next_rc_scan_state()
-{
-	int new_state = _rc_scan_state + 1;
-
-	if (new_state >= RC_SCAN::RC_SCAN_MAX) {
-		new_state = 0;
+	} else {
+		_rc_in.rssi = rssi;
 	}
 
-	PX4_DEBUG("RC scan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[new_state]);
+	if (valid_chans == 0) {
+		_rc_in.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;
+}
+
+void RCInput::set_rc_scan_state(RC_SCAN newState)
+{
+	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
 	_rc_scan_begin = 0;
-	_rc_scan_state = static_cast<RC_SCAN>(new_state);
+	_rc_scan_state = newState;
 	_rc_scan_locked = false;
 
 	_report_lock = true;
@@ -252,6 +295,8 @@ void RCInput::Run()
 
 	} else {
 
+		perf_begin(_cycle_perf);
+
 		// Check if parameters have changed
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -269,10 +314,13 @@ void RCInput::Run()
 			}
 		}
 
+		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+
+
 		/* vehicle command */
 		vehicle_command_s vcmd;
 
-		while (_vehicle_cmd_sub.update(&vcmd)) {
+		if (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
 
@@ -329,8 +377,9 @@ void RCInput::Run()
 			if (_adc_report_sub.copy(&adc)) {
 				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
 					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-
-						float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;
+						float adc_volt = adc.raw_data[i] *
+								 adc.v_ref /
+								 adc.resolution;
 
 						if (_analog_rc_rssi_volt < 0.0f) {
 							_analog_rc_rssi_volt = adc_volt;
@@ -349,25 +398,18 @@ void RCInput::Run()
 
 #endif // ADC_RC_RSSI_CHANNEL
 
+		bool rc_updated = false;
 
 		// This block scans for a supported serial RC input and locks onto the first one found
 		// Scan for 300 msec, then switch protocol
-		static constexpr hrt_abstime rc_scan_max = 300_ms;
+		constexpr hrt_abstime rc_scan_max = 300_ms;
 
-		// poll with 1 second timeout
-		pollfd fds[1];
-		fds[0].fd = _rcs_fd;
-		fds[0].events = POLLIN;
-		int ret = poll(fds, 1, 1000);
+		unsigned frame_drops = 0;
 
-		perf_begin(_cycle_perf);
-		perf_count(_cycle_interval_perf);
-
-		if (ret < 0) {
-			PX4_DEBUG("poll error %d", ret);
-		}
-
-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+		// TODO: needs work (poll _rcs_fd)
+		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+		// then update priority to SCHED_PRIORITY_FAST_DRIVER
+		// read all available data from the serial RC input UART
 
 		// read all available data from the serial RC input UART
 		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
@@ -376,48 +418,7 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}
 
-		bool rc_updated = false;
-
 		switch (_rc_scan_state) {
-#if defined(HRT_PPM_CHANNEL)
-
-		case RC_SCAN_PPM:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} 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 != _last_publish_time) && (ppm_decoded_channels >= 4)) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-
-					input_rc_s input_rc{};
-					input_rc.timestamp_last_signal = ppm_last_valid_decode;
-					input_rc.channel_count = math::max(ppm_decoded_channels, (unsigned)input_rc_s::RC_INPUT_MAX_CHANNELS);
-					input_rc.rc_lost = (ppm_decoded_channels == 0);
-					input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					input_rc.rc_ppm_frame_length = ppm_frame_length;
-
-					for (int i = 0; i < input_rc.channel_count; i++) {
-						input_rc.values[i] = ppm_buffer[i];
-					}
-
-					PublishInputRc(input_rc);
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-
-				// Scan the next protocol
-				set_next_rc_scan_state();
-			}
-
-			break;
-#endif  // HRT_PPM_CHANNEL
-
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -429,40 +430,30 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
+				// parse new data
 				if (newBytes > 0) {
-					// parse new data
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
 					bool sbus_failsafe = false;
 					bool sbus_frame_drop = false;
-					unsigned frame_drops = 0;
 
-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe,
+					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
 								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rc_failsafe = sbus_failsafe;
-						input_rc.rc_lost = (raw_rc_count == 0);
-						input_rc.rc_lost_frame_count = frame_drops;
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
-						}
-
-						PublishInputRc(input_rc);
+						_rc_in.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;
 					}
 				}
 
 			} else {
 				// Scan the next protocol
 				rc_io_invert(false);
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_DSM);
 			}
 
 			break;
@@ -477,38 +468,29 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
-				if (newBytes > 0) {
-					// parse new data
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
-					bool dsm_11_bit = false;
-					unsigned frame_drops = 0;
-					int8_t dsm_rssi = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				if (newBytes > 0) {
+					int8_t dsm_rssi = 0;
+					bool dsm_11_bit = false;
+
+					// parse new data
+					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
 							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rc_lost = (raw_rc_count == 0);
-						input_rc.rc_lost_frame_count = frame_drops;
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
-						}
-
-						PublishInputRc(input_rc);
+						_rc_in.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;
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_ST24);
 			}
 
 			break;
@@ -523,43 +505,43 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t st24_rssi = 0;
-					uint8_t lost_count = 0;
-					uint16_t raw_rc_count = 0;
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+					uint8_t st24_rssi, lost_count;
+
+					rc_updated = false;
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						// set updated flag if one complete packet was parsed
+						/* set updated flag if one complete packet was parsed */
 						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &raw_rc_count, raw_rc_values,
-										input_rc_s::RC_INPUT_MAX_CHANNELS));
+						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
+										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
 					}
 
 					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
 					// The only way to detect RC loss is therefore to look at the lost_count.
+
 					if (rc_updated) {
-						// we have a new ST24 frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rssi = st24_rssi;
-						input_rc.rc_lost = (raw_rc_count == 0) || (lost_count > 0);
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
+						if (lost_count == 0) {
+							// we have a new ST24 frame. Publish it.
+							_rc_in.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;
 
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						} else {
+							// if the lost count > 0 means that there is an RC loss
+							_rc_in.rc_lost = true;
 						}
-
-						PublishInputRc(input_rc);
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_SUMD);
 			}
 
 			break;
@@ -574,47 +556,74 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
 				if (newBytes > 0) {
 					// parse new data
-					uint8_t sumd_rssi = 0;
-					uint8_t rx_count = 0;
-					uint16_t raw_rc_count = 0;
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					bool sumd_failsafe = false;
+					uint8_t sumd_rssi, rx_count;
+					bool sumd_failsafe;
+
+					rc_updated = false;
 
 					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						// set updated flag if one complete packet was parsed
+						/* set updated flag if one complete packet was parsed */
 						sumd_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, &raw_rc_count, raw_rc_values,
-										input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
+						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
+										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
 					}
 
 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rssi = sumd_rssi;
-						input_rc.rc_failsafe = sumd_failsafe;
-						input_rc.rc_lost = (raw_rc_count == 0);
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
-						}
-
-						PublishInputRc(input_rc);
+						_rc_in.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;
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_PPM);
 			}
 
 			break;
 
+		case RC_SCAN_PPM:
+			// skip PPM if it's not supported
+#ifdef HRT_PPM_CHANNEL
+			if (_rc_scan_begin == 0) {
+				_rc_scan_begin = cycle_timestamp;
+				// Configure timer input pin for CPPM
+				px4_arch_configgpio(GPIO_PPM_IN);
+
+			} 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) {
+					// we have a new PPM frame. Publish it.
+					rc_updated = true;
+					_rc_in.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;
+				}
+
+			} else {
+				// disable CPPM input by mapping it away from the timer capture input
+				px4_arch_unconfiggpio(GPIO_PPM_IN);
+				// Scan the next protocol
+				set_rc_scan_state(RC_SCAN_CRSF);
+			}
+
+#else   // skip PPM if it's not supported
+			set_rc_scan_state(RC_SCAN_CRSF);
+
+#endif  // HRT_PPM_CHANNEL
+
+			break;
+
 		case RC_SCAN_CRSF:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -625,44 +634,40 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
-				if (newBytes > 0) {
-					// parse new data
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count,
+				// parse new data
+				if (newBytes > 0) {
+					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
 								input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rc_lost = (raw_rc_count == 0);
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
 
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// on Pixhawk (-related) boards we cannot write to the RC UART
+						// another option is to use a different UART port
+#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
+
+						if (!_rc_scan_locked && !_crsf_telemetry) {
+							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
 						}
 
-						PublishInputRc(input_rc);
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
 
-						if (_rc_serial_port_output) {
-							if (!_rc_scan_locked && !_crsf_telemetry) {
-								_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
-							}
+						_rc_scan_locked = true;
 
-							if (_crsf_telemetry) {
-								_crsf_telemetry->update(cycle_timestamp);
-							}
+						if (_crsf_telemetry) {
+							_crsf_telemetry->update(cycle_timestamp);
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_GHST);
 			}
 
 			break;
@@ -677,55 +682,55 @@ void RCInput::Run()
 				tcflush(_rcs_fd, TCIOFLUSH);
 				memset(_rcs_buf, 0, sizeof(_rcs_buf));
 
-			} else if (_rc_scan_locked || (cycle_timestamp - _rc_scan_begin < rc_scan_max)) {
+			} else if (_rc_scan_locked
+				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+
 				// parse new data
 				if (newBytes > 0) {
-					uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
-					uint16_t raw_rc_count = 0;
 					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count,
-								input_rc_s::RC_INPUT_MAX_CHANNELS);
+					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
+								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						input_rc_s input_rc{};
-						input_rc.timestamp_last_signal = cycle_timestamp;
-						input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
-						input_rc.rssi = ghst_rssi;
-						input_rc.rc_lost = (raw_rc_count == 0);
-						input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+						_rc_in.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);
 
-						for (int i = 0; i < input_rc.channel_count; i++) {
-							input_rc.values[i] = raw_rc_values[i];
+						// ghst telemetry works on fmu-v5
+						// on other Pixhawk (-related) boards we cannot write to the RC UART
+						// another option is to use a different UART port
+#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
+
+						if (!_rc_scan_locked && !_ghst_telemetry) {
+							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
 						}
 
-						PublishInputRc(input_rc);
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
 
-						if (_rc_serial_port_output) {
-							if (!_rc_scan_locked && !_ghst_telemetry) {
-								_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-							}
+						_rc_scan_locked = true;
 
-							if (_ghst_telemetry) {
-								_ghst_telemetry->update(cycle_timestamp);
-							}
+						if (_ghst_telemetry) {
+							_ghst_telemetry->update(cycle_timestamp);
 						}
 					}
 				}
 
 			} else {
 				// Scan the next protocol
-				set_next_rc_scan_state();
+				set_rc_scan_state(RC_SCAN_SBUS);
 			}
 
 			break;
-
-		default:
-			// Scan the next protocol
-			set_next_rc_scan_state();
 		}
 
-		if (!rc_updated && !_armed && (hrt_elapsed_time(&_last_publish_time) > 1_s)) {
+		perf_end(_cycle_perf);
+
+		if (rc_updated) {
+			perf_count(_publish_interval_perf);
+
+			_to_input_rc.publish(_rc_in);
+
+		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -733,16 +738,6 @@ void RCInput::Run()
 			_report_lock = false;
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
-
-		// reschedule immediately if RC is locked
-		if (_rc_scan_locked) {
-			ScheduleNow();
-
-		} else {
-			ScheduleDelayed(_backup_update_interval);
-		}
-
-		perf_end(_cycle_perf);
 	}
 }
 
@@ -817,6 +812,8 @@ int RCInput::custom_command(int argc, char *argv[])
 
 int RCInput::print_status()
 {
+	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+
 	if (_device[0] != '\0') {
 		PX4_INFO("UART device: %s", _device);
 		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
@@ -838,23 +835,42 @@ int RCInput::print_status()
 			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
 			break;
 
-		default:
+
+		case RC_SCAN_DSM:
+			// DSM status output
+#if defined(SPEKTRUM_POWER)
+#endif
+			break;
+
+		case RC_SCAN_PPM:
+			// PPM status output
+			break;
+
+		case RC_SCAN_SUMD:
+			// SUMD status output
+			break;
+
+		case RC_SCAN_ST24:
+			// SUMD status output
 			break;
 		}
 	}
 
-#if defined(ADC_RC_RSSI_CHANNEL)
+#if ADC_RC_RSSI_CHANNEL
 
 	if (_analog_rc_rssi_stable) {
 		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
 	}
 
-#endif // ADC_RC_RSSI_CHANNEL
+#endif
 
 	perf_print_counter(_cycle_perf);
-	perf_print_counter(_cycle_interval_perf);
 	perf_print_counter(_publish_interval_perf);
 
+	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _rc_in);
+	}
+
 	return 0;
 }
 

src/drivers/rc_input/RCInput.hpp

@@ -91,31 +91,23 @@ public:
 private:
 
 	enum RC_SCAN {
-#if defined(HRT_PPM_CHANNEL)
-		RC_SCAN_PPM,
-#endif // HRT_PPM_CHANNEL
+		RC_SCAN_PPM = 0,
 		RC_SCAN_SBUS,
 		RC_SCAN_DSM,
 		RC_SCAN_SUMD,
 		RC_SCAN_ST24,
 		RC_SCAN_CRSF,
-		RC_SCAN_GHST,
-
-		RC_SCAN_MAX
+		RC_SCAN_GHST
 	} _rc_scan_state{RC_SCAN_SBUS};
 
-	static constexpr char const *RC_SCAN_STRING[] {
-#if defined(HRT_PPM_CHANNEL)
+	static constexpr char const *RC_SCAN_STRING[7] {
 		"PPM",
-#endif // HRT_PPM_CHANNEL
 		"SBUS",
 		"DSM",
 		"SUMD",
 		"ST24",
 		"CRSF",
-		"GHST",
-
-		"NONE"
+		"GHST"
 	};
 
 	void Run() override;
@@ -124,36 +116,38 @@ private:
 	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER
 
-	void FillRssi(input_rc_s &input_rc);
-	void PublishInputRc(input_rc_s &input_rc);
+	void fill_rc_in(uint16_t raw_rc_count_local,
+			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+			hrt_abstime now, bool frame_drop, bool failsafe,
+			unsigned frame_drops, int rssi);
+
+	void set_rc_scan_state(RC_SCAN _rc_scan_state);
 
 	void rc_io_invert(bool invert);
-	void set_next_rc_scan_state();
 
 	hrt_abstime _rc_scan_begin{0};
-	hrt_abstime _last_publish_time{0};
 
 	bool _initialized{false};
 	bool _rc_scan_locked{false};
 	bool _report_lock{true};
 
-	static constexpr unsigned	_backup_update_interval{100000}; // 10 Hz (backup schedule)
+	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-#if defined(ADC_RC_RSSI_CHANNEL)
-	uORB::Subscription	_adc_report_sub {ORB_ID(adc_report)};
-	float _analog_rc_rssi_volt{-1.0f};
-	bool _analog_rc_rssi_stable{false};
-#endif // ADC_RC_RSSI_CHANNEL
-
+	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	bool _rc_serial_port_output{true};
+	input_rc_s	_rc_in{};
+
+	float		_analog_rc_rssi_volt{-1.0f};
+	bool		_analog_rc_rssi_stable{false};
+
 	bool _armed{false};
 
-	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
+
+	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
@@ -161,13 +155,14 @@ private:
 	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
 	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
 
+	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
+	uint16_t _raw_rc_count{};
+
 	CRSFTelemetry *_crsf_telemetry{nullptr};
 	GHSTTelemetry *_ghst_telemetry{nullptr};
 
-	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
-	perf_counter_t	_cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
-	perf_counter_t	_publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
-
+	perf_counter_t	_cycle_perf;
+	perf_counter_t	_publish_interval_perf;
 	uint32_t	_bytes_rx{0};
 
 	DEFINE_PARAMETERS(

src/drivers/rc_input/module.yaml

@@ -5,7 +5,6 @@ serial_config:
         name: RC_PORT_CONFIG
         group: Serial
         default: RC
-        depends_on_port: RC
         description_extended: |
             Setting this to 'Disabled' will use a board-specific default port
             for RC input.

src/modules/battery_status/analog_battery.cpp

@@ -41,12 +41,11 @@
 static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
 static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
 #else
-static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
-static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+#error  "BOARD_BATT_V_LIST and BOARD_BATT_I_LIST need to be defined"
 #endif
 #else
-static constexpr int DEFAULT_V_CHANNEL[1] = {0};
-static constexpr int DEFAULT_I_CHANNEL[1] = {0};
+static constexpr int DEFAULT_V_CHANNEL[1] = {-1};
+static constexpr int DEFAULT_I_CHANNEL[1] = {-1};
 #endif
 
 AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source,

src/modules/battery_status/battery_status.cpp

@@ -177,6 +177,7 @@ BatteryStatus::adc_poll()
 	/* Per Brick readings with default unread channels at 0 */
 	float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
 	float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
+	bool has_bat_voltage_adc_channel[BOARD_NUMBER_BRICKS] {};
 
 	int selected_source = -1;
 
@@ -201,16 +202,19 @@ BatteryStatus::adc_poll()
 
 				/* look for specific channels and process the raw voltage to measurement data */
 
-				if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
-					/* Voltage in volts */
-					bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
-								      adc_report.v_ref /
-								      adc_report.resolution;
+				if (adc_report.channel_id[i] >= 0) {
+					if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
+						/* Voltage in volts */
+						bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
+									      adc_report.v_ref /
+									      adc_report.resolution;
+						has_bat_voltage_adc_channel[b] = true;
 
-				} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
-					bat_current_adc_readings[b] = adc_report.raw_data[i] *
-								      adc_report.v_ref /
-								      adc_report.resolution;
+					} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
+						bat_current_adc_readings[b] = adc_report.raw_data[i] *
+									      adc_report.v_ref /
+									      adc_report.resolution;
+					}
 				}
 
 			}
@@ -218,11 +222,13 @@ BatteryStatus::adc_poll()
 
 		for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
 
-			_analogBatteries[b]->updateBatteryStatusADC(
-				hrt_absolute_time(),
-				bat_voltage_adc_readings[b],
-				bat_current_adc_readings[b]
-			);
+			if (has_bat_voltage_adc_channel[b]) { // Do not publish if no voltage channel configured
+				_analogBatteries[b]->updateBatteryStatusADC(
+					hrt_absolute_time(),
+					bat_voltage_adc_readings[b],
+					bat_current_adc_readings[b]
+				);
+			}
 		}
 	}
 }

src/modules/ekf2/EKF/common.h

@@ -490,6 +490,7 @@ union filter_control_status_u {
 		uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
 		uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
 		uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
+		uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -134,19 +134,9 @@ void Ekf::controlFusionModes()
 		// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
 		// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
 		// in this case we need to empty the buffer
-		if (!_flow_data_ready || !_control_status.flags.opt_flow) {
+		if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
 			_flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
 		}
-
-		// check if we should fuse flow data for terrain estimation
-		if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
-			// TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion
-			// due to some checks failing
-			// only fuse flow for terrain if range data hasn't been fused for 5 seconds
-			_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
-			// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
-			_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
-		}
 	}
 
 	if (_ext_vision_buffer) {
@@ -420,7 +410,6 @@ void Ekf::controlOpticalFlowFusion()
 		} else {
 			// don't use this flow data and wait for the next data to arrive
 			_flow_data_ready = false;
-			_flow_for_terrain_data_ready = false; // TODO: find a better place
 		}
 	}
 

src/modules/ekf2/EKF/ekf.cpp

@@ -209,8 +209,8 @@ bool Ekf::initialiseFilter()
 		increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
 	}
 
-	// try to initialise the terrain estimator
-	_terrain_initialised = initHagl();
+	// Initialise the terrain estimator
+	initHagl();
 
 	// reset the essential fusion timeout counters
 	_time_last_hgt_fuse = _time_last_imu;

src/modules/ekf2/EKF/ekf.h

@@ -386,6 +386,7 @@ private:
 	uint64_t _time_last_fake_pos_fuse{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
 	Vector2f _last_known_posNE{};		///< last known local NE position vector (m)
@@ -535,8 +536,6 @@ private:
 	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
 	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
-	uint64_t _time_last_fake_hagl_fuse{0};	///< last system time that a fake range sample was fused by the terrain estimator
-	bool _terrain_initialised{false};	///< true when the terrain estimator has been initialized
 	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
@@ -679,21 +678,30 @@ private:
 	bool calcOptFlowBodyRateComp();
 
 	// initialise the terrain vertical position estimator
-	// return true if the initialisation is successful
-	bool initHagl();
+	void initHagl();
 
-	bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); }
-	bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); }
-
-	// run the terrain estimator
 	void runTerrainEstimator();
+	void predictHagl();
 
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
-	void fuseHagl();
+	void controlHaglRngFusion();
+	void fuseHaglRng();
+	void startHaglRngFusion();
+	void resetHaglRngIfNeeded();
+	void resetHaglRng();
+	void stopHaglRngFusion();
+	float getRngVar();
 
 	// update the terrain vertical position estimate using an optical flow measurement
+	void controlHaglFlowFusion();
+	void startHaglFlowFusion();
+	void resetHaglFlow();
+	void stopHaglFlowFusion();
 	void fuseFlowForTerrain();
 
+	void controlHaglFakeFusion();
+	void resetHaglFake();
+
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
 	bool resetMagHeading(bool increase_yaw_var = true, bool update_buffer = true);

src/modules/ekf2/EKF/sensor_range_finder.cpp

@@ -62,7 +62,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 {
 	updateDtDataLpf(current_time_us);
 
-	if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
+	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
 		_is_sample_valid = false;
 		_is_regularly_sending_data = false;
 		return;

src/modules/ekf2/EKF/sensor_range_finder.hpp

@@ -106,6 +106,8 @@ public:
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
+	void setFaulty() { _is_faulty = true; }
+
 private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
 
@@ -123,6 +125,7 @@ private:
 	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
 	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
 	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
+	bool _is_faulty{false};         ///< the sensor should not be used anymore
 
 	/*
 	 * Stuck check

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -43,44 +43,9 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::initHagl()
+void Ekf::initHagl()
 {
-	bool initialized = false;
-
-	if (!_control_status.flags.in_air) {
-		// if on ground, do not trust the range sensor, but assume a ground clearance
-		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-		// use the ground clearance value as our uncertainty
-		_terrain_var = sq(_params.rng_gnd_clearance);
-		_time_last_fake_hagl_fuse = _time_last_imu;
-		initialized = true;
-
-	} else if (shouldUseRangeFinderForHagl()
-		   && _range_sensor.isDataHealthy()) {
-		// if we have a fresh measurement, use it to initialise the terrain estimator
-		_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
-		// initialise state variance to variance of measurement
-		_terrain_var = sq(_params.range_noise);
-		// success
-		initialized = true;
-
-	} else if (shouldUseOpticalFlowForHagl()
-		   && _flow_for_terrain_data_ready) {
-		// initialise terrain vertical position to origin as this is the best guess we have
-		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
-		_terrain_var = 100.0f;
-		initialized = true;
-
-	} else {
-		// no information - cannot initialise
-	}
-
-	if (initialized) {
-		// has initialized with valid data
-		_time_last_hagl_fuse = _time_last_imu;
-	}
-
-	return initialized;
+	resetHaglFake();
 }
 
 void Ekf::runTerrainEstimator()
@@ -88,49 +53,150 @@ void Ekf::runTerrainEstimator()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = _state.pos(2);
+		_control_status.flags.rng_fault = false;
 	}
 
-	// Perform initialisation check and
-	// on ground, continuously reset the terrain estimator
-	if (!_terrain_initialised || !_control_status.flags.in_air) {
-		_terrain_initialised = initHagl();
+	predictHagl();
 
-	} else {
+	controlHaglRngFusion();
+	controlHaglFlowFusion();
+	controlHaglFakeFusion();
 
-		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
-
-		// process noise due to errors in vehicle height estimate
-		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
-
-		// process noise due to terrain gradient
-		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
-				* (sq(_state.vel(0)) + sq(_state.vel(1)));
-
-		// limit the variance to prevent it becoming badly conditioned
-		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
-
-		// Fuse range finder data if available
-		if (shouldUseRangeFinderForHagl()
-		    && _range_sensor.isDataHealthy()) {
-			fuseHagl();
-		}
-
-		if (shouldUseOpticalFlowForHagl()
-		    && _flow_for_terrain_data_ready) {
-			fuseFlowForTerrain();
-			_flow_for_terrain_data_ready = false;
-		}
-
-		// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
-		if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
-			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
-		}
+	// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
+	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
+		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
 	}
 
 	updateTerrainValidity();
 }
 
-void Ekf::fuseHagl()
+void Ekf::predictHagl()
+{
+	// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
+
+	// process noise due to errors in vehicle height estimate
+	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
+
+	// process noise due to terrain gradient
+	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
+			* (sq(_state.vel(0)) + sq(_state.vel(1)));
+
+	// limit the variance to prevent it becoming badly conditioned
+	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
+}
+
+void Ekf::controlHaglRngFusion()
+{
+	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
+	    || _control_status.flags.rng_fault) {
+
+		stopHaglRngFusion();
+		return;
+	}
+
+	if (_range_sensor.isDataHealthy()) {
+		const bool continuing_conditions_passing = _control_status.flags.in_air;
+		//const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height
+		const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData();
+
+		_time_last_healthy_rng_data = _time_last_imu;
+
+		if (_hagl_sensor_status.flags.range_finder) {
+			if (continuing_conditions_passing) {
+				fuseHaglRng();
+
+				// We have been rejecting range data for too long
+				const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
+				const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout);
+
+				if (is_fusion_failing) {
+					if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
+						// Data seems good, attempt a reset
+						resetHaglRng();
+
+					} else if (starting_conditions_passing) {
+						// The sensor can probably not detect the ground properly
+						// declare the sensor faulty and stop the fusion
+						_control_status.flags.rng_fault = true;
+						_range_sensor.setFaulty();
+						stopHaglRngFusion();
+
+					} else {
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						stopHaglRngFusion();
+					}
+				}
+
+			} else {
+				stopHaglRngFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				startHaglRngFusion();
+			}
+		}
+
+	} else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) {
+		// No data anymore. Stop until it comes back.
+		stopHaglRngFusion();
+	}
+}
+
+void Ekf::startHaglRngFusion()
+{
+	_hagl_sensor_status.flags.range_finder = true;
+	resetHaglRngIfNeeded();
+}
+
+void Ekf::resetHaglRngIfNeeded()
+{
+	if (_hagl_sensor_status.flags.flow) {
+		const float meas_hagl = _range_sensor.getDistBottom();
+		const float pred_hagl = _terrain_vpos - _state.pos(2);
+		const float hagl_innov = pred_hagl - meas_hagl;
+		const float obs_variance = getRngVar();
+
+		const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
+
+		const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
+		const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var);
+
+		// Reset the state to the measurement only if the test ratio is large,
+		// otherwise let it converge through the fusion
+		if (hagl_test_ratio > 0.2f) {
+			resetHaglRng();
+
+		} else {
+			fuseHaglRng();
+		}
+
+	} else {
+		resetHaglRng();
+	}
+}
+
+float Ekf::getRngVar()
+{
+	return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
+	       + sq(_params.range_noise)
+	       + sq(_params.range_noise_scaler * _range_sensor.getRange());
+}
+
+void Ekf::resetHaglRng()
+{
+	_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
+	_terrain_var = getRngVar();
+	_terrain_vpos_reset_counter++;
+	_time_last_hagl_fuse = _time_last_imu;
+}
+
+void Ekf::stopHaglRngFusion()
+{
+	_hagl_sensor_status.flags.range_finder = false;
+}
+
+void Ekf::fuseHaglRng()
 {
 	// get a height above ground measurement from the range finder assuming a flat earth
 	const float meas_hagl = _range_sensor.getDistBottom();
@@ -142,9 +208,7 @@ void Ekf::fuseHagl()
 	_hagl_innov = pred_hagl - meas_hagl;
 
 	// calculate the observation variance adding the variance of the vehicles own height uncertainty
-	const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
-				   + sq(_params.range_noise)
-				   + sq(_params.range_noise_scaler * _range_sensor.getRange());
+	const float obs_variance = getRngVar();
 
 	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
@@ -165,20 +229,77 @@ void Ekf::fuseHagl()
 		_innov_check_fail_status.flags.reject_hagl = false;
 
 	} else {
-		// If we have been rejecting range data for too long, reset to measurement
-		const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
+		_innov_check_fail_status.flags.reject_hagl = true;
+	}
+}
 
-		if (isTimedOut(_time_last_hagl_fuse, timeout)) {
-			_terrain_vpos = _state.pos(2) + meas_hagl;
-			_terrain_var = obs_variance;
-			_terrain_vpos_reset_counter++;
+void Ekf::controlHaglFlowFusion()
+{
+	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
+		stopHaglFlowFusion();
+		return;
+	}
+
+	if (_flow_data_ready) {
+		const bool continuing_conditions_passing = _control_status.flags.in_air
+		                                           && !_control_status.flags.opt_flow
+							   && _control_status.flags.gps
+							   && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead?
+							   /* && !_hagl_sensor_status.flags.range_finder; */
+		const bool starting_conditions_passing = continuing_conditions_passing;
+
+		if (_hagl_sensor_status.flags.flow) {
+			if (continuing_conditions_passing) {
+
+				// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
+				fuseFlowForTerrain();
+				_flow_data_ready = false;
+
+				// TODO: do something when failing continuously the innovation check
+				/* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */
+
+				/* if (is_fusion_failing) { */
+				/* 	resetHaglFlow(); */
+				/* } */
+
+			} else {
+				stopHaglFlowFusion();
+			}
 
 		} else {
-			_innov_check_fail_status.flags.reject_hagl = true;
+			if (starting_conditions_passing) {
+				startHaglFlowFusion();
+			}
 		}
+
+	} else if (_hagl_sensor_status.flags.flow
+		   && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)5e6)) {
+		// No data anymore. Stop until it comes back.
+		stopHaglFlowFusion();
 	}
 }
 
+void Ekf::startHaglFlowFusion()
+{
+	_hagl_sensor_status.flags.flow = true;
+	// TODO: do a reset instead of trying to fuse the data?
+	fuseFlowForTerrain();
+	_flow_data_ready = false;
+}
+
+void Ekf::stopHaglFlowFusion()
+{
+	_hagl_sensor_status.flags.flow = false;
+}
+
+void Ekf::resetHaglFlow()
+{
+	// TODO: use the flow data
+	_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
+	_terrain_var = 100.0f;
+	_terrain_vpos_reset_counter++;
+}
+
 void Ekf::fuseFlowForTerrain()
 {
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
@@ -279,6 +400,24 @@ void Ekf::fuseFlowForTerrain()
 	}
 }
 
+void Ekf::controlHaglFakeFusion()
+{
+	if (!_control_status.flags.in_air
+	    && !_hagl_sensor_status.flags.range_finder
+	    && !_hagl_sensor_status.flags.flow) {
+		resetHaglFake();
+	}
+}
+
+void Ekf::resetHaglFake()
+{
+	// assume a ground clearance
+	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+	// use the ground clearance value as our uncertainty
+	_terrain_var = sq(_params.rng_gnd_clearance);
+	_time_last_hagl_fuse = _time_last_imu;
+}
+
 void Ekf::updateTerrainValidity()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
@@ -288,10 +427,5 @@ void Ekf::updateTerrainValidity()
 	// this can only be the case if the main filter does not fuse optical flow
 	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6);
 
-	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
-
-	_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
-			&& recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
-
-	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
+	_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
 }

src/modules/ekf2/EKF2.cpp

@@ -1262,6 +1262,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gps_yaw_fault         = _ekf.control_status_flags().gps_yaw_fault;
+		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/test/test_EKF_terrain_estimator.cpp

@@ -107,25 +107,20 @@ public:
 TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion)
 {
 	// WHEN: simulate being 5m above ground
-	// By default, both rng and flow aiding are active
 	const float simulated_distance_to_ground = 1.f;
-	_sensor_simulator._rng.setData(simulated_distance_to_ground, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 9.f);
-	_sensor_simulator.startRangeFinder();
-	_ekf->set_in_air_status(true);
-	_sensor_simulator.runSeconds(1.5f);
+	runFlowAndRngScenario(simulated_distance_to_ground, simulated_distance_to_ground);
 
 	// THEN: By default, both rng and flow aiding are active
 	EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion());
 	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
-	EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground);
+	EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 1e-4);
 
 	// WHEN: rng fusion is disabled
 	_ekf_wrapper.disableTerrainRngFusion();
-	_sensor_simulator.runSeconds(0.2);
+	_sensor_simulator.runSeconds(5.);
 
-	// THEN: only rng fusion should be disabled
+	// THEN: rng fusion should be disabled and flow fusion should take over
 	EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion());
 

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -449,7 +449,6 @@ bool FlightTaskAuto::_evaluateTriplets()
 	if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
 		_updateInternalWaypoints();
 		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
-		_yaw_lock = false;
 	}
 
 	if (_param_com_obs_avoid.get()
@@ -544,17 +543,17 @@ void FlightTaskAuto::_set_heading_from_mode()
 		// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
 		// radius, lock yaw to current yaw.
 		// This prevents excessive yawing.
-		if (!_yaw_lock) {
-			if (v.longerThan(_target_acceptance_radius)) {
-				_compute_heading_from_2D_vector(_yaw_setpoint, v);
-
-			} else {
-				_yaw_setpoint = _yaw;
+		if (v.longerThan(_target_acceptance_radius)) {
+			if (_compute_heading_from_2D_vector(_yaw_setpoint, v)) {
 				_yaw_lock = true;
-
 			}
 		}
 
+		if (!_yaw_lock) {
+			_yaw_setpoint = _yaw;
+			_yaw_lock = true;
+		}
+
 	} else {
 		_yaw_lock = false;
 		_yaw_setpoint = NAN;
@@ -623,24 +622,26 @@ Vector2f FlightTaskAuto::_getTargetVelocityXY()
 State FlightTaskAuto::_getCurrentState()
 {
 	// Calculate the vehicle current state based on the Navigator triplets and the current position.
-	const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
-	const Vector3f pos_to_target(_triplet_target - _position);
-	const Vector3f prev_to_pos(_position - _triplet_prev_wp);
+	const Vector2f u_prev_to_target_xy = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
+	const Vector2f pos_to_target_xy = Vector2f(_triplet_target - _position);
+	const Vector2f prev_to_pos_xy = Vector2f(_position - _triplet_prev_wp);
 	// Calculate the closest point to the vehicle position on the line prev_wp - target
-	_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);
+	const Vector2f closest_pt_xy = Vector2f(_triplet_prev_wp) + u_prev_to_target_xy * (prev_to_pos_xy *
+				       u_prev_to_target_xy);
+	_closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2));
 
 	State return_state = State::none;
 
-	if (u_prev_to_target * pos_to_target < 0.0f) {
-		// Target is behind.
+	if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) {
+		// Target is behind
 		return_state = State::target_behind;
 
-	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
-		// Current position is more than cruise speed in front of previous setpoint.
+	} else if (u_prev_to_target_xy * prev_to_pos_xy < 0.0f && prev_to_pos_xy.longerThan(_target_acceptance_radius)) {
+		// Previous is in front
 		return_state = State::previous_infront;
 
-	} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
-		// Vehicle is more than cruise speed off track.
+	} else if (Vector2f(_position - _closest_pt).longerThan(_target_acceptance_radius)) {
+		// Vehicle too far from the track
 		return_state = State::offtrack;
 
 	}
@@ -653,8 +654,8 @@ void FlightTaskAuto::_updateInternalWaypoints()
 	// The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp.
 	// The cases where it differs:
 	// 1. The vehicle already passed the target -> go straight to target
-	// 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
-	// 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
+	// 2. Previous waypoint is in front of the vehicle -> go straight to previous waypoint
+	// 3. The vehicle is far from track -> go straight to closest point on track
 	switch (_current_state) {
 	case State::target_behind:
 		_target = _triplet_target;

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit 311eee010bb82f5fb2e4e0f64f7961a83212b003
+Subproject commit 4b0558d0d10efbdd550cb5321d56f6a611d0ab14

src/modules/mavlink/mavlink_events.cpp

@@ -138,7 +138,9 @@ void SendProtocol::update(const hrt_abstime &now)
 
 	while (_latest_sequence != buffer_sequence) {
 		// only send if enough tx buffer space available
-		if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+		if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
+		    || _mavlink.over_data_rate()) {
+
 			break;
 		}
 
@@ -218,7 +220,9 @@ void SendProtocol::on_gcs_connected()
 void SendProtocol::send_current_sequence(const hrt_abstime &now)
 {
 	// only send if enough tx buffer space available
-	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+	if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
+	    || _mavlink.over_data_rate()) {
+
 		return;
 	}
 

src/modules/mavlink/mavlink_ftp.cpp

@@ -1064,7 +1064,8 @@ void MavlinkFTP::send()
 	unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
 	PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf());
 
-	if (max_bytes_to_send < get_size()) {
+	// Skip send if not enough room
+	if ((max_bytes_to_send < get_size()) || _mavlink->over_data_rate()) {
 		return;
 	}
 

src/modules/mavlink/mavlink_main.cpp

@@ -731,25 +731,11 @@ void Mavlink::send_start(int length)
 {
 	pthread_mutex_lock(&_send_mutex);
 	_last_write_try_time = hrt_absolute_time();
-
-	// check if there is space in the buffer
-	if (length > (int)get_free_tx_buf()) {
-		// not enough space in buffer to send
-		count_txerrbytes(length);
-
-		_tstatus.tx_buffer_overruns++;
-
-		// prevent writes
-		_tx_buffer_low = true;
-
-	} else {
-		_tx_buffer_low = false;
-	}
 }
 
 void Mavlink::send_finish()
 {
-	if (_tx_buffer_low || (_buf_fill == 0)) {
+	if (_buf_fill == 0) {
 		pthread_mutex_unlock(&_send_mutex);
 		return;
 	}
@@ -806,6 +792,8 @@ void Mavlink::send_finish()
 		count_txbytes(_buf_fill);
 		_last_write_success_time = _last_write_try_time;
 
+		_over_data_rate.store(_bytes_tx > _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f);
+
 	} else {
 		count_txerrbytes(_buf_fill);
 	}
@@ -817,14 +805,12 @@ void Mavlink::send_finish()
 
 void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
-	if (!_tx_buffer_low) {
-		if (_buf_fill + packet_len < sizeof(_buf)) {
-			memcpy(&_buf[_buf_fill], buf, packet_len);
-			_buf_fill += packet_len;
+	if (_buf_fill + packet_len < sizeof(_buf)) {
+		memcpy(&_buf[_buf_fill], buf, packet_len);
+		_buf_fill += packet_len;
 
-		} else {
-			perf_count(_send_byte_error_perf);
-		}
+	} else {
+		perf_count(_send_byte_error_perf);
 	}
 }
 
@@ -1155,10 +1141,12 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 			if (interval != 0) {
 				/* set new interval */
 				stream->set_interval(interval);
+				_force_rate_mult_update = true;
 
 			} else {
 				/* delete stream */
 				_streams.deleteNode(stream);
+				_force_rate_mult_update = true;
 				return OK; // must finish with loop after node is deleted
 			}
 
@@ -1173,6 +1161,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	if (stream != nullptr) {
 		stream->set_interval(interval);
 		_streams.add(stream);
+		_force_rate_mult_update = true;
 
 		return OK;
 	}
@@ -1364,23 +1353,39 @@ Mavlink::close_shell()
 void
 Mavlink::update_rate_mult()
 {
+	int min_interval = INT32_MAX;
+
 	float const_rate = 0.0f;
 	float rate = 0.0f;
 
 	/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
 	for (const auto &stream : _streams) {
-		if (stream->const_rate()) {
-			const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
+		const int interval = stream->get_interval();
 
-		} else {
-			rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
+		if (interval > 0) {
+			if (stream->const_rate()) {
+				const_rate += stream->get_size_avg() * 1e6f / interval;
+
+			} else {
+				rate += stream->get_size_avg() * 1e6f / interval;
+			}
+
+			if (interval < min_interval) {
+				min_interval = interval;
+			}
 		}
 	}
 
+	// update main loop delay
+	_main_loop_delay = math::constrain(min_interval / 3, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL);
+
 	float mavlink_ulog_streaming_rate_inv = 1.0f;
 
 	if (_mavlink_ulog) {
 		mavlink_ulog_streaming_rate_inv = 1.0f - _mavlink_ulog->current_data_rate();
+
+		// ensure update is at least twice the default logger rate
+		_main_loop_delay = math::min(_main_loop_delay, 3500 / 2); // TODO: poll ulog_stream
 	}
 
 	/* scale up and down as the link permits */
@@ -1416,17 +1421,21 @@ Mavlink::update_rate_mult()
 		hardware_mult *= _radio_status_mult;
 	}
 
+	// reset
+	_radio_status_changed = false;
+
 	pthread_mutex_unlock(&_radio_status_mutex);
 
 	if (log_radio_timeout) {
 		PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
 	}
 
-	/* pick the minimum from bandwidth mult and hardware mult as limit */
-	_rate_mult = fminf(bandwidth_mult, hardware_mult);
+	// pick the minimum from bandwidth mult and hardware mult as limit
+	// ensure the rate multiplier never drops below 5% so that something is always sent
+	float rate_mult = math::constrain(fminf(bandwidth_mult, hardware_mult), 0.05f, 1.0f);
 
-	/* ensure the rate multiplier never drops below 5% so that something is always sent */
-	_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
+	_rate_mult = rate_mult;
+	_rate_div = 1.f / rate_mult;
 }
 
 void
@@ -1438,6 +1447,9 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
 
 	if (_use_software_mav_throttling) {
 
+		const bool radio_status_critical = _radio_status_critical;
+		const float radio_status_mult = _radio_status_mult;
+
 		/* check hardware limits */
 		_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
 
@@ -1456,6 +1468,10 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
 
 		/* Constrain radio status multiplier between 1% and 100% to allow recovery */
 		_radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f);
+
+		if ((radio_status_critical != _radio_status_critical) || (fabsf(radio_status_mult - _radio_status_mult) > 0.01f)) {
+			_radio_status_changed = true;
+		}
 	}
 
 	pthread_mutex_unlock(&_radio_status_mutex);
@@ -1821,6 +1837,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		break;
 	}
 
+	_force_rate_mult_update = true;
+
 	if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) {
 		// stream was not found, assume it is disabled by default
 		return configure_stream(configure_single_stream, 0.0f);
@@ -2190,19 +2208,6 @@ Mavlink::task_main(int argc, char *argv[])
 		PX4_ERR("configure_streams_to_default() failed");
 	}
 
-	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
-
-	/* hard limit to 1000 Hz at max */
-	if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
-		_main_loop_delay = MAVLINK_MIN_INTERVAL;
-	}
-
-	/* hard limit to 100 Hz at least */
-	if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
-		_main_loop_delay = MAVLINK_MAX_INTERVAL;
-	}
-
 	/* open the UART device after setting the instance, as it might block */
 	if (get_protocol() == Protocol::SERIAL) {
 		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
@@ -2234,6 +2239,7 @@ Mavlink::task_main(int argc, char *argv[])
 	uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink
 
 	_mavlink_start_time = hrt_absolute_time();
+	_bytes_timestamp = _mavlink_start_time;
 
 	while (!should_exit()) {
 		/* main loop */
@@ -2249,7 +2255,12 @@ Mavlink::task_main(int argc, char *argv[])
 
 		const hrt_abstime t = hrt_absolute_time();
 
-		update_rate_mult();
+		// update rate multiplier periodically, or immediately if radio status or intervals have changed
+		if (_radio_status_changed || _force_rate_mult_update || (t >= _rate_multi_update_last + 1_s)) {
+			update_rate_mult();
+			_rate_multi_update_last = t;
+			_force_rate_mult_update = false;
+		}
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
@@ -2436,19 +2447,6 @@ Mavlink::task_main(int argc, char *argv[])
 		/* update streams */
 		for (const auto &stream : _streams) {
 			stream->update(t);
-
-			if (!_first_heartbeat_sent) {
-				if (_mode == MAVLINK_MODE_IRIDIUM) {
-					if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
-						_first_heartbeat_sent = stream->first_message_sent();
-					}
-
-				} else {
-					if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
-						_first_heartbeat_sent = stream->first_message_sent();
-					}
-				}
-			}
 		}
 
 		/* check for ulog streaming messages */
@@ -2551,21 +2549,25 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		/* update TX/RX rates*/
-		if (t > _bytes_timestamp + 1_s) {
-			if (_bytes_timestamp != 0) {
-				const float dt = (t - _bytes_timestamp) * 1e-6f;
+		// update TX/RX rates
+		if (t >= _bytes_timestamp + 1_s) {
+			pthread_mutex_lock(&_send_mutex);
 
-				_tstatus.tx_rate_avg = _bytes_tx / dt;
-				_tstatus.tx_error_rate_avg = _bytes_txerr / dt;
-				_tstatus.rx_rate_avg = _bytes_rx / dt;
+			const float dt_inv = 1e6f / (t - _bytes_timestamp);
 
-				_bytes_tx = 0;
-				_bytes_txerr = 0;
-				_bytes_rx = 0;
-			}
+			_tstatus.tx_rate_avg = _bytes_tx * dt_inv;
+			_tstatus.tx_error_rate_avg = _bytes_txerr * dt_inv;
+			_tstatus.rx_rate_avg = _bytes_rx * dt_inv;
 
-			_bytes_timestamp = t;
+			_bytes_tx = 0;
+			_bytes_txerr = 0;
+			_bytes_rx = 0;
+
+			_bytes_timestamp = _last_write_try_time;
+
+			_force_rate_mult_update = true;
+
+			pthread_mutex_unlock(&_send_mutex);
 		}
 
 		// publish status at 1 Hz, or sooner if HEARTBEAT has updated

src/modules/mavlink/mavlink_main.h

@@ -405,6 +405,7 @@ public:
 	List<MavlinkStream *> &get_streams() { return _streams; }
 
 	float			get_rate_mult() const { return _rate_mult; }
+	float			get_rate_div() const { return _rate_div; }
 
 	float			get_baudrate() { return _baudrate; }
 
@@ -415,6 +416,19 @@ 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 canTransmit(size_t size_bytes)
+	{
+		if (size_bytes <= get_free_tx_buf()) {
+			if ((_bytes_tx + size_bytes) <= _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	bool 			over_data_rate() { return _over_data_rate.load(); }
+
 	bool			message_buffer_write(const void *ptr, int size);
 
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
@@ -519,7 +533,9 @@ public:
 
 	static hrt_abstime &get_first_start_time() { return _first_start_time; }
 
-	bool radio_status_critical() const { return _radio_status_critical; }
+	bool radio_status_critical() const { return _radio_status_available && _radio_status_critical; }
+
+	void set_first_heartbeat_sent() { _first_heartbeat_sent = true; }
 
 private:
 	MavlinkReceiver 	_receiver;
@@ -548,9 +564,8 @@ private:
 
 	static bool		_boot_complete;
 
-	static constexpr int	MAVLINK_MIN_INTERVAL{1500};
-	static constexpr int	MAVLINK_MAX_INTERVAL{10000};
-	static constexpr float	MAVLINK_MIN_MULTIPLIER{0.0005f};
+	static constexpr int	MAVLINK_MIN_INTERVAL{1000};  // 1000 Hz
+	static constexpr int	MAVLINK_MAX_INTERVAL{20000}; //   50 Hz
 
 	mavlink_message_t	_mavlink_buffer {};
 	mavlink_status_t	_mavlink_status {};
@@ -563,7 +578,7 @@ private:
 
 	px4::atomic_bool	_should_check_events{false};    /**< Events subscription: only one MAVLink instance should check */
 
-	unsigned		_main_loop_delay{1000};	/**< mainloop delay, depends on data rate */
+	int		_main_loop_delay{MAVLINK_MAX_INTERVAL};	/**< mainloop delay, depends on data rate */
 
 	List<MavlinkStream *>		_streams;
 
@@ -585,9 +600,11 @@ private:
 	int			_baudrate{57600};
 	int			_datarate{1000};		///< data rate for normal streams (attitude, position, etc.)
 	float			_rate_mult{1.0f};
+	float			_rate_div{1.0f};
 
 	bool			_radio_status_available{false};
 	bool			_radio_status_critical{false};
+	bool			_radio_status_changed{false};
 	float			_radio_status_mult{1.0f};
 
 	/**
@@ -616,6 +633,11 @@ private:
 	unsigned		_bytes_rx{0};
 	hrt_abstime		_bytes_timestamp{0};
 
+	px4::atomic_bool        _over_data_rate{false};
+
+	hrt_abstime             _rate_multi_update_last{0};
+	bool			_force_rate_mult_update{true};
+
 #if defined(MAVLINK_UDP)
 	BROADCAST_MODE		_mav_broadcast {BROADCAST_MODE_OFF};
 
@@ -635,8 +657,6 @@ private:
 	uint8_t			_buf[MAVLINK_MAX_PACKET_LEN] {};
 	unsigned		_buf_fill{0};
 
-	bool			_tx_buffer_low{false};
-
 	const char 		*_interface_name{nullptr};
 
 	int			_socket_fd{-1};

src/modules/mavlink/mavlink_mission.cpp

@@ -466,12 +466,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send()
 {
-	// do not send anything over high latency communication
-	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
-		return;
-	}
-
-	mission_result_s mission_result{};
+	mission_result_s mission_result;
 
 	if (_mission_result_sub.update(&mission_result)) {
 
@@ -1744,11 +1739,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 void MavlinkMissionManager::check_active_mission()
 {
-	// do not send anything over high latency communication
-	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
-		return;
-	}
-
 	if (!(_my_dataman_id == _dataman_id)) {
 		PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
 

src/modules/mavlink/mavlink_parameters.cpp

@@ -66,6 +66,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			mavlink_param_request_list_t req_list;
 			mavlink_msg_param_request_list_decode(msg, &req_list);
 
+			PX4_DEBUG("PARAM_REQUEST_LIST target_system: %d, target_component: %d", req_list.target_system,
+				  req_list.target_component);
+
 			if (req_list.target_system == mavlink_system.sysid &&
 			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				if (_send_all_index < 0) {
@@ -96,6 +99,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			mavlink_param_set_t set;
 			mavlink_msg_param_set_decode(msg, &set);
 
+			PX4_DEBUG("PARAM_SET param_id:%s", set.param_id);
+
 			if (set.target_system == mavlink_system.sysid &&
 			    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 
@@ -162,6 +167,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 		}
 
 	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			PX4_DEBUG("PARAM_REQUEST_READ");
 			/* request one parameter */
 			mavlink_param_request_read_t req_read;
 			mavlink_msg_param_request_read_decode(msg, &req_read);
@@ -183,6 +189,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 						param_value.param_type = MAV_PARAM_TYPE_UINT32;
 						memcpy(&param_value.param_value, &hash, sizeof(hash));
+
+						PX4_DEBUG("sending PARAM_REQUEST_READ param_id:'%s', param_index:%d", param_value.param_id, param_value.param_index);
 						mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
 
 					} else {
@@ -228,6 +236,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 		}
 
 	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+			PX4_DEBUG("PARAM_MAP_RC");
 			/* map a rc channel to a parameter */
 			mavlink_param_map_rc_t map_rc;
 			mavlink_msg_param_map_rc_decode(msg, &map_rc);
@@ -278,6 +287,10 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send()
 {
+	if (_mavlink->over_data_rate() || _mavlink->radio_status_critical()) {
+		return;
+	}
+
 	if (!_first_send) {
 		// parameters QGC can't tolerate not finding (2020-11-11)
 		param_find("BAT_CRIT_THR");
@@ -308,21 +321,32 @@ MavlinkParametersManager::send()
 		_first_send = true;
 	}
 
-	int max_num_to_send;
+	int max_num_to_send = 3;
 
-	if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) {
-		max_num_to_send = 3;
-
-	} else {
-		// speed up parameter loading via UDP or USB: try to send 20 at once
-		max_num_to_send = 20;
+	if ((_mavlink->get_protocol() != Protocol::SERIAL)
+	    || _mavlink->is_usb_uart()
+	    || ((_mavlink->get_protocol() == Protocol::SERIAL) && _mavlink->get_flow_control_enabled())
+	   ) {
+		// speed up parameter loading via UDP, USB, or serial with working flow control: try to send up to 30 at once (respecting max data rate)
+		max_num_to_send = 30;
 	}
 
-	int i = 0;
+	for (int i = 0; i < max_num_to_send; i++) {
+		// Send while burst is not exceeded, we still have buffer space and still something to send
+		bool transmit = _mavlink->get_free_tx_buf() >= get_size() && !_mavlink->radio_status_critical()
+				&& !_mavlink->over_data_rate();
 
-	// Send while burst is not exceeded, we still have buffer space and still something to send
-	while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
-	       && send_params()) {}
+		if (transmit) {
+			if (!send_params()) {
+				return;
+			}
+
+		} else {
+			PX4_DEBUG("MavlinkParametersManager::send() can't transmit, free TX buf: %d, over data rate: %d",
+				  _mavlink->get_free_tx_buf(), _mavlink->over_data_rate());
+			return;
+		}
+	}
 }
 
 bool
@@ -382,6 +406,7 @@ MavlinkParametersManager::send_untransmitted()
 				}
 			}
 		} while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
+			 && !_mavlink->over_data_rate()
 			 && (_param_update_index < (int) param_count()));
 
 		// Flag work as done once all params have been sent
@@ -397,7 +422,7 @@ bool
 MavlinkParametersManager::send_uavcan()
 {
 	/* Send parameter values received from the UAVCAN topic */
-	uavcan_parameter_value_s value{};
+	uavcan_parameter_value_s value;
 
 	if (_uavcan_parameter_value_sub.update(&value)) {
 
@@ -470,6 +495,8 @@ MavlinkParametersManager::send_one()
 			strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 			msg.param_type = MAV_PARAM_TYPE_UINT32;
 			memcpy(&msg.param_value, &hash, sizeof(hash));
+
+			PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d", msg.param_id, msg.param_index);
 			mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 			/* after this we should start sending all params */
@@ -511,12 +538,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 		return 1;
 	}
 
-	/* no free TX buf to send this param */
-	if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
-		return 1;
-	}
-
-	mavlink_param_value_t msg;
+	mavlink_param_value_t msg{};
 
 	/*
 	 * get param value, since MAVLink encodes float and int params in the same
@@ -579,6 +601,8 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 
 	/* default component ID */
 	if (component_id < 0) {
+		PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d, param_count:%d", msg.param_id, msg.param_index,
+			  msg.param_count);
 		mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 	} else {

src/modules/mavlink/mavlink_receiver.cpp

@@ -3085,9 +3085,6 @@ MavlinkReceiver::run()
 		px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
 	}
 
-	// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
-	const int timeout = 10;
-
 #if defined(__PX4_POSIX)
 	/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
 	uint8_t buf[1600 * 5];
@@ -3133,7 +3130,10 @@ MavlinkReceiver::run()
 			updateParams();
 		}
 
-		int ret = poll(&fds[0], 1, timeout);
+		// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
+		const int timeout_ms = 10;
+
+		int ret = poll(&fds[0], 1, timeout_ms);
 
 		if (ret > 0) {
 			if (_mavlink->get_protocol() == Protocol::SERIAL) {
@@ -3268,17 +3268,24 @@ MavlinkReceiver::run()
 
 		CheckHeartbeats(t);
 
-		if (t - last_send_update > timeout * 1000) {
-			_mission_manager.check_active_mission();
-			_mission_manager.send();
+		if (t - last_send_update > timeout_ms * 1000) {
 
-			_parameters_manager.send();
+			// do not send anything over high latency communication
+			if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
+
+				_mission_manager.check_active_mission();
+				_mission_manager.send();
+
+				_parameters_manager.send();
+
+				if (_mavlink->ftp_enabled()) {
+					_mavlink_ftp.send();
+				}
+
+				_mavlink_log_handler.send();
 
-			if (_mavlink->ftp_enabled()) {
-				_mavlink_ftp.send();
 			}
 
-			_mavlink_log_handler.send();
 			last_send_update = t;
 		}
 

src/modules/mavlink/mavlink_stream.cpp

@@ -47,7 +47,6 @@
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	_mavlink(mavlink)
 {
-	_last_sent = hrt_absolute_time();
 }
 
 /**
@@ -58,71 +57,38 @@ MavlinkStream::update(const hrt_abstime &t)
 {
 	update_data();
 
-	// If the message has never been sent before we want
-	// to send it immediately and can return right away
-	if (_last_sent == 0) {
-		// this will give different messages on the same run a different
-		// initial timestamp which will help spacing them out
-		// on the link scheduling
-		if (send()) {
-			_last_sent = hrt_absolute_time();
-
-			if (!_first_message_sent) {
-				_first_message_sent = true;
-			}
-		}
+	int sz = get_size();
 
+	if ((sz <= 0) || (_interval == 0)) {
+		// We don't need to send anything if the size or interval is 0.
 		return 0;
-	}
 
-	// One of the previous iterations sent the update
-	// already before the deadline
-	if (_last_sent > t) {
-		return -1;
-	}
-
-	int64_t dt = t - _last_sent;
-	int interval = _interval;
-
-	if (!const_rate()) {
-		interval /= _mavlink->get_rate_mult();
-	}
-
-	// We don't need to send anything if the inverval is 0. send() will be called manually.
-	if (interval == 0) {
-		return 0;
-	}
-
-	const bool unlimited_rate = interval < 0;
-
-	// Send the message if it is due or
-	// if it will overrun the next scheduled send interval
-	// by 30% of the interval time. This helps to avoid
-	// sending a scheduled message on average slower than
-	// scheduled. Doing this at 50% would risk sending
-	// the message too often as the loop runtime of the app
-	// needs to be accounted for as well.
-	// This method is not theoretically optimal but a suitable
-	// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
-
-	if (unlimited_rate || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 3))) {
-		// interval expired, send message
-
-		// If the interval is non-zero and dt is smaller than 1.5 times the interval
-		// do not use the actual time but increment at a fixed rate, so that processing delays do not
-		// distort the average rate. The check of the maximum interval is done to ensure that after a
-		// long time not sending anything, sending multiple messages in a short time is avoided.
-		if (send()) {
-			_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
-
-			if (!_first_message_sent) {
-				_first_message_sent = true;
+	} else if ((_interval < 0) || (_last_sent == 0)) {
+		// if unlimited rate or message has never been sent before
+		// we want to send it immediately and can return right away
+		if (_mavlink->canTransmit(sz)) {
+			if (send()) {
+				_last_sent = t;
+				return 0;
 			}
 
-			return 0;
-
 		} else {
-			return -1;
+			_mavlink->count_txerrbytes(sz);
+		}
+
+	} else {
+		int interval = const_rate() ? _interval : (_interval * _mavlink->get_rate_div());
+
+		if (t >= _last_sent + interval) {
+			if (_mavlink->canTransmit(sz)) {
+				if (send()) {
+					_last_sent = math::max(_last_sent + interval, t - interval);
+					return 0;
+				}
+
+			} else {
+				_mavlink->count_txerrbytes(sz);
+			}
 		}
 	}
 

src/modules/mavlink/mavlink_stream.h

@@ -114,13 +114,7 @@ public:
 	/**
 	 * @return true if the first message of this stream has been sent
 	 */
-	bool first_message_sent() const { return _first_message_sent; }
-
-	/**
-	 * Reset the time of last sent to 0. Can be used if a message over this
-	 * stream needs to be sent immediately.
-	 */
-	void reset_last_sent() { _last_sent = 0; }
+	bool first_message_sent() const { return _last_sent != 0; }
 
 protected:
 	Mavlink      *const _mavlink;
@@ -138,7 +132,6 @@ protected:
 
 private:
 	hrt_abstime _last_sent{0};
-	bool _first_message_sent{false};
 };
 
 

src/modules/mavlink/streams/ADSB_VEHICLE.hpp

@@ -61,14 +61,12 @@ private:
 
 	bool send() override
 	{
-		bool sent = false;
-
 		transponder_report_s pos;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _transponder_report_sub.update(&pos)) {
+		if (_transponder_report_sub.update(&pos)) {
 
 			if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) {
-				continue;
+				return false;
 			}
 
 			mavlink_adsb_vehicle_t msg{};
@@ -100,10 +98,10 @@ private:
 			if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) { msg.flags |= ADSB_FLAGS_VALID_SQUAWK; }
 
 			mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
-			sent = true;
+			return true;
 		}
 
-		return sent;
+		return false;
 	}
 };
 

src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp

@@ -63,7 +63,7 @@ private:
 	{
 		camera_capture_s capture;
 
-		if ((_mavlink->get_free_tx_buf() >= get_size()) && _capture_sub.update(&capture)) {
+		if (_capture_sub.update(&capture)) {
 			mavlink_camera_image_captured_t msg{};
 
 			msg.time_boot_ms = capture.timestamp / 1000;

src/modules/mavlink/streams/CAMERA_TRIGGER.hpp

@@ -76,7 +76,7 @@ private:
 	{
 		camera_trigger_s camera_trigger;
 
-		if ((_mavlink->get_free_tx_buf() >= get_size()) && _camera_trigger_sub.update(&camera_trigger)) {
+		if (_camera_trigger_sub.update(&camera_trigger)) {
 			/* ensure that only active trigger events are sent and ignore camera capture feedback messages*/
 			if (camera_trigger.timestamp > 0 && !camera_trigger.feedback) {
 				mavlink_camera_trigger_t msg{};

src/modules/mavlink/streams/COLLISION.hpp

@@ -62,7 +62,7 @@ private:
 		collision_report_s report;
 		bool sent = false;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _collision_sub.update(&report)) {
+		if (_collision_sub.update(&report)) {
 			mavlink_collision_t msg = {};
 
 			msg.src = report.src;

src/modules/mavlink/streams/COMMAND_LONG.hpp

@@ -60,38 +60,32 @@ private:
 	bool send() override
 	{
 		bool sent = false;
+		const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+		vehicle_command_s cmd;
 
-		static constexpr size_t COMMAND_LONG_SIZE = MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		if (_vehicle_command_sub.update(&cmd)) {
+			if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
+					_vehicle_command_sub.get_last_generation());
+			}
 
-		while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) && _vehicle_command_sub.updated()) {
+			// mavlink mavlink commands are <= UINT16_MAX
+			const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
 
-			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
-			vehicle_command_s cmd;
+			// internal commands
+			const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
+							    && (cmd.target_component == _mavlink->get_component_id())
+							    && (cmd.source_system == cmd.target_system)
+							    && (cmd.source_component == cmd.target_component);
 
-			if (_vehicle_command_sub.update(&cmd)) {
-				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
-					PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
-						_vehicle_command_sub.get_last_generation());
-				}
+			if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
+				PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
 
-				// mavlink mavlink commands are <= UINT16_MAX
-				const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
+				MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
+				sent = true;
 
-				// internal commands
-				const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
-								    && (cmd.target_component == _mavlink->get_component_id())
-								    && (cmd.source_system == cmd.target_system)
-								    && (cmd.source_component == cmd.target_component);
-
-				if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
-					PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
-
-					MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
-					sent = true;
-
-				} else {
-					PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
-				}
+			} else {
+				PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
 			}
 		}
 

src/modules/mavlink/streams/GPS_RTCM_DATA.hpp

@@ -60,9 +60,8 @@ private:
 	bool send() override
 	{
 		gps_inject_data_s gps_inject_data;
-		bool sent = false;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
+		if (_gps_inject_data_sub.update(&gps_inject_data)) {
 			mavlink_gps_rtcm_data_t msg{};
 
 			msg.len = gps_inject_data.len;
@@ -71,10 +70,10 @@ private:
 
 			mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
 
-			sent = true;
+			return true;
 		}
 
-		return sent;
+		return false;
 	}
 };
 

src/modules/mavlink/streams/HEARTBEAT.hpp

@@ -67,85 +67,83 @@ private:
 
 	bool send() override
 	{
-		if (_mavlink->get_free_tx_buf() >= get_size()) {
-			// always send the heartbeat, independent of the update status of the topics
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
+		// always send the heartbeat, independent of the update status of the topics
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
 
-			vehicle_status_flags_s vehicle_status_flags{};
-			_vehicle_status_flags_sub.copy(&vehicle_status_flags);
+		vehicle_status_flags_s vehicle_status_flags{};
+		_vehicle_status_flags_sub.copy(&vehicle_status_flags);
 
-			vehicle_control_mode_s vehicle_control_mode{};
-			_vehicle_control_mode_sub.copy(&vehicle_control_mode);
+		vehicle_control_mode_s vehicle_control_mode{};
+		_vehicle_control_mode_sub.copy(&vehicle_control_mode);
 
-			actuator_armed_s actuator_armed{};
-			_acturator_armed_sub.copy(&actuator_armed);
+		actuator_armed_s actuator_armed{};
+		_acturator_armed_sub.copy(&actuator_armed);
 
-			// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
-			uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+		// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
+		uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 
-			if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-				base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-			}
-
-			if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-				base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_manual_enabled) {
-				base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_attitude_enabled) {
-				base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_auto_enabled) {
-				base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			}
-
-
-			// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
-			union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
-
-
-			// uint8_t system_status (MAV_STATE) - System status flag.
-			uint8_t system_status = MAV_STATE_UNINIT;
-
-			switch (vehicle_status.arming_state) {
-			case vehicle_status_s::ARMING_STATE_ARMED:
-				system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
-				break;
-
-			case vehicle_status_s::ARMING_STATE_STANDBY:
-				system_status = MAV_STATE_STANDBY;
-				break;
-
-			case vehicle_status_s::ARMING_STATE_SHUTDOWN:
-				system_status = MAV_STATE_POWEROFF;
-				break;
-			}
-
-			// system_status overrides
-			if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
-			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
-				system_status = MAV_STATE_FLIGHT_TERMINATION;
-
-			} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
-				system_status = MAV_STATE_EMERGENCY;
-
-			} else if (vehicle_status_flags.condition_calibration_enabled) {
-				system_status = MAV_STATE_CALIBRATING;
-			}
-
-
-			mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
-						   base_mode, custom_mode.data, system_status);
-
-			return true;
+		if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 		}
 
-		return false;
+		if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_manual_enabled) {
+			base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_attitude_enabled) {
+			base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_auto_enabled) {
+			base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		}
+
+
+		// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
+		union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
+
+
+		// uint8_t system_status (MAV_STATE) - System status flag.
+		uint8_t system_status = MAV_STATE_UNINIT;
+
+		switch (vehicle_status.arming_state) {
+		case vehicle_status_s::ARMING_STATE_ARMED:
+			system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
+			break;
+
+		case vehicle_status_s::ARMING_STATE_STANDBY:
+			system_status = MAV_STATE_STANDBY;
+			break;
+
+		case vehicle_status_s::ARMING_STATE_SHUTDOWN:
+			system_status = MAV_STATE_POWEROFF;
+			break;
+		}
+
+		// system_status overrides
+		if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
+		    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
+			system_status = MAV_STATE_FLIGHT_TERMINATION;
+
+		} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
+			system_status = MAV_STATE_EMERGENCY;
+
+		} else if (vehicle_status_flags.condition_calibration_enabled) {
+			system_status = MAV_STATE_CALIBRATING;
+		}
+
+
+		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
+					   base_mode, custom_mode.data, system_status);
+
+		_mavlink->set_first_heartbeat_sent();
+
+		return true;
 	}
 };
 

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -87,7 +87,6 @@ private:
 		_throttle(SimpleAnalyzer::AVERAGE),
 		_windspeed(SimpleAnalyzer::AVERAGE)
 	{
-		reset_last_sent();
 	}
 
 	struct PerBatteryData {
@@ -203,6 +202,10 @@ private:
 				mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg);
 			}
 
+			if (updated) {
+				_mavlink->set_first_heartbeat_sent();
+			}
+
 			return updated;
 
 		} else {

src/modules/mavlink/streams/LINK_NODE_STATUS.hpp

@@ -55,29 +55,25 @@ private:
 
 	bool send() override
 	{
-		if (_mavlink->get_free_tx_buf() >= get_size()) {
-			mavlink_link_node_status_t link_node_status{};
+		mavlink_link_node_status_t link_node_status{};
 
-			const telemetry_status_s &tstatus = _mavlink->telemetry_status();
-			link_node_status.tx_buf = 0; // % TODO
-			link_node_status.rx_buf = 0; // % TODO
-			link_node_status.tx_rate = tstatus.tx_rate_avg;
-			link_node_status.rx_rate = tstatus.rx_rate_avg;
-			link_node_status.rx_parse_err = tstatus.rx_parse_errors;
-			link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
-			link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
-			link_node_status.messages_sent = tstatus.tx_message_count;
-			link_node_status.messages_received = tstatus.rx_message_count;
-			link_node_status.messages_lost = tstatus.rx_message_lost_count;
+		const telemetry_status_s &tstatus = _mavlink->telemetry_status();
+		link_node_status.tx_buf = 0; // % TODO
+		link_node_status.rx_buf = 0; // % TODO
+		link_node_status.tx_rate = tstatus.tx_rate_avg;
+		link_node_status.rx_rate = tstatus.rx_rate_avg;
+		link_node_status.rx_parse_err = tstatus.rx_parse_errors;
+		link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
+		link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
+		link_node_status.messages_sent = tstatus.tx_message_count;
+		link_node_status.messages_received = tstatus.rx_message_count;
+		link_node_status.messages_lost = tstatus.rx_message_lost_count;
 
-			link_node_status.timestamp = hrt_absolute_time();
+		link_node_status.timestamp = hrt_absolute_time();
 
-			mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
+		mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
 
-			return true;
-		}
-
-		return false;
+		return true;
 	}
 };
 

src/modules/mavlink/streams/STATUSTEXT.hpp

@@ -69,56 +69,53 @@ private:
 	bool send() override
 	{
 		if (_mavlink->is_connected()) {
-			while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) {
+			const unsigned last_generation = _mavlink_log_sub.get_last_generation();
 
-				const unsigned last_generation = _mavlink_log_sub.get_last_generation();
+			mavlink_log_s mavlink_log;
 
-				mavlink_log_s mavlink_log;
+			if (_mavlink_log_sub.update(&mavlink_log)) {
+				// don't send stale messages
+				if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
 
-				if (_mavlink_log_sub.update(&mavlink_log)) {
-					// don't send stale messages
-					if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
-
-						if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
-							perf_count(_missed_msg_count_perf);
-							PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
-								  perf_event_count(_missed_msg_count_perf));
-						}
-
-						mavlink_statustext_t msg{};
-						const char *text = mavlink_log.text;
-						constexpr unsigned max_chunk_size = sizeof(msg.text);
-						msg.severity = mavlink_log.severity;
-						msg.chunk_seq = 0;
-						msg.id = _id++;
-						unsigned text_size;
-
-						while ((text_size = strlen(text)) > 0) {
-							unsigned chunk_size = math::min(text_size, max_chunk_size);
-
-							if (chunk_size < max_chunk_size) {
-								memcpy(&msg.text[0], &text[0], chunk_size);
-								// pad with zeros
-								memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
-
-							} else {
-								memcpy(&msg.text[0], &text[0], chunk_size);
-							}
-
-							mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
-
-							if (text_size <= max_chunk_size) {
-								break;
-
-							} else {
-								text += max_chunk_size;
-							}
-
-							msg.chunk_seq += 1;
-						}
-
-						return true;
+					if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
+						perf_count(_missed_msg_count_perf);
+						PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
+							  perf_event_count(_missed_msg_count_perf));
 					}
+
+					mavlink_statustext_t msg{};
+					const char *text = mavlink_log.text;
+					constexpr unsigned max_chunk_size = sizeof(msg.text);
+					msg.severity = mavlink_log.severity;
+					msg.chunk_seq = 0;
+					msg.id = _id++;
+					unsigned text_size;
+
+					while ((text_size = strlen(text)) > 0) {
+						unsigned chunk_size = math::min(text_size, max_chunk_size);
+
+						if (chunk_size < max_chunk_size) {
+							memcpy(&msg.text[0], &text[0], chunk_size);
+							// pad with zeros
+							memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
+
+						} else {
+							memcpy(&msg.text[0], &text[0], chunk_size);
+						}
+
+						mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
+
+						if (text_size <= max_chunk_size) {
+							break;
+
+						} else {
+							text += max_chunk_size;
+						}
+
+						msg.chunk_seq += 1;
+					}
+
+					return true;
 				}
 			}
 		}

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -86,6 +86,11 @@ void MulticopterHoverThrustEstimator::updateParams()
 	}
 
 	_hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get());
+
+	_hover_thrust_ekf.setMinHoverThrust(math::constrain(_param_mpc_thr_hover.get() - _param_hte_thr_range.get(), 0.f,
+					    0.8f));
+	_hover_thrust_ekf.setMaxHoverThrust(math::constrain(_param_mpc_thr_hover.get() + _param_hte_thr_range.get(), 0.2f,
+					    0.9f));
 }
 
 void MulticopterHoverThrustEstimator::Run()

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -121,6 +121,7 @@ private:
 		(ParamFloat<px4::params::HTE_HT_NOISE>) _param_hte_ht_noise,
 		(ParamFloat<px4::params::HTE_ACC_GATE>) _param_hte_acc_gate,
 		(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init,
+		(ParamFloat<px4::params::HTE_THR_RANGE>) _param_hte_thr_range,
 		(ParamFloat<px4::params::HTE_VXY_THR>) _param_hte_vxy_thr,
 		(ParamFloat<px4::params::HTE_VZ_THR>) _param_hte_vz_thr,
 		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover

src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -113,3 +113,20 @@ PARAM_DEFINE_FLOAT(HTE_VXY_THR, 10.0);
  * @group Hover Thrust Estimator
  */
 PARAM_DEFINE_FLOAT(HTE_VZ_THR, 2.0);
+
+/**
+ * Max deviation from MPC_THR_HOVER
+ *
+ * Defines the range of the hover thrust estimate around MPC_THR_HOVER.
+ * A value of 0.2 with MPC_THR_HOVER at 0.5 results in a range of [0.3, 0.7].
+ *
+ * Set to a large value if the vehicle operates in varying physical conditions that
+ * affect the required hover thrust strongly (e.g. differently sized payloads).
+ *
+ * @decimal 2
+ * @min 0.01
+ * @max 0.4
+ * @unit normalized_thrust
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_THR_RANGE, 0.2);

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -121,7 +121,7 @@ inline bool ZeroOrderHoverThrustEkf::isTestRatioPassing(const float innov_test_r
 
 inline void ZeroOrderHoverThrustEkf::updateState(const float K, const float innov)
 {
-	_hover_thr = math::constrain(_hover_thr + K * innov, 0.1f, 0.9f);
+	_hover_thr = math::constrain(_hover_thr + K * innov, _hover_thr_min, _hover_thr_max);
 }
 
 inline void ZeroOrderHoverThrustEkf::updateStateCovariance(const float K, const float H)

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -80,10 +80,11 @@ public:
 
 	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
 	void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
-	void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; }
 	void setMeasurementNoiseScale(float scale) { _acc_var_scale = scale * scale; }
 	void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; }
 	void setAccelInnovGate(float gate_size) { _gate_size = gate_size; }
+	void setMinHoverThrust(float hover_thrust_min) { _hover_thr_min = hover_thrust_min; }
+	void setMaxHoverThrust(float hover_thrust_max) { _hover_thr_max = hover_thrust_max; }
 
 	float getHoverThrustEstimate() const { return _hover_thr; }
 	float getHoverThrustEstimateVar() const { return _state_var; }
@@ -94,6 +95,8 @@ public:
 
 private:
 	float _hover_thr{0.5f};
+	float _hover_thr_min{0.1f};
+	float _hover_thr_max{0.9f};
 
 	float _gate_size{3.f};
 	float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -127,7 +127,7 @@ void VehicleAirData::AirTemperatureUpdate()
 	}
 }
 
-void VehicleAirData::ParametersUpdate()
+bool VehicleAirData::ParametersUpdate()
 {
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated()) {
@@ -136,7 +136,10 @@ void VehicleAirData::ParametersUpdate()
 		_parameter_update_sub.copy(&param_update);
 
 		updateParams();
+		return true;
 	}
+
+	return false;
 }
 
 void VehicleAirData::Run()
@@ -145,7 +148,7 @@ void VehicleAirData::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
-	ParametersUpdate();
+	const bool parameter_update = ParametersUpdate();
 
 	SensorCorrectionsUpdate();
 
@@ -203,7 +206,8 @@ void VehicleAirData::Run()
 	_voter.get_best(time_now_us, &best_index);
 
 	if (best_index >= 0) {
-		if (_selected_sensor_sub_index != best_index) {
+		// handle selection change (don't process on same iteration as parameter update)
+		if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
 			// clear all registered callbacks
 			for (auto &sub : _sensor_sub) {
 				sub.unregisterCallback();
@@ -281,8 +285,8 @@ void VehicleAirData::Run()
 		}
 	}
 
-	// check failover and report
-	if (_last_failover_count != _voter.failover_count()) {
+	// check failover and report (save failover report for a cycle where parameters didn't update)
+	if (_last_failover_count != _voter.failover_count() && !parameter_update) {
 		uint32_t flags = _voter.failover_state();
 		int failover_index = _voter.failover_index();
 

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -71,7 +71,7 @@ public:
 private:
 	void Run() override;
 
-	void ParametersUpdate();
+	bool ParametersUpdate();
 	void SensorCorrectionsUpdate(bool force = false);
 	void AirTemperatureUpdate();
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -79,7 +79,7 @@ void VehicleMagnetometer::Stop()
 	}
 }
 
-void VehicleMagnetometer::ParametersUpdate(bool force)
+bool VehicleMagnetometer::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated() || force) {
@@ -161,7 +161,11 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
 			}
 
 		}
+
+		return true;
 	}
+
+	return false;
 }
 
 void VehicleMagnetometer::UpdateMagBiasEstimate()
@@ -360,7 +364,7 @@ void VehicleMagnetometer::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
-	ParametersUpdate();
+	const bool parameter_update = ParametersUpdate();
 
 	// check vehicle status for changes to armed state
 	if (_vehicle_control_mode_sub.updated()) {
@@ -450,7 +454,8 @@ void VehicleMagnetometer::Run()
 	_voter.get_best(time_now_us, &best_index);
 
 	if (best_index >= 0) {
-		if (_selected_sensor_sub_index != best_index) {
+		// handle selection change (don't process on same iteration as parameter update)
+		if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
 			// clear all registered callbacks
 			for (auto &sub : _sensor_sub) {
 				sub.unregisterCallback();
@@ -488,9 +493,9 @@ void VehicleMagnetometer::Run()
 	}
 
 
-	// check failover and report
 	if (_param_sens_mag_mode.get()) {
-		if (_last_failover_count != _voter.failover_count()) {
+		// check failover and report (save failover report for a cycle where parameters didn't update)
+		if (_last_failover_count != _voter.failover_count() && !parameter_update) {
 			uint32_t flags = _voter.failover_state();
 			int failover_index = _voter.failover_index();
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -79,7 +79,7 @@ public:
 private:
 	void Run() override;
 
-	void ParametersUpdate(bool force = false);
+	bool ParametersUpdate(bool force = false);
 
 	void Publish(uint8_t instance, bool multi = false);
 

src/modules/sensors/voted_sensors_update.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -80,6 +80,8 @@ void VotedSensorsUpdate::initializeSensors()
 
 void VotedSensorsUpdate::parametersUpdate()
 {
+	_parameter_update = true;
+
 	updateParams();
 
 	// run through all IMUs
@@ -188,40 +190,44 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	}
 
 	// find the best sensor
-	int accel_best_index = -1;
-	int gyro_best_index = -1;
-	_accel.voter.get_best(time_now_us, &accel_best_index);
-	_gyro.voter.get_best(time_now_us, &gyro_best_index);
+	int accel_best_index = _accel.last_best_vote;
+	int gyro_best_index = _gyro.last_best_vote;
 
-	if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
-		// use sensor_selection to find best
-		if (_sensor_selection_sub.update(&_selection)) {
-			// reset inconsistency checks against primary
-			for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-				_accel_diff[sensor_index].zero();
-				_gyro_diff[sensor_index].zero();
+	if (!_parameter_update) {
+		// update current accel/gyro selection, skipped on cycles where parameters update
+		_accel.voter.get_best(time_now_us, &accel_best_index);
+		_gyro.voter.get_best(time_now_us, &gyro_best_index);
+
+		if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
+			// use sensor_selection to find best
+			if (_sensor_selection_sub.update(&_selection)) {
+				// reset inconsistency checks against primary
+				for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+					_accel_diff[sensor_index].zero();
+					_gyro_diff[sensor_index].zero();
+				}
 			}
+
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
+					accel_best_index = i;
+				}
+
+				if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
+					gyro_best_index = i;
+				}
+			}
+
+		} else {
+			// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
+			checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
+			checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
 		}
-
-		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-			if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
-				accel_best_index = i;
-			}
-
-			if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
-				gyro_best_index = i;
-			}
-		}
-
-	} else {
-		// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
-		checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
-		checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
 	}
 
 	// write data for the best sensor to output variables
-	if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT)
-	    && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT)) {
+	if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0)
+	    && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) {
 
 		raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
 		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
@@ -423,6 +429,11 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 
 	status.timestamp = hrt_absolute_time();
 	_sensors_status_imu_pub.publish(status);
+
+	if (_parameter_update) {
+		// reset
+		_parameter_update = false;
+	}
 }
 
 void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)

src/modules/sensors/voted_sensors_update.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -182,6 +182,8 @@ private:
 
 	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
 
+	bool _parameter_update{false};
+
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)