Update NuttX (Testing Needs merge it first)

rc_input: Add SINGLEWIRE_MANUAL_DUPLEX for manually changing TXDIR
Fixes telemetry on fmu-v6xrt
2026-05-31 03:20:04 +08:00 · 2024-05-04 21:47:55 +02:00 · 2024-05-04 21:47:35 +02:00 · 2024-05-03 06:59:55 +02:00 · 2024-05-02 17:52:26 -04:00 · 2024-05-02 17:40:58 -04:00
66 changed files with 450 additions and 316 deletions
@@ -451,10 +451,12 @@
 #define HRT_PPM_CHANNEL         /* GPIO_EMC_B1_09 GPIO_GPT5_CAPTURE1_1 */  1  /* use capture/compare channel 1 */
 #define GPIO_PPM_IN             /* GPIO_EMC_B1_09 GPT1_CAPTURE2 */ (GPIO_GPT5_CAPTURE1_1 | GENERAL_INPUT_IOMUX)

-#define RC_SERIAL_PORT                  "/dev/ttyS4"
-#define RC_SERIAL_SINGLEWIRE            1 // Suport Single wire wiring
-#define RC_SERIAL_SWAP_RXTX             1 // Set Swap (but not supported in HW) to use Single wire
-#define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap
+#define RC_SERIAL_PORT                     "/dev/ttyS4"
+#define RC_SERIAL_SINGLEWIRE               1 // Suport Single wire wiring
+#define RC_SERIAL_SWAP_RXTX                1 // Set Swap (but not supported in HW) to use Single wire
+#define RC_SERIAL_SWAP_USING_SINGLEWIRE    1 // Set to use Single wire swap as HW does not support swap
+#define RC_SERIAL_SINGLEWIRE_MANUAL_DUPLEX 1 // Set to use ioctl's to manually swap duplex
+#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT

 /* FLEXSPI4 */

@@ -172,7 +172,9 @@ void ADIS16507::RunImpl()
 			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);

 			if (DIAG_STAT != 0) {
-				PX4_ERR("DIAG_STAT: %#X", DIAG_STAT);
+				PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT);
+				_state = STATE::RESET;
+				ScheduleDelayed(3_s);

 			} else {
 				PX4_DEBUG("self test passed");
@@ -734,7 +734,15 @@ void RCInput::Run()
 						}

 						if (_crsf_telemetry) {
+#if defined(RC_SERIAL_SINGLEWIRE_MANUAL_DUPLEX)
+							ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_DIR_TX);
+#endif
 							_crsf_telemetry->update(cycle_timestamp);
+#if defined(RC_SERIAL_SINGLEWIRE_MANUAL_DUPLEX)
+							tcflush(_rcs_fd, TCIOFLUSH);
+							tcdrain(_rcs_fd); // Ensure TX FIFO is empty before swapping
+							ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_DIR_RX);
+#endif
 						}
 					}
 				}
@@ -787,7 +795,15 @@ void RCInput::Run()
 						}

 						if (_ghst_telemetry) {
+#if defined(RC_SERIAL_SINGLEWIRE_MANUAL_DUPLEX)
+							ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_DIR_TX);
+#endif
 							_ghst_telemetry->update(cycle_timestamp);
+#if defined(RC_SERIAL_SINGLEWIRE_MANUAL_DUPLEX)
+							tcflush(_rcs_fd, TCIOFLUSH);
+							tcdrain(_rcs_fd); // Ensure TX FIFO is empty before swapping
+							ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED | SER_SINGLEWIRE_DIR_RX);
+#endif
 						}
 					}
 				}
@@ -116,111 +116,108 @@ endif()
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
-	EKF/bias_estimator.cpp
 	EKF/control.cpp
 	EKF/covariance.cpp
 	EKF/ekf.cpp
 	EKF/ekf_helper.cpp
 	EKF/estimator_interface.cpp
-	EKF/fake_height_control.cpp
-	EKF/fake_pos_control.cpp
 	EKF/height_control.cpp
-	EKF/imu_down_sampler.cpp
-	EKF/output_predictor.cpp
 	EKF/velocity_fusion.cpp
 	EKF/position_fusion.cpp
 	EKF/yaw_fusion.cpp
-	EKF/zero_innovation_heading_update.cpp

+	EKF/imu_down_sampler/imu_down_sampler.cpp
+
+	EKF/aid_sources/fake_height_control.cpp
+	EKF/aid_sources/fake_pos_control.cpp
 	EKF/aid_sources/ZeroGyroUpdate.cpp
 	EKF/aid_sources/ZeroVelocityUpdate.cpp
+	EKF/aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/airspeed/airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS EKF/aux_global_position.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/auxvel/auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		EKF/baro_height_control.cpp
+		EKF/aid_sources/barometer/baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/drag/drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		EKF/ev_control.cpp
-		EKF/ev_height_control.cpp
-		EKF/ev_pos_control.cpp
-		EKF/ev_vel_control.cpp
-		EKF/ev_yaw_control.cpp
+		EKF/aid_sources/external_vision/ev_control.cpp
+		EKF/aid_sources/external_vision/ev_height_control.cpp
+		EKF/aid_sources/external_vision/ev_pos_control.cpp
+		EKF/aid_sources/external_vision/ev_vel_control.cpp
+		EKF/aid_sources/external_vision/ev_yaw_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		EKF/gnss_height_control.cpp
-		EKF/gps_checks.cpp
-		EKF/gps_control.cpp
+		EKF/aid_sources/gnss/gnss_height_control.cpp
+		EKF/aid_sources/gnss/gps_checks.cpp
+		EKF/aid_sources/gnss/gps_control.cpp
 	)

