Merge branch 'PX4:main' into pr_add_bmm350

Makefile

@@ -549,7 +549,7 @@ distclean:
 # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error.
 %:
 	$(if $(filter $(FIRST_ARG),$@), \
-		$(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#)
+		$(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) list_config_targets' to get a list of all possible [configuration] targets."),@#)
 
 # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069)
 help:

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -15,14 +15,29 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 0
 param set-default SENS_EN_MAGSIM 1
-param set-default SENS_EN_ARSPDSIM 1
+param set-default SENS_EN_ARSPDSIM 0
 # We can arm and drive in manual mode when it slides and GPS check fails:
 param set-default COM_ARM_WO_GPS 1
 
-# Set Differential Drive Kinematics Library parameters:
-param set RD_WHEEL_BASE 0.9
-param set RD_WHEEL_RADIUS 0.22
-param set RD_WHEEL_SPEED 10.0	# Maximum wheel speed rad/s, approx 8 km/h
+# Rover parameters
+param set-default RD_WHEEL_TRACK 0.9
+param set-default RD_MAN_YAW_SCALE 0.1
+param set-default RD_YAW_RATE_I 0.1
+param set-default RD_YAW_RATE_P 5
+param set-default RD_MAX_ACCEL 1
+param set-default RD_MAX_JERK 3
+param set-default RD_MAX_SPEED 8
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_I 0.1
+param set-default RD_MAX_YAW_RATE 30
+param set-default RD_MISS_SPD_DEF 8
+param set-default RD_TRANS_DRV_TRN 0.349066
+param set-default RD_TRANS_TRN_DRV 0.174533
+
+# Pure pursuit parameters
+param set-default PP_LOOKAHD_MAX 30
+param set-default PP_LOOKAHD_MIN 2
+param set-default PP_LOOKAHD_GAIN 1
 
 # Actuator mapping - set SITL motors/servos output parameters:
 
@@ -36,7 +51,7 @@ param set-default SIM_GZ_WH_FUNC1 101 # right wheel
 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
 #param set-default SIM_GZ_WH_MIN2 0
 #param set-default SIM_GZ_WH_MAX2 200
-#aram set-default SIM_GZ_WH_DIS2 100
+#param set-default SIM_GZ_WH_DIS2 100
 #param set-default SIM_GZ_WH_FAIL2 100
 
 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels

boards/mro/ctrl-zero-classic/nuttx-config/include/board.h

@@ -197,6 +197,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h

@@ -197,6 +197,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/ctrl-zero-h7/nuttx-config/include/board.h

@@ -196,6 +196,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

boards/mro/pixracerpro/nuttx-config/include/board.h

@@ -196,6 +196,12 @@
 
 #define STM32_FDCANCLK               STM32_HSE_FREQUENCY
 
+/* UART clock selection */
+/* reset to default to overwrite any changes done by any bootloader */
+
+#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC
+#define STM32_RCC_D2CCIP2R_USART16_SEL     RCC_D2CCIP2R_USART16SEL_RCC
+
 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
 

msg/CMakeLists.txt

@@ -146,6 +146,7 @@ set(msg_files
 	MountOrientation.msg
 	ModeCompleted.msg
 	NavigatorMissionItem.msg
+	NavigatorStatus.msg
 	NormalizedUnsignedSetpoint.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg

msg/FailsafeFlags.msg

@@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure   # vehicle in fixed-wing system failure fai
 bool wind_limit_exceeded              # Wind limit exceeded
 bool flight_time_limit_exceeded       # Maximum flight time exceeded
 bool local_position_accuracy_low      # Local position estimate has dropped below threshold, but is currently still declared valid
+bool navigator_failure        	      # Navigator failed to execute a mode
 
 # Failure detector
 bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)

msg/NavigatorStatus.msg

@@ -0,0 +1,9 @@
+# Current status of a Navigator mode
+# The possible values of nav_state are defined in the VehicleStatus msg.
+uint64 timestamp  # time since system start (microseconds)
+
+uint8 nav_state   # Source mode (values in VehicleStatus)
+uint8 failure     # Navigator failure enum
+
+uint8 FAILURE_NONE = 0
+uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground

msg/VehicleAttitudeSetpoint.msg

@@ -1,9 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
 
-float32 roll_body		# body angle in NED frame (can be NaN for FW)
-float32 pitch_body		# body angle in NED frame (can be NaN for FW)
-float32 yaw_body		# body angle in NED frame (can be NaN for FW)
-
 float32 yaw_sp_move_rate	# rad/s (commanded by user)
 
 # For quaternion-based attitude control

src/drivers/imu/invensense/icm40609d/ICM40609D.cpp

@@ -203,57 +203,45 @@ void ICM40609D::RunImpl()
 
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
+			uint8_t samples = FIFOReadCount();
 
-			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
-				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
-
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-					samples = _fifo_gyro_samples;
+			if (samples == 0) {
+				perf_count(_fifo_empty_perf);
 
-				} else {
-					perf_count(_drdy_missed_perf);
+			} else {
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+					samples--;
 				}
 
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
-
-				if (fifo_count >= FIFO::SIZE) {
+				if (samples > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
+					samples = 0;
+				}
+			}
 
-				} else if (fifo_count == 0) {
-					perf_count(_fifo_empty_perf);
+			bool success = false;
 
-				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+			if (samples > 0) {
+				if (_data_ready_interrupt_enabled) {
+					// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+					const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
+					if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+						timestamp_sample = drdy_timestamp_sample;
+						samples = _fifo_gyro_samples;
 
-					if (samples > FIFO_MAX_SAMPLES) {
-						// not technically an overflow, but more samples than we expected or can publish
-						FIFOReset();
-						perf_count(_fifo_overflow_perf);
-						samples = 0;
+					} else {
+						perf_count(_drdy_missed_perf);
 					}
-				}
-			}
 
-			bool success = false;
+					// push backup schedule back
+					ScheduleDelayed(_fifo_empty_interval_us * 2);
+				}
 
-			if (samples >= 1) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate)
 
 void ICM40609D::ConfigureFIFOWatermark(uint8_t samples)
 {
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
 	for (auto &r : _register_bank0_cfg) {
 		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
 			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
+			r.set_bits = samples & 0xFF;
 
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
 		}
 	}
 }
