Merge branch 'main' into dev/sbgecom_driver

boards/ark/fmu-v6x/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # board specific defaults
 #------------------------------------------------------------------------------
 
+param set-default EKF2_MULTI_IMU 0
+
 # Mavlink ethernet (CFG 1000)
 param set-default MAV_2_CONFIG 1000
 param set-default MAV_2_BROADCAST 1
@@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
 param set-default SENS_EN_THERMAL 1
+param set-default SENS_IMU_MODE 1
 param set-default SENS_IMU_TEMP 10.0
 #param set-default SENS_IMU_TEMP_FF 0.0
 #param set-default SENS_IMU_TEMP_I 0.025

boards/ark/pi6x/init/rc.board_defaults

@@ -32,10 +32,11 @@ then
 	param set-default SENS_TEMP_ID 2490378
 fi
 
-param set-default EKF2_MULTI_IMU 2
+param set-default EKF2_MULTI_IMU 0
 param set-default EKF2_OF_CTRL 1
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
 param set-default EKF2_RNG_QLTY_T 0.1
 
 param set-default SENS_FLOW_RATE 150
+param set-default SENS_IMU_MODE 1

boards/holybro/kakutef7/default.px4board

@@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/holybro/kakuteh7/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/holybro/kakuteh7mini/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/holybro/kakuteh7v2/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/modalai/fc-v1/default.px4board

@@ -104,3 +104,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set

boards/px4/fmu-v2/default.px4board

@@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/px4/fmu-v5/cryptotest.px4board

@@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_BOARD_CRYPTO=y
 CONFIG_DRIVERS_STUB_KEYSTORE=y
 CONFIG_DRIVERS_SW_CRYPTO=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
 CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"

boards/px4/fmu-v5/rover.px4board

@@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v5/stackcheck.px4board

@@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y

boards/px4/fmu-v5x/rover.px4board

@@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6c/rover.px4board

@@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6u/rover.px4board

@@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6x/multicopter.px4board

@@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_COMMON_RC=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v6x/rover.px4board

@@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/sitl/default.px4board

@@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

src/modules/ekf2/Kconfig

@@ -34,7 +34,7 @@ depends on MODULES_EKF2
 menuconfig EKF2_AUX_GLOBAL_POSITION
 depends on MODULES_EKF2
 	bool "aux global position fusion support"
-	default n
+	default y
 	---help---
 		EKF2 auxiliary global position fusion support.
 

src/modules/logger/logged_topics.cpp

@@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("autotune_attitude_control_status", 100);
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
-	add_optional_topic("can_interface_status", 10);
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
 	add_topic("config_overrides");
@@ -251,6 +250,10 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("yaw_estimator_status");
 
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
+
+#ifdef CONFIG_BOARD_UAVCAN_INTERFACES
+	add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES);
+#endif
 }
 
 void LoggedTopics::add_high_rate_topics()

src/modules/mc_att_control/mc_att_control.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2025 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
@@ -47,6 +47,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
+#include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -98,9 +99,11 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -118,8 +121,10 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
 
-	float _man_yaw_sp{0.f};                 /**< current yaw setpoint in manual mode */
-	float _man_tilt_max;                    /**< maximum tilt allowed for manual flight [rad] */
+	float _hover_thrust{NAN};
+
+	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
+	float _man_tilt_max{0.f};			/**< maximum tilt allowed for manual flight [rad] */
 
 	SlewRate<float> _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air
 	SlewRate<float> _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2025 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
@@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
 	float thrust = 0.f;
 
+	{
+		hover_thrust_estimate_s hte;
+
+		if (_hover_thrust_estimate_sub.update(&hte)) {
+			if (hte.valid) {
+				_hover_thrust = hte.hover_thrust;
+			}
+		}
+	}
+
+	if (!PX4_ISFINITE(_hover_thrust)) {
+		_hover_thrust = _param_mpc_thr_hover.get();
+	}
+
+	// throttle_stick_input is in range [-1, 1]
 	switch (_param_mpc_thr_curve.get()) {
 	case 1: // no rescaling to hover throttle
 		thrust = math::interpolate(throttle_stick_input, -1.f, 1.f,
 					   _manual_throttle_minimum.getState(), _param_mpc_thr_max.get());
 		break;
 
-	default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle
-		thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f},
-		{_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
+	default: // 0 or other: rescale to hover throttle at 0 stick input
+		thrust = math::interpolateNXY(throttle_stick_input,
+		{-1.f, 0.f, 1.f},
+		{_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()});
 		break;
 	}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run()
 		}
 	}
 
-	// new local position setpoint needed every iteration
-	if (!_vehicle_local_position_setpoint_sub.updated()) {
-		return;
-	}
-
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run()
 
 		_hover_thrust_ekf.predict(dt);
 
-		vehicle_local_position_setpoint_s local_pos_sp;
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+
+		vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
+		control_allocator_status_s control_allocator_status;
+
+		if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)
+		    && _control_allocator_status_sub.update(&control_allocator_status)
+		    && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms)
+		    && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms)
+		   ) {
+			matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz);
+			matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust);
+			matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated;
+
+			const matrix::Quatf q_att{vehicle_attitude.q};
+			matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated);
 
-		if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) {
-			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
+			if (PX4_ISFINITE(thrust_allocated(2))) {
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
 				// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
@@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run()
 									1.f);
 
 				_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
-				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2));
 
 				bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
 
@@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run()
 				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
 				_valid = _valid_hysteresis.get_state();
 
-				publishStatus(local_pos.timestamp);
+				publishStatus(vehicle_thrust_setpoint.timestamp);
 			}
 		}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -54,10 +54,12 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/control_allocator_status.h>
 
 #include "zero_order_hover_thrust_ekf.hpp"
 
@@ -101,9 +103,12 @@ class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrust
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
 
 	hrt_abstime _timestamp_last{0};