+	if(CONFIG_EKF2_GNSS_YAW)
+		list(APPEND EKF_SRCS EKF/aid_sources/gnss/gps_yaw_fusion.cpp)
+	endif()
+
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

-if(CONFIG_EKF2_GNSS_YAW)
-	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
-endif()
-
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS EKF/gravity_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/gravity/gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		EKF/mag_3d_control.cpp
-		EKF/mag_control.cpp
-		EKF/mag_fusion.cpp
+		EKF/aid_sources/magnetometer/mag_3d_control.cpp
+		EKF/aid_sources/magnetometer/mag_control.cpp
+		EKF/aid_sources/magnetometer/mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		EKF/optical_flow_control.cpp
-		EKF/optflow_fusion.cpp
+		EKF/aid_sources/optical_flow/optical_flow_control.cpp
+		EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		EKF/range_finder_consistency_check.cpp
-		EKF/range_height_control.cpp
-		EKF/sensor_range_finder.cpp
+		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp)
 endif()

 add_subdirectory(EKF)

-
-
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -260,7 +257,10 @@ px4_add_module(
 		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
+
 		${EKF_LIBS}
+		bias_estimator
+		output_predictor
 	UNITY_BUILD
 	)

@@ -31,109 +31,111 @@
 #
 ############################################################################

+add_subdirectory(bias_estimator)
+add_subdirectory(output_predictor)
+
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
-	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	ekf.cpp
 	ekf_helper.cpp
 	estimator_interface.cpp
-	fake_height_control.cpp
-	fake_pos_control.cpp
 	height_control.cpp
-	imu_down_sampler.cpp
-	output_predictor.cpp
 	velocity_fusion.cpp
 	position_fusion.cpp
 	yaw_fusion.cpp
-	zero_innovation_heading_update.cpp

+	imu_down_sampler/imu_down_sampler.cpp
+
+	aid_sources/fake_height_control.cpp
+	aid_sources/fake_pos_control.cpp
 	aid_sources/ZeroGyroUpdate.cpp
 	aid_sources/ZeroVelocityUpdate.cpp
+	aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS aux_global_position.cpp)
+	list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		baro_height_control.cpp
+		aid_sources/barometer/baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS drag_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/drag/drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		ev_control.cpp
-		ev_height_control.cpp
-		ev_pos_control.cpp
-		ev_vel_control.cpp
-		ev_yaw_control.cpp
+		aid_sources/external_vision/ev_control.cpp
+		aid_sources/external_vision/ev_height_control.cpp
+		aid_sources/external_vision/ev_pos_control.cpp
+		aid_sources/external_vision/ev_vel_control.cpp
+		aid_sources/external_vision/ev_yaw_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		gnss_height_control.cpp
-		gps_checks.cpp
-		gps_control.cpp
+		aid_sources/gnss/gnss_height_control.cpp
+		aid_sources/gnss/gps_checks.cpp
+		aid_sources/gnss/gps_control.cpp
 	)

+	if(CONFIG_EKF2_GNSS_YAW)
+		list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp)
+	endif()
+
 	add_subdirectory(yaw_estimator)
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

-if(CONFIG_EKF2_GNSS_YAW)
-	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
-endif()
-
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS gravity_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/gravity/gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		mag_3d_control.cpp
-		mag_control.cpp
-		mag_fusion.cpp
+		aid_sources/magnetometer/mag_3d_control.cpp
+		aid_sources/magnetometer/mag_control.cpp
+		aid_sources/magnetometer/mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		optical_flow_control.cpp
-		optflow_fusion.cpp
+		aid_sources/optical_flow/optical_flow_control.cpp
+		aid_sources/optical_flow/optical_flow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		range_finder_consistency_check.cpp
-		range_height_control.cpp
-		sensor_range_finder.cpp
+		aid_sources/range_finder/range_finder_consistency_check.cpp
+		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/sideslip/sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator.cpp)
+	list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp)
 endif()

 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -146,7 +148,9 @@ target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PAT

 target_link_libraries(ecl_EKF
 	PRIVATE
+		bias_estimator
 		geo
+		output_predictor
 		world_magnetic_model
 		${EKF_LIBS}
 )
@@ -33,7 +33,7 @@

 #include "ekf.h"

-#include "aux_global_position.hpp"
+#include "aid_sources/aux_global_position/aux_global_position.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -42,8 +42,8 @@
 //  WelfordMean for init?
 //  WelfordMean for rate

-#include "common.h"
-#include "RingBuffer.h"
+#include "../../common.h"
+#include "../../RingBuffer.h"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -196,3 +196,39 @@ void Ekf::stopBaroHgtFusion()
 		_control_status.flags.baro_hgt = false;
 	}
 }
