Merge branch 'main' into pr-ekf2_disable_gps_fusion_w_spoofing

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -15,6 +15,9 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 	if [ -n "${PX4_HOME_LON}" ]; then
 		param set SIH_LOC_LON0 ${PX4_HOME_LON}
 	fi
+	if [ -n "${PX4_HOME_ALT}" ]; then
+		param set SIH_LOC_H0 ${PX4_HOME_ALT}
+	fi
 
 	if simulator_sih start; then
 

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

@@ -8,6 +8,8 @@
 # @maintainer
 # @board px4_fmu-v2 exclude
 # @board cuav_x7pro exclude
+# @board px4_fmu-v4pro exclude
+# @board px4_fmu-v5x exclude
 # @board px4_fmu-v6x exclude
 #
 

ROMFS/px4fmu_common/init.d/airframes/4071_ifo

@@ -12,6 +12,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board bitcraze_crazyflie exclude
 # @board cuav_x7pro exclude
 #

ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s

@@ -10,6 +10,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board bitcraze_crazyflie exclude
 # @board cuav_x7pro exclude
 #

ROMFS/px4fmu_common/init.d/airframes/4500_clover4

@@ -8,6 +8,7 @@
 # @maintainer Oleg Kalachev <[email protected]>
 #
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v4pro exclude
 # @board bitcraze_crazyflie exclude
 #
 

boards/px4/fmu-v6xrt/nuttx-config/include/board.h

@@ -52,6 +52,7 @@
 #define IMXRT_IPG_PODF_DIVIDER 5
 #define BOARD_GPT_FREQUENCY  24000000
 #define BOARD_XTAL_FREQUENCY 24000000
+#define BOARD_FLEXIO_PREQ    108000000
 
 /* SDIO *********************************************************************/
 

boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c

@@ -114,11 +114,11 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
+		.flexio1_clk_root = /* 432 / 4 = 108Mhz */
 		{
 			.enable        = 1,
-			.div           = 2,
-			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2,
+			.div           = 4,
+			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3,
 		},
 		.flexio2_clk_root =
 		{
@@ -492,9 +492,9 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		.mfd             = 268435455,
 		.ss_enable       = 0,
 		.pfd0            = 27, /* (528 * 18) / 27 = 352 MHz */
-		.pfd1            = 16, /* (528 * 16) / 16 = 594 MHz */
-		.pfd2            = 24, /* (528 * 24) / 27 = 396 MHz */
-		.pfd3            = 32, /* (528 * 32) / 27 = 297 MHz */
+		.pfd1            = 16, /* (528 * 18) / 16 = 594 MHz */
+		.pfd2            = 24, /* (528 * 18) / 24 = 396 MHz */
+		.pfd3            = 22, /* (528 * 18) / 22 = 216 MHz */
 	},
 	.sys_pll3 =
 	{

boards/px4/fmu-v6xrt/src/mtd.cpp

@@ -53,18 +53,12 @@ static const px4_mft_device_t i2c6 = {             // 24LC64T on BASE  8K 32 X 2
 
 static const px4_mtd_entry_t fmum_fram = {
 	.device = &qspi_flash,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = 256
 		}
 	},
 };

msg/AirspeedValidated.msg

@@ -10,3 +10,7 @@ float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspe
 bool airspeed_sensor_measurement_valid 		# True if data from at least one airspeed sensor is declared valid.
 
 int8 selected_airspeed_index 			# 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
+
+float32 airspeed_derivative_filtered		# filtered indicated airspeed derivative [m/s/s]
+float32 throttle_filtered			# filtered fixed-wing throttle [-]
+float32 pitch_filtered				# filtered pitch [rad]

platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c

@@ -46,7 +46,6 @@
 #include "arm_internal.h"
 
 #define FLEXIO_BASE			IMXRT_FLEXIO1_BASE
-#define FLEXIO_PREQ			120000000
 #define DSHOT_TIMERS			FLEXIO_SHIFTBUFNIS_COUNT
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
@@ -305,8 +304,8 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
 int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
 	/* Calculate dshot timings based on dshot_pwm_freq */
-	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
-	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);
+	dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF);
 
 	/* Clock FlexIO peripheral */
 	imxrt_clockall_flexio1();

src/drivers/gnss/septentrio/util.h