@@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
 	// check FIFO header in every sample
 	uint8_t valid_samples = 0;
 
-	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+	for (int i = 0; i < samples; i++) {
 		bool valid = true;
 
 		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
@@ -599,6 +566,9 @@ void ICM40609D::FIFOReset()
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 
+	// Read INT_STATUS to clear
+	RegisterRead(Register::BANK_0::INT_STATUS);
+
 	// reset while FIFO is disabled
 	_drdy_timestamp_sample.store(0);
 }

src/drivers/imu/invensense/icm40609d/ICM40609D.hpp

@@ -160,10 +160,11 @@ class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
 	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{10};
+	static constexpr uint8_t size_register_bank0_cfg{11};
 	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
 		// Register                        | Set bits, Clear bits
 		{ Register::BANK_0::INT_CONFIG,    INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::INTF_CONFIG0,  INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0},
 		{ Register::BANK_0::FIFO_CONFIG,   FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
 		{ Register::BANK_0::PWR_MGMT0,     PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
 		{ Register::BANK_0::GYRO_CONFIG0,  GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },

src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp

@@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t {
 
 	SIGNAL_PATH_RESET = 0x4B,
 
+	INTF_CONFIG0      = 0x4C,
+
 	PWR_MGMT0         = 0x4E,
 	GYRO_CONFIG0      = 0x4F,
 	ACCEL_CONFIG0     = 0x50,
@@ -132,6 +134,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t {
 	FIFO_FLUSH      = Bit1,
 };
 
+// INTF_CONFIG0
+enum INTF_CONFIG0_BIT : uint8_t {
+	FIFO_HOLD_LAST_DATA_EN = Bit7,
+	FIFO_COUNT_REC = Bit6,
+	FIFO_COUNT_ENDIAN = Bit5,
+	SENSOR_DATA_ENDIAN = Bit4,
+	UI_SIFS_CFG_DISABLE_SPI = Bit1,
+	UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0
+};
+
 // PWR_MGMT0
 enum PWR_MGMT0_BIT : uint8_t {
 	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks
 	checks/manualControlCheck.cpp
 	checks/missionCheck.cpp
 	checks/modeCheck.cpp
+	checks/navigatorCheck.cpp
 	checks/offboardCheck.cpp
 	checks/openDroneIDCheck.cpp
 	checks/parachuteCheck.cpp

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -49,6 +49,7 @@
 #include "checks/escCheck.hpp"
 #include "checks/estimatorCheck.hpp"
 #include "checks/failureDetectorCheck.hpp"
+#include "checks/navigatorCheck.hpp"
 #include "checks/gyroCheck.hpp"
 #include "checks/imuConsistencyCheck.hpp"
 #include "checks/loggerCheck.hpp"
@@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams
 	EscChecks _esc_checks;
 	EstimatorChecks _estimator_checks;
 	FailureDetectorChecks _failure_detector_checks;
+	NavigatorChecks _navigator_checks;
 	GyroChecks _gyro_checks;
 	ImuConsistencyChecks _imu_consistency_checks;
 	LoggerChecks _logger_checks;
@@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams
 		&_esc_checks,
 		&_estimator_checks,
 		&_failure_detector_checks,
+		&_navigator_checks,
 		&_gyro_checks,
 		&_imu_consistency_checks,
 		&_logger_checks,