+
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
+float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+{
+	if (_control_status.flags.wind && local_position_is_valid()) {
+		// calculate static pressure error = Pmeas - Ptruth
+		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
+		// negative X and Y directions. Used to correct baro data for positional errors
+
+		// Calculate airspeed in body frame
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
+
+		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
+
+		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
+
+		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
+
+		const Vector3f K_pstatic_coef(
+			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
+			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
+			_params.static_pressure_coef_z);
+
+		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+
+		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
+
+		// correct baro measurement using pressure error estimate and assuming sea level gravity
+		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
+	}
+
+	// otherwise return the uncorrected baro measurement
+	return baro_alt_uncompensated;
+}
+#endif // CONFIG_EKF2_BARO_COMPENSATION
@@ -42,7 +42,7 @@
 #ifndef EKF_SENSOR_HPP
 #define EKF_SENSOR_HPP

-#include "common.h"
+#include <cstdint>

 namespace estimator
 {
@@ -35,9 +35,10 @@
 * @file range_finder_consistency_check.cpp
 */

-#include "range_finder_consistency_check.hpp"
+#include <aid_sources/range_finder/range_finder_consistency_check.hpp>

-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
+		bool horizontal_motion, uint64_t time_us)
 {
 	if (horizontal_motion) {
 		_time_last_horizontal_motion = time_us;
@@ -55,7 +56,8 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
 	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down

-	const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	const float var = 2.f * dist_bottom_var / (dt * dt);
 	_innov_var = var + vz_var;

 	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
@@ -84,8 +86,9 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 		}

 	} else {
-		if (_test_ratio < 1.f
-		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if ((_test_ratio < 1.f)
+		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
+		   ) {
 			_is_kinematically_consistent = true;
 		}
 	}
@@ -64,13 +64,14 @@ void Ekf::controlRangeHeightFusion()

 			if (_control_status.flags.in_air) {
 				const bool horizontal_motion = _control_status.flags.fixed_wing
-								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
+							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));

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

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

 		} else {
@@ -121,13 +122,15 @@ void Ekf::controlRangeHeightFusion()
 		}

 		// determine if we should use height aiding
-		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable();
+		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
+
 		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy();

 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)