@@ -39,6 +39,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 namespace septentrio
 {
 

src/lib/fw_performance_model/PerformanceModel.hpp

@@ -131,7 +131,6 @@ class PerformanceModel : public ModuleParams
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
 		(ParamFloat<px4::params::FW_SERVICE_CEIL>) _param_service_ceiling,
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
-		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
 		(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,

src/lib/mathlib/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 px4_add_library(mathlib
 	math/test/test.cpp
+	math/filter/FilteredDerivative.hpp
 	math/filter/LowPassFilter2p.hpp
 	math/filter/MedianFilter.hpp
 	math/filter/NotchFilter.hpp

src/lib/mathlib/math/filter/FilteredDerivative.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 FilteredDerivative.hpp
+ *
+ * @brief Derivative function passed through a first order "alpha" IIR digital filter
+ *
+ * @author Silvan Fuhrer <[email protected]>
+ */
+
+#pragma once
+
+// #include <float.h>
+// #include <mathlib/math/Functions.hpp>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+using namespace math;
+
+template <typename T>
+class FilteredDerivative
+{
+public:
+	FilteredDerivative() = default;
+	~FilteredDerivative() = default;
+
+	/**
+	 * Set filter parameters for time abstraction
+	 *
+	 * Both parameters have to be provided in the same units.
+	 *
+	 * @param sample_interval interval between two samples
+	 * @param time_constant filter time constant determining convergence
+	 */
+	void setParameters(float sample_interval, float time_constant)
+	{
+		_alpha_filter.setParameters(sample_interval, time_constant);
+		_sample_interval = sample_interval;
+	}
+
+	/**
+	 * Set filter state to an initial value
+	 *
+	 * @param sample new initial value
+	 */
+	void reset(const T &sample)
+	{
+		_alpha_filter.reset(sample);
+		_initialized = false;
+	}
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	const T &update(const T &sample)
+	{
+		if (_initialized) {
+			if (_sample_interval > FLT_EPSILON) {
+				_alpha_filter.update((sample - _previous_sample) / _sample_interval);
+
+			} else {
+				_initialized = false;
+			}
+
+		} else {
+			// don't update in the first iteration
+			_initialized = true;
+		}
+
+		_previous_sample = sample;
+		return _alpha_filter.getState();
+	}
+
+	const T &getState() const { return _alpha_filter.getState(); }
+
+
+private:
+	AlphaFilter<T> _alpha_filter;
+	float _sample_interval{0.f};
+	T _previous_sample{0.f};
+	bool _initialized{false};
+};

src/modules/airspeed_selector/AirspeedValidator.cpp

@@ -60,6 +60,8 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	check_load_factor(input_data.accel_z);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
 				  input_data.ground_velocity, input_data.gnss_valid);
+	check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle,
+			      input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att);
 	update_airspeed_valid_status(input_data.timestamp);
 }
 
@@ -277,16 +279,76 @@ AirspeedValidator::check_load_factor(float accel_z)
 	}
 }
 
+void
+AirspeedValidator::check_first_principle(const uint64_t timestamp, const float throttle_fw, const float throttle_trim,
+		const uint64_t tecs_timestamp, const Quatf &att_q)
+{
+	if (! _first_principle_check_enabled) {
+		_first_principle_check_failed = false;
+		_time_last_first_principle_check_passing = timestamp;
+		return;
+	}
+
+	const float pitch = matrix::Eulerf(att_q).theta();
+	const hrt_abstime tecs_dt = timestamp - tecs_timestamp; // return if TECS data is old (TECS not running)
+
+	if (!_in_fixed_wing_flight || tecs_dt > 500_ms || !PX4_ISFINITE(_IAS) || !PX4_ISFINITE(throttle_fw)
+	    || !PX4_ISFINITE(throttle_trim) || !PX4_ISFINITE(pitch)) {
+		// do not do anything in that case
+		return;
+	}
+
+	const float dt = static_cast<float>(timestamp - _time_last_first_principle_check) / 1_s;
+	_time_last_first_principle_check = timestamp;
+
+	// update filters
+	if (dt < FLT_EPSILON || dt > 1.f) {
+		// reset if dt is too large
+		_IAS_derivative.reset(0.f);
+		_throttle_filtered.reset(throttle_fw);
+		_pitch_filtered.reset(pitch);
+		_time_last_first_principle_check_passing = timestamp;
+
+	} else {
+		// update filters, with different time constant
+		_IAS_derivative.setParameters(dt, 5.f);
+		_throttle_filtered.setParameters(dt, 0.5f);
+		_pitch_filtered.setParameters(dt, 1.5f);
+
+		_IAS_derivative.update(_IAS);
+		_throttle_filtered.update(throttle_fw);
+		_pitch_filtered.update(pitch);
+	}
+
+	// declare high throttle if more than 5% above trim
+	const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max);
+	const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold;
+	const bool pitching_down = _pitch_filtered.getState() < _param_psp_off;
+
+	// check if the airspeed derivative is too low given the throttle and pitch
+	const bool check_failing = _IAS_derivative.getState() < kIASDerivateThreshold && high_throttle && pitching_down;
+
+	if (!check_failing) {
+		_time_last_first_principle_check_passing = timestamp;
+		_first_principle_check_failed = false;
+	}
+
+	if (timestamp - _time_last_first_principle_check_passing > _aspd_fp_t_window * 1_s) {
+		// only update the test_failed flag once the timeout since first principle check failing is over
+		_first_principle_check_failed = check_failing;
+	}
+}
 
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
+	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed
+	    || _first_principle_check_failed) {
 		// at least one check (data stuck, innovation or load factor) failed, so record timestamp
 		_time_checks_failed = timestamp;
 
 	} else if (! _data_stuck_test_failed && !_innovations_check_failed
-		   && !_load_factor_check_failed) {
+		   && !_load_factor_check_failed && !_first_principle_check_failed) {
 		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}