+				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();

 		if (_control_status.flags.rng_hgt) {
@@ -165,13 +168,17 @@ void Ekf::controlRangeHeightFusion()

 		} else {
 			if (starting_conditions_passing) {
-				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
+				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
+				    && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+				   ) {
 					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
 					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 					bias_est.setBias(_state.pos(2) + measurement);

-				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
+				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
+					   && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
+					  ) {
 					// Range finder is the primary height source, the ground is now the datum used
 					// to compute the local vertical position
 					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
@@ -193,7 +200,7 @@ void Ekf::controlRangeHeightFusion()
 		}

 	} else if (_control_status.flags.rng_hgt
-		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) {
+		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
 		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
@@ -203,6 +210,7 @@ void Ekf::controlRangeHeightFusion()
 bool Ekf::isConditionalRangeAidSuitable()
 {
 #if defined(CONFIG_EKF2_TERRAIN)
+
 	if (_control_status.flags.in_air
 	    && _range_sensor.isHealthy()
 	    && isTerrainEstimateValid()) {
@@ -236,6 +244,7 @@ bool Ekf::isConditionalRangeAidSuitable()

 		return is_in_range && is_hagl_stable && is_below_max_speed;
 	}
+
 #endif // CONFIG_EKF2_TERRAIN

 	return false;
@@ -38,20 +38,22 @@
 *
 */

-#include "sensor_range_finder.hpp"
+#include <aid_sources/range_finder/sensor_range_finder.hpp>
+
+#include <lib/matrix/matrix/math.hpp>

 namespace estimator
 {
 namespace sensor
 {

-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth)
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
 {
 	updateSensorToEarthRotation(R_to_earth);
 	updateValidity(current_time_us);
 }

-void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth)
+void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
 {
 	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
 	// this is required for use of range finder and flow data
@@ -114,7 +116,7 @@ inline bool SensorRangeFinder::isDataInRange() const

 void SensorRangeFinder::updateStuckCheck()
 {
-	if(!isStuckDetectorEnabled()){
+	if (!isStuckDetectorEnabled()) {
 		// Stuck detector disabled
 		_is_stuck = false;
 		return;
@@ -42,6 +42,7 @@
 #define EKF_SENSOR_RANGE_FINDER_HPP

 #include "Sensor.hpp"
+
 #include <matrix/math.hpp>

 namespace estimator
@@ -49,6 +50,14 @@ namespace estimator
 namespace sensor
 {

+struct rangeSample {
+	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
+	float       rng{};      ///< range (distance to ground) measurement (m)
+	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
+};
+
+static constexpr uint64_t RNG_MAX_INTERVAL = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
+
 class SensorRangeFinder : public Sensor
 {
 public:
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_library(bias_estimator
+	bias_estimator.cpp
+	bias_estimator.hpp
+	height_bias_estimator.hpp
+	position_bias_estimator.hpp
+)
+
+add_dependencies(bias_estimator prebuild_targets)
@@ -39,7 +39,7 @@
 #define EKF_HEIGHT_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
-#include "common.h"
+#include "../common.h"

 class HeightBiasEstimator: public BiasEstimator
 {
@@ -39,7 +39,6 @@
 #define EKF_POSITION_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
-#include "common.h"

 class PositionBiasEstimator
 {
@@ -70,7 +70,6 @@ static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable
 static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
 static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
-static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL      = 500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)

 // bad accelerometer detection and mitigation
@@ -197,12 +196,6 @@ struct baroSample {
 	bool        reset{false};
 };

-struct rangeSample {
-	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
-	float       rng{};      ///< range (distance to ground) measurement (m)
-	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
-};
-
 struct airspeedSample {
 	uint64_t    time_us{};          ///< timestamp of the measurement (uSec)
 	float       true_airspeed{};    ///< true airspeed measurement (m/sec)
@@ -282,6 +282,25 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 	P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
 }

+void Ekf::resetGyroBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetGyroBiasZCov()
+{
+	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetAccelBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+}
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void Ekf::resetMagCov()
 {
@@ -295,7 +314,10 @@ void Ekf::resetMagCov()
 }
 #endif // CONFIG_EKF2_MAGNETOMETER

-void Ekf::resetGyroBiasZCov()
+#if defined(CONFIG_EKF2_WIND)
+void Ekf::resetWindCov()
 {
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
 }
+#endif // CONFIG_EKF2_WIND
@@ -49,9 +49,9 @@
 # include "yaw_estimator/EKFGSF_yaw.h"
 #endif // CONFIG_EKF2_GNSS

-#include "bias_estimator.hpp"
-#include "height_bias_estimator.hpp"
-#include "position_bias_estimator.hpp"
+#include "bias_estimator/bias_estimator.hpp"
+#include "bias_estimator/height_bias_estimator.hpp"
+#include "bias_estimator/position_bias_estimator.hpp"

 #include <ekf_derivation/generated/state.h>

@@ -63,7 +63,7 @@
 #include "aid_sources/ZeroVelocityUpdate.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-# include "aux_global_position.hpp"
+# include "aid_sources/aux_global_position/aux_global_position.hpp"
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

 enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -265,12 +265,13 @@ public:
 	// get the 1-sigma horizontal and vertical velocity uncertainty
 	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;

-	// get the vehicle control limits required by the estimator to keep within sensor limitations
+	// Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
+	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
+	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
+	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
+	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
 	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;

-	// Reset all IMU bias states and covariances to initial alignment values.
-	void resetImuBias();
-
 	void resetGyroBias();
 	void resetGyroBiasCov();

@@ -390,7 +391,7 @@ public:
 	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 					float &hagl, float &beta) const;

-	// return a bitmask integer that describes which state estimates can be used for flight control
+	// return a bitmask integer that describes which state estimates are valid
 	void get_ekf_soln_status(uint16_t *status) const;

 	HeightSensor getHeightSensorRef() const { return _height_sensor_ref; }
@@ -947,10 +948,6 @@ private:
 	// and a scalar innovation value
 	void fuse(const VectorState &K, float innovation);

-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
-#endif // CONFIG_EKF2_BARO_COMPENSATION
-
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;

@@ -1079,6 +1076,11 @@ private:
 	void stopBaroHgtFusion();

 	void updateGroundEffect();
+
+# if defined(CONFIG_EKF2_BARO_COMPENSATION)
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
+# endif // CONFIG_EKF2_BARO_COMPENSATION
+
 #endif // CONFIG_EKF2_BAROMETER

 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
@@ -56,43 +56,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }

-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
-{
-	if (_control_status.flags.wind && local_position_is_valid()) {
-		// calculate static pressure error = Pmeas - Ptruth
-		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
-		// negative X and Y directions. Used to correct baro data for positional errors
-
-		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
-		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
-
-		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
-
-		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
-
-		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
-
-		const Vector3f K_pstatic_coef(
-			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
-			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
-			_params.static_pressure_coef_z);
-
-		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
-
-		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
-
-		// correct baro measurement using pressure error estimate and assuming sea level gravity
-		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
-	}
-
-	// otherwise return the uncorrected baro measurement
-	return baro_alt_uncompensated;
-}
-#endif // CONFIG_EKF2_BARO_COMPENSATION
-
-// calculate the earth rotation vector
 Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 {
 	return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
@@ -235,7 +198,6 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2));
 }

-// get the 1-sigma horizontal and vertical velocity uncertainty
 void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 {
 	float hvel_err = sqrtf(P.trace<2>(State::vel.idx));
@@ -276,13 +238,6 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }

-/*
-Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
-vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
-vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
-hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
-*/
 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
 {
 	// Do not require limiting by default
@@ -330,12 +285,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 #endif // CONFIG_EKF2_RANGE_FINDER
 }

-void Ekf::resetImuBias()
-{
-	resetGyroBias();
-	resetAccelBias();
-}
-
 void Ekf::resetGyroBias()
 {
 	// Zero the gyro bias states
@@ -344,13 +293,6 @@ void Ekf::resetGyroBias()
 	resetGyroBiasCov();
 }

-void Ekf::resetGyroBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
-}
-
 void Ekf::resetAccelBias()
 {
 	// Zero the accel bias states
@@ -359,18 +301,6 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }

-void Ekf::resetAccelBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
-}
-
-// get EKF innovation consistency check status information comprising of:
-// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
 void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 				     float &hagl, float &beta) const
 {
@@ -498,7 +428,6 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 #endif // CONFIG_EKF2_SIDESLIP
 }

-// return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
@@ -694,53 +623,6 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER

-void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
-{
-	// save a copy of the quaternion state for later use in calculating the amount of reset change
-	const Quatf quat_before_reset = _state.quat_nominal;
-
-	// update the yaw angle variance
-	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
-		P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
-	}
-
-	// update transformation matrix from body to world frame using the current estimate
-	// update the rotation matrix using the new yaw value
-	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
-
-	// calculate the amount that the quaternion has changed by
-	const Quatf quat_after_reset(_R_to_earth);
-	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
-
-	// update quaternion states
-	_state.quat_nominal = quat_after_reset;
-
-	// add the reset amount to the output observer buffered data
-	_output_predictor.resetQuaternion(q_error);
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	// update EV attitude error filter
-	if (_ev_q_error_initialized) {
-		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
-		_ev_q_error_filt.reset(ev_q_error_updated);
-	}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-	// record the state change
-	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
-		_state_reset_status.quat_change = q_error;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
-		_state_reset_status.quat_change.normalize();
-	}
-
-	_state_reset_status.reset_count.quat++;
-
-	_time_last_heading_fuse = _time_delayed_us;
-}
-
 #if defined(CONFIG_EKF2_WIND)
 void Ekf::resetWind()
 {
@@ -764,11 +646,6 @@ void Ekf::resetWindToZero()
 	resetWindCov();
 }

-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
 #endif // CONFIG_EKF2_WIND

 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
@@ -266,7 +266,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const rangeSample &range_sample)
+void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
 	if (!_initialised) {
 		return;
@@ -274,7 +274,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)

 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);

 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {

-		rangeSample range_sample_new{range_sample};
+		sensor::rangeSample range_sample_new{range_sample};
 		range_sample_new.time_us = time_us;

 		_range_buffer->push(range_sample_new);
@@ -63,12 +63,12 @@

 #include "common.h"
 #include "RingBuffer.h"
-#include "imu_down_sampler.hpp"
-#include "output_predictor.h"
+#include "imu_down_sampler/imu_down_sampler.hpp"
+#include "output_predictor/output_predictor.h"

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-# include "range_finder_consistency_check.hpp"
-# include "sensor_range_finder.hpp"
+# include "aid_sources/range_finder/range_finder_consistency_check.hpp"
+# include "aid_sources/range_finder/sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER

 #include <lib/atmosphere/atmosphere.h>
@@ -107,7 +107,7 @@ public:
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const rangeSample &range_sample);
+	void setRangeData(const estimator::sensor::rangeSample &range_sample);

 	// set sensor limitations reported by the rangefinder
 	void set_rangefinder_limits(float min_distance, float max_distance)
@@ -115,7 +115,7 @@ public:
 		_range_sensor.setLimits(min_distance, max_distance);
 	}

-	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
+	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -356,7 +356,7 @@ protected:
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<rangeSample> *_range_buffer{nullptr};
+	RingBuffer<sensor::rangeSample> *_range_buffer{nullptr};
 	uint64_t _time_last_range_buffer_push{0};

 	sensor::SensorRangeFinder _range_sensor{};
@@ -1,4 +1,4 @@
-#include "imu_down_sampler.hpp"
+#include "imu_down_sampler/imu_down_sampler.hpp"

 #include <lib/mathlib/mathlib.h>

@@ -41,7 +41,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>

-#include "common.h"
+#include "../common.h"

 using namespace estimator;

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_library(output_predictor
+	output_predictor.cpp
+	output_predictor.h
+)
+
+add_dependencies(output_predictor prebuild_targets)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4. All rights reserved.
+ *   Copyright (c) 2022-2024 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4. All rights reserved.
+ *   Copyright (c) 2022-2024 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -36,8 +36,7 @@

 #include <matrix/math.hpp>

-#include "common.h"
-#include "RingBuffer.h"
+#include "../RingBuffer.h"

 #include <lib/geo/geo.h>

@@ -37,7 +37,12 @@ import symforce
 symforce.set_epsilon_to_symbol()

 import symforce.symbolic as sf
-from utils.derivation_utils import *
+
+# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils
+import os, sys
+derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils"
+sys.path.append(derivation_utils_dir)
+import derivation_utils

 def predict_opt_flow(
        terrain_vpos: sf.Scalar,
@@ -49,7 +54,7 @@ def predict_opt_flow(
    R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix()
    flow_pred = sf.V2()
    dist = - (pos_z - terrain_vpos)
-    dist = add_epsilon_sign(dist, dist, epsilon)
+    dist = derivation_utils.add_epsilon_sign(dist, dist, epsilon)
    flow_pred[0] = -v[1] / dist * R_to_earth[2, 2]
    flow_pred[1] = v[0] / dist * R_to_earth[2, 2]
    return flow_pred
@@ -90,5 +95,5 @@ def terr_est_compute_flow_y_innov_var_and_h(
    return (innov_var, Hy[0, 0])

 print("Derive terrain estimator equations...")
-generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
-generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
+derivation_utils.generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
+derivation_utils.generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
@@ -38,8 +38,8 @@
 */

 #include "ekf.h"
-#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
-#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
+#include "terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
+#include "terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"

 #include <mathlib/mathlib.h>

@@ -136,3 +136,50 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec
 {
 	sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
 }
+
+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
+{
+	// save a copy of the quaternion state for later use in calculating the amount of reset change
+	const Quatf quat_before_reset = _state.quat_nominal;
+
+	// update the yaw angle variance
+	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
+		P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
+	}
+
+	// update transformation matrix from body to world frame using the current estimate
+	// update the rotation matrix using the new yaw value
+	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
+
+	// calculate the amount that the quaternion has changed by
+	const Quatf quat_after_reset(_R_to_earth);
+	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
+
+	// update quaternion states
+	_state.quat_nominal = quat_after_reset;
+
+	// add the reset amount to the output observer buffered data
+	_output_predictor.resetQuaternion(q_error);
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	// update EV attitude error filter
+	if (_ev_q_error_initialized) {
+		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
+		_ev_q_error_filt.reset(ev_q_error_updated);
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+	// record the state change
+	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
+		_state_reset_status.quat_change = q_error;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
+		_state_reset_status.quat_change.normalize();
+	}
+
+	_state_reset_status.reset_count.quat++;
+
+	_time_last_heading_fuse = _time_delayed_us;
+}
@@ -2368,7 +2368,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)

 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;

-			rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = optical_flow.timestamp_sample,
 				.rng = optical_flow.distance_m,
 				.quality = quality,
@@ -2507,7 +2507,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
 		// EKF range sample
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
-			rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
 				.rng = distance_sensor.current_distance,
 				.quality = distance_sensor.signal_quality,
@@ -55,7 +55,7 @@ public:
 	void setLimits(float min_distance_m, float max_distance_m);

 private:
-	rangeSample _range_sample{};
+	estimator::sensor::rangeSample _range_sample{};
 	float _min_distance{0.2f};
 	float _max_distance{20.0f};

@@ -34,7 +34,7 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/ekf.h"
-#include "EKF/imu_down_sampler.hpp"
+#include "EKF/imu_down_sampler/imu_down_sampler.hpp"

 class EkfImuSamplingTest : public ::testing::TestWithParam<std::tuple<float, float, Vector3f, Vector3f>>
 {
@@ -34,10 +34,10 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/common.h"
-#include "EKF/sensor_range_finder.hpp"
+#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
 #include <matrix/math.hpp>

-using estimator::rangeSample;
+using estimator::sensor::rangeSample;
 using matrix::Dcmf;
 using matrix::Eulerf;
 using namespace estimator::sensor;
@@ -2255,9 +2255,6 @@ Mavlink::task_main(int argc, char *argv[])

 	_task_running.store(true);

-	bool cmd_logging_start_acknowledgement = false;
-	bool cmd_logging_stop_acknowledgement = false;
-
 	while (!should_exit()) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);
@@ -2266,7 +2263,7 @@ Mavlink::task_main(int argc, char *argv[])
 			check_requested_subscriptions();
 			handleStatus();
 			handleCommands();
-			handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
+			handleAndGetCurrentCommandAck();
 			continue;
 		}

@@ -2291,7 +2288,7 @@ Mavlink::task_main(int argc, char *argv[])

 		handleStatus();
 		handleCommands();
-		handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
+		handleAndGetCurrentCommandAck();

 		/* check for shell output */
 		if (_mavlink_shell && _mavlink_shell->available() > 0) {
@@ -2330,25 +2327,15 @@ Mavlink::task_main(int argc, char *argv[])

 		/* check for ulog streaming messages */
 		if (_mavlink_ulog) {
-			if (cmd_logging_stop_acknowledgement) {
+			const int ret = _mavlink_ulog->handle_update(get_channel());
+
+			if (ret < 0) { // abort the streaming on error
+				if (ret != -1) {
+					PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
+				}
+
 				_mavlink_ulog->stop();
 				_mavlink_ulog = nullptr;
-
-			} else {
-				if (cmd_logging_start_acknowledgement) {
-					_mavlink_ulog->start_ack_received();
-				}
-
-				int ret = _mavlink_ulog->handle_update(get_channel());
-
-				if (ret < 0) { //abort the streaming on error
-					if (ret != -1) {
-						PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
-					}
-
-					_mavlink_ulog->stop();
-					_mavlink_ulog = nullptr;
-				}
 			}
 		}

@@ -2571,7 +2558,7 @@ void Mavlink::handleCommands()
 	}
 }

-void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack)
+void Mavlink::handleAndGetCurrentCommandAck()
 {
 	if (_vehicle_command_ack_sub.updated()) {
 		static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -2601,6 +2588,20 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
 					msg.target_system = command_ack.target_system;
 					msg.target_component = command_ack.target_component;

+					// Handle logging acks before sending out the mavlink message to prevent a race condition
+					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+						if (_mavlink_ulog) {
+							_mavlink_ulog->start_ack_received();
+						}
+
+					} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
+						   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
+						if (_mavlink_ulog) {
+							_mavlink_ulog->stop();
+							_mavlink_ulog = nullptr;
+						}
+					}
+
 					if (_mode == MAVLINK_MODE_IRIDIUM) {
 						if (command_ack.from_external) {
 							// for MAVLINK_MODE_IRIDIUM send only if external
@@ -2611,13 +2612,6 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
 						mavlink_msg_command_ack_send_struct(get_channel(), &msg);
 					}

-					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
-						logging_start_ack = true;
-
-					} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
-						   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
-						logging_stop_ack = true;
-					}
 				}
 			}
 		}
@@ -710,7 +710,7 @@ private:

 	void handleCommands();

-	void handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack);
+	void handleAndGetCurrentCommandAck();

 	void handleStatus();

@@ -277,7 +277,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 					// if no gyro was selected use the first valid sensor_gyro_fifo
 					if (!device_id_valid) {
 						device_id = sensor_gyro_fifo_sub.get().device_id;
-						PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id);
+						PX4_DEBUG("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id);
 					}

 					if (sensor_gyro_fifo_sub.get().device_id == device_id) {
@@ -319,7 +319,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
 					// if no gyro was selected use the first valid sensor_gyro
 					if (!device_id_valid) {
 						device_id = sensor_gyro_sub.get().device_id;
-						PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
+						PX4_DEBUG("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
 					}

 					if (sensor_gyro_sub.get().device_id == device_id) {
@@ -56,7 +56,7 @@ typedef struct sdb_config {
 	int num_runs; ///< number of runs
 	int run_duration; ///< duration of a single run [ms]
 	bool synchronized; ///< call fsync after each block?
-	bool aligned;
+	int unaligned;
 	unsigned int total_blocks_written;
 } sdb_config_t;

@@ -85,6 +85,7 @@ static void usage()
 	PRINT_MODULE_USAGE_PARAM_FLAG('k', "Keep the test file", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('s', "Call fsync after each block (default=at end of each run)", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('u', "Test performance with unaligned data", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('U', "Test performance with forced byte unaligned data", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('v', "Verify data and block number", true);
 }

@@ -100,10 +101,11 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[])
 	cfg.synchronized = false;
 	cfg.num_runs = 5;
 	cfg.run_duration = 2000;
-	cfg.aligned = true;
+	cfg.unaligned = 0;
 	uint8_t *block = nullptr;
+	uint8_t *block_alloc = nullptr;

-	while ((ch = px4_getopt(argc, argv, "b:r:d:ksuv", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:ksuUv", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			block_size = strtol(myoptarg, nullptr, 0);
@@ -126,7 +128,11 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[])
 			break;

 		case 'u':
-			cfg.aligned = false;
+			cfg.unaligned = 2;
+			break;
+
+		case 'U':
+			cfg.unaligned = 1;
 			break;

 		case 'v':
@@ -153,11 +159,24 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[])
 	}

 	//create some data block
-	if (cfg.aligned) {
-		block = (uint8_t *)px4_cache_aligned_alloc(block_size);
+	if (cfg.unaligned == 0) {
+		block_alloc = (uint8_t *)px4_cache_aligned_alloc(block_size);
+		block = block_alloc;

 	} else {
-		block = (uint8_t *)malloc(block_size);
+		block_alloc = (uint8_t *)malloc(block_size + 4);
+
+		if (block_alloc) {
+			// Force odd byte alignment
+			if (cfg.unaligned == 1 && ((uintptr_t)block_alloc % 0x1) == 0) {
+				block = block_alloc + 1;
+
+			} else {
+				block = block_alloc;
+			}
+		}
+
+		printf("Block ptr %p\n", block);
 	}

 	if (!block) {
@@ -179,7 +198,7 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[])
 		read_test(bench_fd, &cfg, block, block_size);
 	}

-	free(block);
+	free(block_alloc);
 	close(bench_fd);

 	if (!keep) {
@@ -259,15 +278,29 @@ void write_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size)
 int read_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size)
 {
 	uint8_t *read_block = nullptr;
+	uint8_t *block_alloc = nullptr;

 	PX4_INFO("");
 	PX4_INFO("Testing Sequential Read Speed of %d blocks", cfg->total_blocks_written);

-	if (cfg->aligned) {
-		read_block = (uint8_t *)px4_cache_aligned_alloc(block_size);
+	if (cfg->unaligned == 0) {
+		block_alloc = (uint8_t *)px4_cache_aligned_alloc(block_size);
+		read_block = block_alloc;

 	} else {
-		read_block = (uint8_t *)malloc(block_size);
+		block_alloc = (uint8_t *)malloc(block_size + 4);
+
+		if (block_alloc) {
+			// Force odd byte alignment
+			if (cfg->unaligned == 1 && ((uintptr_t)block_alloc % 0x1) == 0) {
+				read_block = block_alloc + 1;
+
+			} else {
+				read_block = block_alloc;
+			}
+		}
+
+		printf("Read Block ptr %p\n", read_block);
 	}

 	if (!read_block) {
@@ -331,6 +364,6 @@ int read_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size)

 	PX4_INFO("  Avg   : %8.2lf KB/s %d blocks read and verified", (double)block_size * total_blocks / total_elapsed / 1024.,
 		 total_blocks);
-	free(read_block);
+	free(block_alloc);
 	return 0;
 }
Author	SHA1	Message	Date
Peter van der Perk	7bbc73b390	Update NuttX (Testing Needs merge it first)	2024-05-04 21:47:55 +02:00
Peter van der Perk	beccd10f66	rc_input: Add SINGLEWIRE_MANUAL_DUPLEX for manually changing TXDIR Fixes telemetry on fmu-v6xrt	2024-05-04 21:47:35 +02:00
Beat Küng	9e6dcb1f60	fix mavlink: cmd_logging_{start,stop}_acknowledgement flags were not reset Regression from https://github.com/PX4/PX4-Autopilot/pull/23043 Also avoids a race condition by making sure the command ack is handled before sending out the mavlink message (in case an external component reacts immediately to the mavlink message).	2024-05-03 06:59:55 +02:00
Alex Klimaj	36ea872e72	drivers: adis16507 reschedule reset after failed self test	2024-05-02 17:52:26 -04:00
Daniel Agar	224d6f2fa7	ekf2: ekf_helper.cpp remove duplicate method comments (comment on declaration only, not definition)	2024-05-02 17:40:58 -04:00
Daniel Agar	c1fc893cca	ekf2: move gyro/accel/wind covariance reset helpers to covariance.cpp	2024-05-02 17:40:58 -04:00
Daniel Agar	63c2ea33c1	ekf2: move Ekf::resetQuatStateYaw() to yaw_fusion.cpp	2024-05-02 17:40:58 -04:00
Daniel Agar	1ca4056b6a	ekf2: delete unused Ekf::resetImuBias()	2024-05-02 17:40:58 -04:00
Daniel Agar	6b3b66619b	ekf2: move baro dynamic pressure compensation to aid_sources/barometer	2024-05-02 17:40:58 -04:00
Daniel Agar	4f0eb72fc9	ekf2: move IMU down sampler to imu_down_sampler/	2024-05-02 17:40:58 -04:00
Daniel Agar	58637d3825	ekf2: move terrain estimator and derivation to terrain_estimator/	2024-05-02 17:40:58 -04:00
Daniel Agar	58de8cbb77	ekf2: move fake_height, fake_pos, zero_innovation_heading to aid_sources/	2024-05-02 17:40:58 -04:00
Daniel Agar	49c782bad9	ekf2: move bias estimators to bias_estimtor/	2024-05-02 17:40:58 -04:00
Daniel Agar	e262fde4dc	ekf2: move aux global position fusion to aid_sources/aux_global_position	2024-05-02 17:40:58 -04:00
Daniel Agar	b8d46e60a5	ekf2: move mag fusion to aid_sources/magnetometer	2024-05-02 17:40:58 -04:00
Daniel Agar	3f6c3e0649	ekf2: move output predictor to output_predictor/	2024-05-02 17:40:58 -04:00
Daniel Agar	24fdd696cb	ekf2: move range finder files to aid_sources/range_finder	2024-05-02 17:40:58 -04:00
Daniel Agar	3dbd3f8a1a	ekf2: move airspeed fusion file to aid_sources/airspeed	2024-05-02 17:40:58 -04:00
Daniel Agar	789b2b3d8a	ekf2: move sideslip fusion file to aid_sources/sideslip	2024-05-02 17:40:58 -04:00
Daniel Agar	eb8ee74066	ekf2: move baro height file to aid_sources/barometer	2024-05-02 17:40:58 -04:00
Daniel Agar	de178b1435	ekf2: move gravity fusion file to aid_sources/gravity	2024-05-02 17:40:58 -04:00
Daniel Agar	78f2ccbb60	ekf2: move optical flow files to aid_sources/optical_flow	2024-05-02 17:40:58 -04:00
Daniel Agar	fcf94e7670	ekf2: move GNSS files to aid_sources/gnss	2024-05-02 17:40:58 -04:00
Daniel Agar	31ae5b77fe	ekf2: move drag_fusion file to aid_sources/drag	2024-05-02 17:40:58 -04:00
Daniel Agar	c3fb0b1090	ekf2: move auxvel file to aid_sources/auxvel	2024-05-02 17:40:58 -04:00
Daniel Agar	b5d1e87368	ekf2: move EV files to aid_sources/external_vision	2024-05-02 17:40:58 -04:00
Peter van der Perk	f382e585e8	sd_bench: Add U option for forcing byte aligned Co-authored-by: David Sidrane <david.sidrane@nscdg.com>	2024-05-02 12:33:25 -04:00
Daniel Agar	c64104e9f1	sensors/vehicle_angular_velocity: silence gyro selection fallback warning (PX4_WARN -> PX4_DEBUG) - this warning was to catch any potential errors in sensor selection relative to what's actually available, we don't need to complain about initial selection before the EKF selector is available	2024-05-02 11:53:31 -04:00