Jenkinsfile-compile fix invalid boards

- since last_us is set to 0 every time the bias is not observable, the
  total time was also reset -> needed 30 consecutive seconds in mag 3D
  to be declared "stable"
- after landing, the mag_aligned_in_flight flag is reset. Using this for
  bias validity makes it invalid before we have a chance to save it to
  the calibration.

.ci/Jenkinsfile-compile

@@ -40,6 +40,8 @@ pipeline {
                      "ark_can-flow_default",
                      "ark_can-gps_canbootloader",
                      "ark_can-gps_default",
+                     "ark_cannode_canbootloader",
+                     "ark_cannode_default",
                      "ark_can-rtk-gps_canbootloader",
                      "ark_can-rtk-gps_default",
                      "ark_fmu-v6x_bootloader",
@@ -53,8 +55,10 @@ pipeline {
                      "cuav_nora_default",
                      "cuav_x7pro_default",
                      "cubepilot_cubeorange_default",
+                     "cubepilot_cubeorangeplus_default",
                      "cubepilot_cubeyellow_default",
                      "diatone_mamba-f405-mk2_default",
+                     "flywoo_gn-f405_default",
                      "freefly_can-rtk-gps_canbootloader",
                      "freefly_can-rtk-gps_default",
                      "holybro_can-gps-v1_canbootloader",
@@ -72,7 +76,7 @@ pipeline {
                      "matek_h743_default",
                      "modalai_fc-v1_default",
                      "modalai_fc-v2_default",
-                     "modalai_voxl2-io_default",
+                     "mro_ctrl-zero-classic_default",
                      "mro_ctrl-zero-f7_default",
                      "mro_ctrl-zero-f7-oem_default",
                      "mro_ctrl-zero-h7-oem_default",
@@ -85,6 +89,7 @@ pipeline {
                      "nxp_fmuk66-v3_default",
                      "nxp_fmuk66-v3_socketcan",
                      "nxp_fmurt1062-v1_default",
+                     "nxp_mr-canhubk3_default",
                      "nxp_ucans32k146_canbootloader",
                      "nxp_ucans32k146_default",
                      "omnibus_f4sd_default",

.github/PULL_REQUEST_TEMPLATE.md

@@ -23,7 +23,7 @@ For release notes:
 ```
 Feature/Bugfix XYZ
 New parameter: XYZ_Z
-Documentation: Need to clarfiy page ... / done, read docs.px4.io/...
+Documentation: Need to clarify page ... / done, read docs.px4.io/...
 ```
 
 ### Alternatives

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -17,6 +17,7 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -48,35 +48,32 @@ param set-default PWM_MAIN_REV 96 # invert both elevons
 param set-default EKF2_MULTI_IMU 0
 param set-default SENS_IMU_MODE 1
 
-param set-default NPFG_PERIOD 12
-param set-default FW_PR_I 0.2
-param set-default FW_PR_P 0.2
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_FF 0.1
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_P 0.2
-param set-default FW_THR_TRIM 0.33
-param set-default FW_THR_MAX 0.6
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.2
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
 param set-default FW_THR_MIN 0.05
-param set-default FW_T_ALT_TC 2
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-param set-default FW_T_TAS_TC 2
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
 
 param set-default MC_AIRMODE 1
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
 param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
 
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-
+param set-default VT_ARSP_TRANS 10
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.5
+param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
-param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
 
 param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter

@@ -0,0 +1,74 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 0
+
+parm set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -60,6 +60,7 @@ px4_add_romfs_files(
 	1042_gazebo-classic_tiltrotor
 	1043_gazebo-classic_standard_vtol_drop
 	1044_gazebo-classic_plane_lidar
+	1045_gazebo-classic_quadtailsitter
 	1060_gazebo-classic_rover
 	1061_gazebo-classic_r1_rover
 	1062_flightgear_tf-r1

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -20,6 +20,7 @@
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
 
 param set UAVCAN_ENABLE 0
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -31,3 +31,4 @@ param set-default CA_SV_CS1_TYPE 6
 param set-default MAV_TYPE 19
 param set-default VT_TYPE 0
 param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_B_TRANS_DUR 5

ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2

@@ -34,9 +34,9 @@ param set-default CA_ROTOR3_PX -0.25
 param set-default CA_ROTOR3_PY 0.25
 param set-default CA_ROTOR3_KM -0.05
 
-param set-default PWM_AUX_FUNC1 101
-param set-default PWM_AUX_FUNC2 102
-param set-default PWM_AUX_FUNC3 103
-param set-default PWM_AUX_FUNC4 104
-param set-default PWM_AUX_TIM0 -4
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_TIM0 -4
 

ROMFS/px4fmu_common/init.d/rcS

@@ -254,6 +254,8 @@ else
 	#
 	rgbled start -X -q
 	rgbled_ncp5623c start -X -q
+	rgbled_lp5562 start -X -q
+	rgbled_is31fl3195 start -X -q
 
 	#
 	# Override parameters from user configuration file.

boards/bitcraze/crazyflie/src/board_config.h

@@ -140,15 +140,7 @@
 
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-
 
 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */

boards/bitcraze/crazyflie21/src/board_config.h

@@ -141,15 +141,7 @@
 
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-
 
 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */

boards/nxp/fmuk66-e/default.px4board

@@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/nxp/fmurt1062-v1/default.px4board

@@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_COMMON_UWB=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y

boards/nxp/ucans32k146/default.px4board

@@ -13,6 +13,7 @@ CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_UAVCANNODE_ARMING_STATUS=y
 CONFIG_UAVCANNODE_BEEP_COMMAND=y
 CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y

boards/px4/fmu-v6x/default.px4board

@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
+CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
@@ -41,6 +42,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

msg/CMakeLists.txt

@@ -174,6 +174,7 @@ set(msg_files
 	SensorSelection.msg
 	SensorsStatus.msg
 	SensorsStatusImu.msg
+	SensorUwb.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -190,8 +191,6 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
-	UwbDistance.msg
-	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg

msg/SensorUwb.msg

@@ -0,0 +1,34 @@
+# UWB distance contains the distance information measured by an ultra-wideband positioning system,
+# such as Pozyx or NXP Rddrone.
+
+uint64 	timestamp		# time since system start (microseconds)
+
+uint32 	sessionid		# UWB SessionID
+uint32 	time_offset		# Time between Ranging Rounds in ms
+uint32 	counter			# Number of Ranges since last Start of Ranging
+uint16 	mac			# MAC adress of Initiator (controller)
+
+uint16 	mac_dest		# MAC adress of Responder (Controlee)
+uint16 	status			# status feedback #
+uint8 	nlos			# None line of site condition y/n
+float32 distance		# distance in m to the UWB receiver
+
+
+#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees
+float32 aoa_azimuth_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_elevation_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_azimuth_resp	# Angle of arrival of first incomming RX msg at the responder
+float32 aoa_elevation_resp	# Angle of arrival of first incomming RX msg at the responder
+
+# Figure of merit for the angle measurements
+uint8 aoa_azimuth_fom		# AOA Azimuth FOM
+uint8 aoa_elevation_fom		# AOA Elevation FOM
+uint8 aoa_dest_azimuth_fom	# AOA Azimuth FOM
+uint8 aoa_dest_elevation_fom	# AOA Elevation FOM
+
+# Initiator physical configuration
+uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+				# Standard configuration is Antennas facing down and azimuth aligened in forward direction
+float32 offset_x		# UWB initiator offset in X axis (NED drone frame)
+float32 offset_y		# UWB initiator offset in Y axis (NED drone frame)
+float32 offset_z		# UWB initiator offset in Z axis (NED drone frame)

msg/UwbDistance.msg

@@ -1,15 +0,0 @@
-# UWB distance contains the distance information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp		# time since system start (microseconds)
-uint32 	time_uwb_ms		# Time of UWB device in ms
-uint32 	counter			# Number of Ranges since last Start of Ranging
-uint32 	sessionid		# UWB SessionID
-uint32 	time_offset		# Time between Ranging Rounds in ms
-uint16 	status			# status feedback #
-
-uint16[12] anchor_distance	# distance in cm to each UWB Anchor (UWB Device which takes part in Ranging)
-bool[12] nlos			# Visual line of sight yes/no
-float32[12] aoafirst		# Angle of arrival of first incoming RX msg
-
-float32[3] position		# Position of the Landing point in relation to the Drone (x,y,z in Meters NED)

msg/UwbGrid.msg

@@ -1,25 +0,0 @@
-# UWB report message contains the grid information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp	# time since system start (microseconds)
-uint16 	initator_time	# time to synchronize clocks (microseconds)
-uint8 	num_anchors	# Number of anchors
-
-float64[4] target_gps	# GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North)
-
-# Grid position information
-# Position in x,y,z in  (x,y,z in centimeters NED)
-# staring with POI and Anchor 0 up to Anchor 11
-int16[3] target_pos 	# Point of Interest, mostly landing Target x,y,z
-int16[3] anchor_pos_0
-int16[3] anchor_pos_1
-int16[3] anchor_pos_2
-int16[3] anchor_pos_3
-int16[3] anchor_pos_4
-int16[3] anchor_pos_5
-int16[3] anchor_pos_6
-int16[3] anchor_pos_7
-int16[3] anchor_pos_8
-int16[3] anchor_pos_9
-int16[3] anchor_pos_10
-int16[3] anchor_pos_11

src/drivers/actuators/modal_io/modal_io.cpp

@@ -48,7 +48,6 @@ const char *_device;
 
 ModalIo::ModalIo() :
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)),
-	_mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
 {

src/drivers/actuators/modal_io/modal_io.hpp

@@ -174,7 +174,7 @@ private:
 	} led_rsc_t;
 
 	ch_assign_t		_output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
-	MixingOutput 		_mixing_output;
+	MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;

src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp

@@ -250,7 +250,7 @@ ICP201XX::RunImpl()
 	case STATE::CONFIG: {
 			if (configure()) {
 				_state = STATE::WAIT_READ;
-				ScheduleDelayed(10_ms);
+				ScheduleDelayed(30_ms);
 
 			} else {
 				if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {

src/drivers/drv_pwm_output.h

@@ -61,41 +61,11 @@ __BEGIN_DECLS
  */
 #define PWM_LOWEST_MIN 90
 
-/**
- * Default value for a shutdown motor
- */
-#define PWM_MOTOR_OFF	900
-
-/**
- * Default minimum PWM in us
- */
-#define PWM_DEFAULT_MIN 1000
-
-/**
- * Highest PWM allowed as the minimum PWM
- */
-#define PWM_HIGHEST_MIN 1600
-
 /**
  * Highest maximum PWM in us
  */
 #define PWM_HIGHEST_MAX 2500
 
-/**
- * Default maximum PWM in us
- */
-#define PWM_DEFAULT_MAX 2000
-
-/**
- * Default trim PWM in us
- */
-#define PWM_DEFAULT_TRIM 0
-
-/**
- * Lowest PWM allowed as the maximum PWM
- */
-#define PWM_LOWEST_MAX 200
-
 #endif // not PX4_PWM_ALTERNATE_RANGES
 
 /**

src/drivers/drv_sensor.h

@@ -157,6 +157,8 @@
 #define DRV_LED_DEVTYPE_RGBLED        0x7a
 #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
 #define DRV_LED_DEVTYPE_RGBLED_IS31FL3195    0xbf
+#define DRV_LED_DEVTYPE_RGBLED_LP5562    0xc0
+
 #define DRV_BAT_DEVTYPE_SMBUS         0x7c
 #define DRV_SENS_DEVTYPE_IRLOCK       0x7d
 #define DRV_SENS_DEVTYPE_PCF8583      0x7e

src/drivers/dshot/DShot.h

@@ -143,7 +143,7 @@ private:
 
 	void handle_vehicle_commands();
 
-	MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};
 
 	Telemetry *_telemetry{nullptr};

src/drivers/gps/gps.cpp

@@ -60,6 +60,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
@@ -203,7 +204,7 @@ private:
 
 	const Instance 			_instance;
 
-	uORB::Subscription		     _orb_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
 	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::Publication<gps_dump_s>	     _dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			     *_dump_to_device{nullptr};
@@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic()
 	// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
 	if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
 
-		for (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) {
-			if (_orb_inject_data_sub.ChangeInstance(i)) {
-				if (_orb_inject_data_sub.copy(&msg)) {
+		for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
+			const bool exists = _orb_inject_data_sub[instance].advertised();
+
+			if (exists) {
+				if (_orb_inject_data_sub[instance].copy(&msg)) {
 					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
 						// Remember that we already did a copy on this instance.
 						already_copied = true;
-						_selected_rtcm_instance = i;
+						_selected_rtcm_instance = instance;
 						break;
 					}
 				}
@@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic()
 		}
 	}
 
-	// Reset instance in case we didn't actually want to switch
-	_orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance);
-
 	bool updated = already_copied;
 
 	// Limit maximum number of GPS injections to 8 since usually
@@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		updated = _orb_inject_data_sub.update(&msg);
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
 
 	} while (updated && num_injections < max_num_injections);
 }

src/drivers/lights/CMakeLists.txt

@@ -35,4 +35,5 @@
 add_subdirectory(rgbled)
 add_subdirectory(rgbled_ncp5623c)
 add_subdirectory(rgbled_is31fl3195)
+add_subdirectory(rgbled_lp5562)
 #add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)

src/drivers/lights/Kconfig

@@ -5,6 +5,7 @@ menu "Lights"
         select DRIVERS_LIGHTS_RGBLED
         select DRIVERS_LIGHTS_RGBLED_NCP5623C
         select DRIVERS_LIGHTS_RGBLED_IS31FL3195
+        select DRIVERS_LIGHTS_RGBLED_LP5562
         ---help---
             Enable default set of light drivers
     rsource "*/Kconfig"

src/drivers/lights/rgbled_lp5562/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__rgbled_lp5562
+	MAIN rgbled_lp5562
+	SRCS
+		rgbled_lp5562.cpp
+	DEPENDS
+		drivers__device
+		led
+	)

src/drivers/lights/rgbled_lp5562/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_LIGHTS_RGBLED_LP5562
+	bool "rgbled lp5562"
+	default n
+	---help---
+		Enable support for RGBLED LP5562 driver

src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp

@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 rgbled_lp5562.cpp
+ *
+ * Driver for the RGB LED controller Texas Instruments LP5562 connected via I2C.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/device/i2c.h>
+#include <lib/led/led.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+
+
+using namespace time_literals;
+
+// The addresses are 0x60, 0x62, 0x64, 0x66 according to the datasheet page 27.
+// We specify 7bit addresses, hence 0x60 becomes 0x30.
+#define I2C_ADDR 0x30
+
+// Unfortunately, there is no WHO_AM_I or device id register, so
+// instead we query the W_CURRENT which has a certain pattern
+// after reset, and we don't use it or change it, so we don't have
+// to reset it and therefore don't mess with a device that we're
+// not sure what it is.
+static constexpr uint8_t LED_MAP_ADDR = 0x70;
+static constexpr uint8_t LED_MAP_ALL_PWM = 0b00000000;
+
+static constexpr uint8_t ENABLE_ADDR = 0x00;
+static constexpr uint8_t ENABLE_CHIP_EN = 0b01000000;
+
+static constexpr uint8_t CONFIG_ADDR = 0x08;
+static constexpr uint8_t CONFIG_ENABLE_INTERNAL_CLOCK = 0b00000001;
+
+static constexpr uint8_t RESET_ADDR = 0x0D;
+static constexpr uint8_t RESET_DO_RESET = 0xFF;
+
+static constexpr uint8_t B_PWM_ADDR = 0x02;
+
+static constexpr uint8_t B_CURRENT_ADDR = 0x05;
+
+static constexpr uint8_t W_CURRENT_ADDR = 0x0F;
+static constexpr uint8_t W_CURRENT_DEFAULT = 0b10101111;
+
+
+class RGBLED_LP5562: public device::I2C, public I2CSPIDriver<RGBLED_LP5562>
+{
+public:
+	RGBLED_LP5562(const I2CSPIDriverConfig &config);
+	virtual ~RGBLED_LP5562() = default;
+
+	static void print_usage();
+
+	int init() override;
+	int probe() override;
+
+	void RunImpl();
+
+private:
+	int read(uint8_t address, uint8_t *data, unsigned count);
+	int write(uint8_t address, uint8_t *data, unsigned count);
+	int send_led_rgb(uint8_t r, uint8_t g, uint8_t b);
+
+	LedController _led_controller;
+	uint8_t _current = 175; // matching default current of 17.5mA
+};
+
+RGBLED_LP5562::RGBLED_LP5562(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config)
+{
+	_current = config.custom1;
+}
+
+int
+RGBLED_LP5562::init()
+{
+	int ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	uint8_t command[1] = {ENABLE_CHIP_EN};
+	ret = write(ENABLE_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// We have to wait 500us after enable.
+	px4_usleep(500);
+
+	command[0] = CONFIG_ENABLE_INTERNAL_CLOCK;
+	ret = write(CONFIG_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	command[0] = LED_MAP_ALL_PWM;
+	ret = write(LED_MAP_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// Write all 3 colors at once.
+	uint8_t currents[3] = {_current, _current, _current};
+	ret = write(B_CURRENT_ADDR, currents, sizeof(currents));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ScheduleNow();
+
+	return OK;
+}
+
+int
+RGBLED_LP5562::probe()
+{
+	uint8_t result[1] = {0};
+	int ret = read(W_CURRENT_ADDR, result, sizeof(result));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	_retries = 1;
+
+	return (result[0] == W_CURRENT_DEFAULT) ? OK : ERROR;
+}
+
+int
+RGBLED_LP5562::read(uint8_t address, uint8_t *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
+int
+RGBLED_LP5562::write(uint8_t address, uint8_t *data, unsigned count)
+{
+	uint8_t buf[4];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], count + 1, nullptr, 0);
+}
+
+void
+RGBLED_LP5562::RunImpl()
+{
+	if (should_exit()) {
+		send_led_rgb(0, 0, 0);
+		return;
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+
+		const uint8_t on = led_control_data.leds[0].brightness;
+
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			send_led_rgb(on, 0, 0);
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			send_led_rgb(0, on, 0);
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			send_led_rgb(0, 0, on);
+			break;
+
+		case led_control_s::COLOR_AMBER: // same as yellow
+		case led_control_s::COLOR_YELLOW:
+			send_led_rgb(on, on, 0);
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			send_led_rgb(on, 0, on);
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			send_led_rgb(0, on, on);
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			send_led_rgb(on, on, on);
+			break;
+
+		case led_control_s::COLOR_OFF:
+		default:
+			send_led_rgb(0, 0, 0);
+			break;
+		}
+
+	}
+
+	ScheduleDelayed(_led_controller.maximum_update_interval());
+}
+
+int
+RGBLED_LP5562::send_led_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	uint8_t leds[3] = {b, g, r};
+	return write(B_PWM_ADDR, leds, sizeof(leds));
+}
+
+void
+RGBLED_LP5562::print_usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for [LP5562](https://www.ti.com/product/LP5562) LED driver connected via I2C.
+
+This used in some GPS modules by Holybro for [PX4 status notification](../getting_started/led_meanings.md)
+
+The driver is included by default in firmware (KConfig key DRIVERS_LIGHTS_RGBLED_LP5562) and is always enabled.
+)DESCR_STR");
+	PRINT_MODULE_USAGE_NAME("rgbled_lp5562", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(I2C_ADDR);
+	PRINT_MODULE_USAGE_PARAM_FLOAT('u', 17.5f, 0.1f, 25.5f, "Current in mA", true);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int rgbled_lp5562_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = RGBLED_LP5562;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = I2C_ADDR;
+	cli.custom1 = 175;
+
+	while ((ch = cli.getOpt(argc, argv, "u:")) != EOF) {
+		switch (ch) {
+		case 'u':
+			float v = atof(cli.optArg());
+
+			if (v >= 0.1f && v <= 25.5f) {
+				cli.custom1 = ((uint8_t)(v * 10.f));
+
+			} else {
+				PX4_ERR("current out of range");
+				return -1;
+			}
+
+			break;
+		}
+	}
+
+
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP5562);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/linux_pwm_out/module.yaml

@@ -4,7 +4,7 @@ actuator_output:
     - param_prefix: PWM_MAIN
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/pca9685_pwm_out/module.yaml

@@ -4,7 +4,7 @@ actuator_output:
     - param_prefix: PCA9685
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/pwm_out/PWMOut.cpp

@@ -218,42 +218,55 @@ void PWMOut::update_params()
 
 	updateParams();
 
-	// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
+	// Automatically set PWM configuration when a channel is first assigned
 	if (!_first_update_cycle) {
 		for (size_t i = 0; i < _num_outputs; i++) {
 			if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 				int32_t output_function;
 
-				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-				    && output_function >= (int)OutputFunction::Servo1
-				    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-					int32_t val = 1500;
-					PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-					param_set(_mixing_output.disarmedParamHandle(i), &val);
+				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+					// Servos need PWM rate 50Hz and disramed value 1500us
+					if (output_function >= (int)OutputFunction::Servo1
+					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+						int32_t val = 1500;
+						PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+						param_set(_mixing_output.disarmedParamHandle(i), &val);
 
-					// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-					for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+						for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
 
-						uint32_t channels = io_timer_get_group(timer);
+							uint32_t channels = io_timer_get_group(timer);
 
-						if ((channels & (1u << i)) == 0) {
-							continue;
-						}
+							if ((channels & (1u << i)) == 0) {
+								continue;
+							}
 
-						if ((channels & previously_set_functions) == 0) { // None of the channels was set
-							char param_name[17];
-							snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+							if ((channels & previously_set_functions) == 0) { // None of the channels was set
+								char param_name[17];
+								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-							int32_t tim_config = 0;
-							param_t handle = param_find(param_name);
+								int32_t tim_config = 0;
+								param_t handle = param_find(param_name);
 
-							if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-								tim_config = 50;
-								PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-								param_set(handle, &tim_config);
+								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+									tim_config = 50;
+									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+									param_set(handle, &tim_config);
+								}
 							}
 						}
 					}
+
+					// Motors need a minimum value that idles the motor and have a deadzone at the top of the range
+					if (output_function >= (int)OutputFunction::Motor1
+					    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+						int32_t val = 1100;
+						PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+						param_set(_mixing_output.minParamHandle(i), &val);
+						val = 1900;
+						PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+						param_set(_mixing_output.maxParamHandle(i), &val);
+					}
 				}
 			}
 		}

src/drivers/pwm_out/module.yaml

@@ -5,7 +5,7 @@ actuator_output:
       param_prefix: '${PWM_MAIN_OR_AUX}'
       channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}']
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/px4io/module.yaml

@@ -7,7 +7,7 @@ actuator_output:
       channel_label_module_name_prefix: false
       timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/px4io/px4io.cpp

@@ -702,36 +702,48 @@ void PX4IO::update_params()
 				if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 					int32_t output_function;
 
-					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-					    && output_function >= (int)OutputFunction::Servo1
-					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-						int32_t val = 1500;
-						PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-						param_set(_mixing_output.disarmedParamHandle(i), &val);
+					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+						if (output_function >= (int)OutputFunction::Servo1
+						    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+							int32_t val = 1500;
+							PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+							param_set(_mixing_output.disarmedParamHandle(i), &val);
 
-						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-						for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
+							// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+							for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
 
-							uint32_t channels = _group_channels[timer];
+								uint32_t channels = _group_channels[timer];
 
-							if ((channels & (1u << i)) == 0) {
-								continue;
-							}
+								if ((channels & (1u << i)) == 0) {
+									continue;
+								}
 
-							if ((channels & previously_set_functions) == 0) { // None of the channels was set
-								char param_name[17];
-								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+								if ((channels & previously_set_functions) == 0) { // None of the channels was set
+									char param_name[17];
+									snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-								int32_t tim_config = 0;
-								param_t handle = param_find(param_name);
+									int32_t tim_config = 0;
+									param_t handle = param_find(param_name);
 
-								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-									tim_config = 50;
-									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-									param_set(handle, &tim_config);
+									if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+										tim_config = 50;
+										PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+										param_set(handle, &tim_config);
+									}
 								}
 							}
 						}
+
+						// Motors need a minimum value that idles the motor
+						if (output_function >= (int)OutputFunction::Motor1
+						    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+							int32_t val = 1100;
+							PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+							param_set(_mixing_output.minParamHandle(i), &val);
+							val = 1900;
+							PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+							param_set(_mixing_output.maxParamHandle(i), &val);
+						}
 					}
 				}
 			}

src/drivers/uavcan/sensors/gnss.cpp

@@ -467,29 +467,61 @@ void UavcanGnssBridge::update()
 // to work.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
-	// Limit maximum number of GPS injections to 6 since usually
-	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 6 packets thus guarantees, that at least a full injection
-	// data set is evaluated.
-	static constexpr size_t MAX_NUM_INJECTIONS = 6;
+	// We don't want to call copy again further down if we have already done a
+	// copy in the selection process.
+	bool already_copied = false;
+	gps_inject_data_s msg;
 
-	size_t num_injections = 0;
-	gps_inject_data_s gps_inject_data;
+	// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
+	if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
 
-	while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) {
-		// Write the message to the gps device. Note that the message could be fragmented.
-		// But as we don't write anywhere else to the device during operation, we don't
-		// need to assemble the message first.
-		if (_publish_rtcm_stream) {
-			PublishRTCMStream(gps_inject_data.data, gps_inject_data.len);
+		for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
+			const bool exists = _orb_inject_data_sub[instance].advertised();
+
+			if (exists) {
+				if (_orb_inject_data_sub[instance].copy(&msg)) {
+					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
+						// Remember that we already did a copy on this instance.
+						already_copied = true;
+						_selected_rtcm_instance = instance;
+						break;
+					}
+				}
+			}
 		}
-
-		if (_publish_moving_baseline_data) {
-			PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
-		}
-
-		num_injections++;
 	}
+
+	bool updated = already_copied;
+
+	// Limit maximum number of GPS injections to 8 since usually
+	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
+	// Looking at 8 packets thus guarantees, that at least a full injection
+	// data set is evaluated.
+	// Moving Base reuires a higher rate, so we allow up to 8 packets.
+	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
+	size_t num_injections = 0;
+
+	do {
+		if (updated) {
+			num_injections++;
+
+			// Write the message to the gps device. Note that the message could be fragmented.
+			// But as we don't write anywhere else to the device during operation, we don't
+			// need to assemble the message first.
+			if (_publish_rtcm_stream) {
+				PublishRTCMStream(msg.data, msg.len);
+			}
+
+			if (_publish_moving_baseline_data) {
+				PublishMovingBaselineData(msg.data, msg.len);
+			}
+
+			_last_rtcm_injection_time = hrt_absolute_time();
+		}
+
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
+
+	} while (updated && num_injections < max_num_injections);
 }
 
 bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)

src/drivers/uavcan/sensors/gnss.hpp

@@ -45,6 +45,7 @@
 #pragma once
 
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/gps_inject_data.h>
@@ -123,7 +124,9 @@ private:
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};
 
-	uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
+	hrt_abstime		_last_rtcm_injection_time{0};	///< time of last rtcm injection
+	uint8_t			_selected_rtcm_instance{0};	///< uorb instance that is being used for RTCM corrections
 
 	bool _system_clock_set{false};  ///< Have we set the system clock at least once from GNSS data?
 

src/drivers/uwb/uwb_sr150/module.yaml

@@ -4,12 +4,11 @@ serial_config:
     port_config_param:
       name: UWB_PORT_CFG
       group: UWB
-      default: ""
+      default: "TEL2"
 
 parameters:
     - group: UWB
       definitions:
-
         UWB_INIT_OFF_X:
           description:
             short: UWB sensor X offset in body frame
@@ -32,7 +31,7 @@ parameters:
 
         UWB_INIT_OFF_Z:
           description:
-            short: UWB sensor Y offset in body frame
+            short: UWB sensor Z offset in body frame
             long: UWB sensor positioning in relation to Drone in NED. Z offset.
           type: float
           unit: m
@@ -40,14 +39,52 @@ parameters:
           increment: 0.01
           default: 0.00
 
-        UWB_INIT_OFF_YAW:
+        UWB_SENS_ROT:
           description:
-            short: UWB sensor YAW offset in body frame
-            long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
-          type: float
-          unit: deg
-          decimal: 1
-          increment: 0.1
-          default: 0.00
-
-
+            short: UWB sensor orientation
+            long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h
+                  Default position is the antannaes downward facing, UWB board parallel with body frame.
+          type: enum
+          values:
+            0: ROTATION_NONE
+            1: ROTATION_YAW_45
+            2: ROTATION_YAW_90
+            3: ROTATION_YAW_135
+            4: ROTATION_YAW_180
+            5: ROTATION_YAW_225
+            6: ROTATION_YAW_270
+            7: ROTATION_YAW_315
+            8: ROTATION_ROLL_180
+            9: ROTATION_ROLL_180_YAW_45
+            10: ROTATION_ROLL_180_YAW_90
+            11: ROTATION_ROLL_180_YAW_135
+            12: ROTATION_PITCH_180
+            13: ROTATION_ROLL_180_YAW_225
+            14: ROTATION_ROLL_180_YAW_270
+            15: ROTATION_ROLL_180_YAW_315
+            16: ROTATION_ROLL_90
+            17: ROTATION_ROLL_90_YAW_45
+            18: ROTATION_ROLL_90_YAW_90
+            19: ROTATION_ROLL_90_YAW_135
+            20: ROTATION_ROLL_270
+            21: ROTATION_ROLL_270_YAW_45
+            22: ROTATION_ROLL_270_YAW_90
+            23: ROTATION_ROLL_270_YAW_135
+            24: ROTATION_PITCH_90
+            25: ROTATION_PITCH_270
+            26: ROTATION_PITCH_180_YAW_90
+            27: ROTATION_PITCH_180_YAW_270
+            28: ROTATION_ROLL_90_PITCH_90
+            29: ROTATION_ROLL_180_PITCH_90
+            30: ROTATION_ROLL_270_PITCH_90
+            31: ROTATION_ROLL_90_PITCH_180
+            32: ROTATION_ROLL_270_PITCH_180
+            33: ROTATION_ROLL_90_PITCH_270
+            34: ROTATION_ROLL_180_PITCH_270
+            35: ROTATION_ROLL_270_PITCH_270
+            36: ROTATION_ROLL_90_PITCH_180_YAW_90
+            37: ROTATION_ROLL_90_YAW_270
+            38: ROTATION_ROLL_90_PITCH_68_YAW_293
+            39: ROTATION_PITCH_315
+            40: ROTATION_ROLL_90_PITCH_315
+          default: 0

src/drivers/uwb/uwb_sr150/uwb_sr150.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -63,128 +63,130 @@
 // The default baudrate of the uwb_sr150 module before configuration
 #define DEFAULT_BAUD B115200
 
-const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP};
-const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START};
-const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP};
-const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP};
-
 extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]);
 
-UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug):
+UWB_SR150::UWB_SR150(const char *port):
 	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")),
 	_read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err"))
 {
-	_uwb_pos_debug = uwb_pos_debug;
-	// start serial port
-	_uart = open(device_name, O_RDWR | O_NOCTTY);
-
-	if (_uart < 0) { err(1, "could not open %s", device_name); }
-
-	int ret = 0;
-	struct termios uart_config {};
-	ret = tcgetattr(_uart, &uart_config);
-
-	if (ret < 0) { err(1, "failed to get attr"); }
-
-	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set input speed"); }
-
-	ret = cfsetospeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set output speed"); }
-
-	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-
-	if (ret < 0) { err(1, "failed to set attr"); }
+	/* store port name */
+	strncpy(_port, port, sizeof(_port) - 1);
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
 }
 
 UWB_SR150::~UWB_SR150()
 {
 	printf("UWB: Ranging Stopped\t\n");
-	int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP));
-
-	if (written < (int) sizeof(CMD_APP_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP));
-	}
 
+	// stop{}; will be implemented when this is changed to a scheduled work task
 	perf_free(_read_err_perf);
 	perf_free(_read_count_perf);
 
 	close(_uart);
 }
 
-void UWB_SR150::run()
+bool UWB_SR150::init()
 {
-	// Subscribe to parameter_update message
-	parameters_update();
-
-	//TODO replace with BLE grid configuration
-	grid_info_read(&_uwb_grid_info.target_pos);
-	_uwb_grid_info.num_anchors = 4;
-	_uwb_grid_info.initator_time = hrt_absolute_time();
-	_uwb_grid_info.mac_mode = 0x00;
-
-	/* Grid Info Message*/
-	_uwb_grid.timestamp = hrt_absolute_time();
-	_uwb_grid.initator_time = _uwb_grid_info.initator_time;
-	_uwb_grid.num_anchors = _uwb_grid_info.num_anchors;
-
-	memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions
-
-	_uwb_grid_pub.publish(_uwb_grid);
-
-	/* Ranging  Command */
-	int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN);
-
-	if (written <  UWB_CMD_LEN) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) *  UWB_CMD_LEN);
+	// execute Run() on every sensor_accel publication
+	if (!_sensor_uwb_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
 	}
 
-	while (!should_exit()) {
-		written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop
-	}
+	// alternatively, Run on fixed interval
+	// ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate
 
-	if (!written) { printf("ERROR: Distance Failed"); }
-
-	// Automatic Stop. This should not be reachable
-	written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN);
-
-	if (written < (int) sizeof(CMD_RANGING_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP));
-	}
+	return true;
 }
 
-void UWB_SR150::grid_info_read(position_t *grid)
+void UWB_SR150::start()
 {
-	//place holder, until UWB initiator can respond with Grid info
-	/*	This Reads the position of each Anchor in the Grid.
-		Right now the Grid configuration is saved on the SD card.
-		In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */
-	PX4_INFO("Reading UWB GRID from SD... \t\n");
-	FILE *file;
-	file = fopen(UWB_GRID_CONFIG, "r");
+	/* schedule a cycle to start things */
+	ScheduleNow();
+}
 
-	int  bread = 0;
+void UWB_SR150::stop()
+{
+	ScheduleClear();
+}
 
-	for (int i = 0; i < 5; i++) {
-		bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z);
+void UWB_SR150::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
 	}
 
-	if (bread == 5 * 3) {
-		PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread);
+	if (_uart < 0) {
+		/* open fd */
+		_uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
-	} else { //use UUID from Grid survey
-		PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread);
-		position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}};
-		memcpy(grid, &grid_setup, sizeof(grid_setup));
-		PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n");
-		PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format:  \"x,y,z\" \t\n");
+		if (_uart < 0) {
+			PX4_ERR("open failed (%i)", errno);
+			return;
+		}
+
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(_uart, &uart_config);
+
+		/* clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
+
+		//TODO: should I keep this?
+		/* no parity, one stop bit */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+		unsigned speed = DEFAULT_BAUD;
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d ISPD", termios_state);
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d OSPD", termios_state);
+		}
+
+		if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("baud %d ATTR", termios_state);
+		}
 	}
 
-	fclose(file);
+	/* perform collection */
+	int collect_ret = collectData();
+
+	if (collect_ret == -EAGAIN) {
+		/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
+		ScheduleDelayed(1042 * 8);
+
+		return;
+	}
+
+	if (OK != collect_ret) {
+
+		/* we know the sensor needs about four seconds to initialize */
+		if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+			PX4_ERR("collection error #%u", _consecutive_fail_count);
+		}
+
+		_consecutive_fail_count++;
+
+		/* restart the measurement state machine */
+		start();
+		return;
+
+	} else {
+		/* apparently success */
+		_consecutive_fail_count = 0;
+	}
 }
 
 int UWB_SR150::custom_command(int argc, char *argv[])
@@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Name of device for serial communication with UWB", false);
 	PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "<int>", "Baudrate for serial communication", false);
-	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<int>", "Position Debug: displays errors in Multilateration", false);
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 	return 0;
 }
 
 int UWB_SR150::task_spawn(int argc, char *argv[])
-{
-	int task_id = px4_task_spawn_cmd(
-			      "uwb_driver",
-			      SCHED_DEFAULT,
-			      SCHED_PRIORITY_DEFAULT,
-			      2048,
-			      &run_trampoline,
-			      argv
-		      );
-
-	if (task_id < 0) {
-		return -errno;
-
-	} else {
-		_task_id = task_id;
-		return 0;
-	}
-}
-
-UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 {
 	int ch;
 	int option_index = 1;
 	const char *option_arg;
-	const char *device_name = nullptr;
-	bool error_flag = false;
+	const char *device_name = UWB_DEFAULT_PORT;
 	int baudrate = 0;
-	bool uwb_pos_debug = false; // Display UWB position calculation debug Messages
 
-	while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device_name = option_arg;
@@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 			px4_get_parameter_value(option_arg, baudrate);
 			break;
 
-		case 'p':
-
-			uwb_pos_debug = true;
-			break;
-
 		default:
 			PX4_WARN("Unrecognized flag: %c", ch);
-			error_flag = true;
 			break;
 		}
 	}
 
-	if (!error_flag && device_name == nullptr) {
-		print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n");
-		device_name = "TEL2";
-		error_flag = true;
-	}
+	UWB_SR150 *instance = new UWB_SR150(device_name);
 
-	if (!error_flag && baudrate == 0) {
-		printf("Baudrate not provided. Using default Baud: 115200 \n");
-		baudrate = B115200;
-	}
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
 
-	if (!error_flag && uwb_pos_debug == true) {
-		printf("UWB Position algorithm Debugging \n");
-	}
+		instance->ScheduleOnInterval(5000_us);
 
-	if (error_flag) {
-		PX4_WARN("Failed to start UWB driver. \n");
-		return nullptr;
+		if (instance->init()) {
+			return PX4_OK;
+		}
 
 	} else {
-		PX4_INFO("Constructing UWB_SR150. Device: %s", device_name);
-		return new UWB_SR150(device_name, baudrate, uwb_pos_debug);
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int uwb_sr150_main(int argc, char *argv[])
+{
+	return UWB_SR150::main(argc, argv);
+}
+
+void UWB_SR150::parameters_update()
+{
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// If any parameter updated, call updateParams() to check if
+		// this class attributes need updating (and do so).
+		updateParams();
 	}
 }
 
-int UWB_SR150::distance()
+int UWB_SR150::collectData()
 {
-	_uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(),
-						_uwb_init_off_z.get()); //set offset at the start
 	uint8_t *buffer = (uint8_t *) &_distance_result_msg;
 
 	FD_ZERO(&_uart_set);
@@ -350,366 +336,54 @@ int UWB_SR150::distance()
 	perf_count(_read_count_perf);
 
 	// All of the following criteria must be met for the message to be acceptable:
-	//  - Size of message == sizeof(distance_msg_t) (51 bytes)
+	//  - Size of message == sizeof(distance_msg_t) (36 bytes)
 	//  - status == 0x00
-	//  - Values of all 3 position measurements are reasonable
-	//      (If one or more anchors is missed, then position might be an unreasonably large number.)
-	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //||
-	//(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b)
-	//);
+	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b);
 
 	if (ok) {
 
 		/* Ranging Message*/
-		_uwb_distance.timestamp = hrt_absolute_time();
-		_uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms;
-		_uwb_distance.counter = _distance_result_msg.seq_ctr;
-		_uwb_distance.sessionid = _distance_result_msg.sessionId;
-		_uwb_distance.time_offset = _distance_result_msg.range_interval;
+		_sensor_uwb.timestamp 		= hrt_absolute_time();
 
-		for (int i = 0; i < 4; i++) {
-			_uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance;
-			_uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos;
-			_uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) /
-						    128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value
-		}
+		_sensor_uwb.sessionid 		= _distance_result_msg.sessionId;
+		_sensor_uwb.time_offset 	= _distance_result_msg.range_interval;
+		_sensor_uwb.counter 		= _distance_result_msg.seq_ctr;
+		_sensor_uwb.mac			= _distance_result_msg.MAC;
 
-		// Algorithm goes here
-		UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization();
+		_sensor_uwb.mac_dest		= _distance_result_msg.measurements.MAC;
+		_sensor_uwb.status		= _distance_result_msg.measurements.status;
+		_sensor_uwb.nlos 		= _distance_result_msg.measurements.nLos;
+		_sensor_uwb.distance 		= double(_distance_result_msg.measurements.distance) / 100;
 
-		_uwb_distance.status = UWB_POS_ERROR;
 
-		if (UWB_OK == UWB_POS_ERROR) {
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
+		_sensor_uwb.aoa_azimuth_dev 	= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_dev 	= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
+		_sensor_uwb.aoa_azimuth_resp 	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_resp 	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
 
-			_uwb_distance.position[0] = _rel_pos(0);
-			_uwb_distance.position[1] = _rel_pos(1);
-			_uwb_distance.position[2] = _rel_pos(2);
 
-		} else {
-			//only print the error if debug is enabled
-			if (_uwb_pos_debug) {
-				switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors
-				case UWB_ANC_BELOW_THREE:
-					PX4_INFO("UWB not enough anchors for doing localization");
-					break;
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
+		_sensor_uwb.aoa_azimuth_fom 		= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_fom 		= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
+		_sensor_uwb.aoa_dest_azimuth_fom	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
+		_sensor_uwb.aoa_dest_elevation_fom	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
 
-				case UWB_LIN_DEP_FOR_THREE:
-					PX4_INFO("UWB localization: linear dependent with 3 Anchors");
-					break;
+		/* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb
+		_sensor_uwb.orientation		= _sensor_rot.get();
+		_sensor_uwb.offset_x		= _offset_x.get();
+		_sensor_uwb.offset_y		= _offset_y.get();
+		_sensor_uwb.offset_z		= _offset_z.get();
 
-				case UWB_ANC_ON_ONE_LEVEL:
-					PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors");
-					break;
-
-				case UWB_LIN_DEP_FOR_FOUR:
-					PX4_INFO("UWB localization: linear dependent with four or more Anchors");
-					break;
-
-				case UWB_RANK_ZERO:
-					PX4_INFO("UWB localization: rank is zero");
-					break;
-
-				default:
-					PX4_INFO("UWB localization: Unknown failure in Position Algorithm");
-					break;
-				}
-			}
-		}
-
-		_uwb_distance_pub.publish(_uwb_distance);
+		_sensor_uwb_pub.publish(_sensor_uwb);
 
 	} else {
-		//PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t));
 		perf_count(_read_err_perf);
 
 		if (buffer_location == 0) {
 			PX4_WARN("UWB module is not responding.");
-
-			//TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power)
 		}
 	}
 
 	return 1;
 }
-
-UWB_POS_ERROR_CODES UWB_SR150::localization()
-{
-// 			WIP
-	/******************************************************
-	 ****************** 3D Localization *******************
-	 *****************************************************/
-
-	/*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors.
-	 *	 	   The function expects more than 4 anchors. The used equation system looks like follows:\n
-	 \verbatim
-	  		    -					-
-	  		   | M_11	M_12	M_13 |	 x	  b[0]
-	  		   | M_12	M_22	M_23 | * y	= b[1]
-	  		   | M_23	M_13	M_33 |	 z	  b[2]
-	  		    -					-
-	 \endverbatim
-	 * @param distances_cm_in_pt: 			Pointer to array that contains the distances to the anchors in cm (including invalid results)
-	 * @param no_distances: 				Number of valid distances in distance array (it's not the size of the array)
-	 * @param anchor_pos: 	Pointer to array that contains anchor positions in cm (including positions related to invalid results)
-	 * @param no_anc_positions: 			Number of valid anchor positions in the position array (it's not the size of the array)
-	 * @param position_result_pt: 			Pointer toposition. position_t variable that holds the result of this calculation
-	 * @return: The function returns a status code. */
-
-	/* 		Algorithm used:
-	 *		Linear Least Sqaures to solve Multilateration
-	 * 		with a Special case if there are only 3 Anchors.
-	 * 		Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing
-	 * 		In cm
-	 */
-
-	/* Resulting Position Vector*/
-	int64_t x_pos = 0;
-	int64_t y_pos = 0;
-	int64_t z_pos = 0;
-	/* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */
-	int64_t M_11 = 0;
-	int64_t M_12 = 0;																						// = M_21
-	int64_t M_13 = 0;																						// = M_31
-	int64_t M_22 = 0;
-	int64_t M_23 = 0;																						// = M_23
-	int64_t M_33 = 0;
-
-	/* Vector components (3*1 Vector resulting from least square error method) [cm^3] */
-	int64_t b[3] = {0};
-
-	/* Miscellaneous variables */
-	int64_t temp = 0;
-	int64_t temp2 = 0;
-	int64_t nominator = 0;
-	int64_t denominator = 0;
-	bool	anchors_on_x_y_plane = true;																		// Is true, if all anchors are on the same height => x-y-plane
-	bool	lin_dep = true;																						// All vectors are linear dependent, if this variable is true
-	uint8_t ind_y_indi =
-		0;	//numberr of independet vectors																					// First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent
-
-	/* Arrays for used distances and anchor positions (without rejected ones) */
-	uint8_t 	no_distances = _uwb_grid_info.num_anchors;
-	uint32_t 	distances_cm[no_distances];
-	position_t 	anchor_pos[no_distances]; //position in CM
-	uint8_t		no_valid_distances = 0;
-
-	/* Reject invalid distances (including related anchor position) */
-	for (int i = 0; i < no_distances; i++) {
-		if (_distance_result_msg.measurements[i].distance != 0xFFFFu) {
-			//excludes any distance that is 0xFFFFU (int16 Maximum Value)
-			distances_cm[no_valid_distances] 		= _distance_result_msg.measurements[i].distance;
-			anchor_pos[no_valid_distances] 	= _uwb_grid_info.anchor_pos[i];
-			no_valid_distances++;
-
-		}
-	}
-
-	/* Check, if there are enough valid results for doing the localization at all */
-	if (no_valid_distances < 3) {
-		return UWB_ANC_BELOW_THREE;
-	}
-
-	/* Check, if anchors are on the same x-y plane */
-	for (int i = 1; i < no_valid_distances; i++) {
-		if (anchor_pos[i].z != anchor_pos[0].z) {
-			anchors_on_x_y_plane = false;
-			break;
-		}
-	}
-
-	/**** Check, if there are enough linear independent anchor positions ****/
-
-	/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2
-	 * 			|(y_1 - y_0) (y_2 - y_0) ... | 				*/
-
-	for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) {
-		temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x -
-				(int64_t)anchor_pos[0].x);
-		temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x -
-				(int64_t)anchor_pos[0].x);
-
-		if ((temp - temp2) != 0) {
-			lin_dep = false;
-			break;
-		}
-	}
-
-	/* Leave function, if rank is below 2 */
-	if (lin_dep == true) {
-		return UWB_LIN_DEP_FOR_THREE;
-	}
-
-	/* If the anchors are not on the same plane, three vectors must be independent => check */
-	if (!anchors_on_x_y_plane) {
-		/* Check, if there are enough valid results for doing the localization */
-		if (no_valid_distances < 4) {
-			return UWB_ANC_ON_ONE_LEVEL;
-		}
-
-		/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked)
-		 * 			|(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... |
-		 * 			|(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... |											*/
-		lin_dep = true;
-
-		for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) {
-			if (i != ind_y_indi) {
-				/* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	= ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */
-				temp 	= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp;
-
-				if (temp2 != 0) { lin_dep = false; }
-			}
-		}
-
-		/* Leave function, if rank is below 3 */
-		if (lin_dep == true) {
-			return UWB_LIN_DEP_FOR_FOUR;
-		}
-	}
-
-	/************************************************** Algorithm ***********************************************************************/
-
-	/* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */
-	for (int i = 1; i < no_valid_distances; i++) {
-		/* Matrix (needed to be multiplied with 2, afterwards) */
-		M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x));
-		M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-
-		/* Vector */
-		temp = sq(distances_cm[0]) - sq(distances_cm[i])
-		       + sq(anchor_pos[i].x) + sq(anchor_pos[i].y)
-		       + sq(anchor_pos[i].z) - sq(anchor_pos[0].x)
-		       - sq(anchor_pos[0].y) - sq(anchor_pos[0].z);
-
-		b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp);
-		b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp);
-		b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp);
-	}
-
-	M_11 = 2 * M_11;
-	M_12 = 2 * M_12;
-	M_13 = 2 * M_13;
-	M_22 = 2 * M_22;
-	M_23 = 2 * M_23;
-	M_33 = 2 * M_33;
-
-	/* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */
-	if (anchors_on_x_y_plane == false) {
-		nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] *
-			    (M_11 * M_22 - M_12 * M_12);			// [cm^7]
-		denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 *
-			      M_13;				// [cm^6]
-
-		/* Check, if denominator is zero (Rank of matrix not high enough) */
-		if (denominator == 0) {
-			return UWB_RANK_ZERO;
-		}
-
-		z_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-	}
-
-	/* Else prepare for different calculation approach (after x and y were calculated) */
-	else {
-		z_pos = 0;
-	}
-
-	/* Calculating the y-position */
-	nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13));	// [cm^5]
-	denominator = M_11 * M_22 - M_12 * M_12;// [cm^4]
-
-	/* Check, if denominator is zero (Rank of matrix not high enough) */
-	if (denominator == 0) {
-		return UWB_RANK_ZERO;
-	}
-
-	y_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-
-	/* Calculating the x-position */
-	nominator = b[0] - z_pos * M_13 - y_pos * M_12;	// [cm^3]
-	denominator = M_11;	// [cm^2]
-
-	x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm]
-
-	/* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */
-	if (anchors_on_x_y_plane == true) {
-		/* Calculate z-positon relative to the anchor grid's height */
-		for (int i = 0; i < no_distances; i++) {
-			/* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */
-			temp = (int64_t)((int64_t)pow(distances_cm[i], 2)
-					 - (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2)
-					 - (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2));
-
-			/* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */
-			if (temp >= 0) {
-				z_pos += (int64_t)sqrt(temp);
-
-			} else {
-				z_pos = 0;
-			}
-		}
-
-		z_pos = z_pos / no_distances;										// Divide sum by number of distances to get the average
-
-		/* Add height of the anchor grid's height */
-		z_pos += anchor_pos[0].z;
-	}
-
-	//Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing
-	// The end goal of this math is to get the position relative to the landing point in the NED frame.
-	_current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos);
-
-	// Construct the rotation from the UWB_R4frame to the NWU frame.
-	// The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset)
-	// To get back to NWU, just rotate by negative this amount about Z.
-	_uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
-					-(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); //
-	// The actual conversion:
-	//  - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame
-	//  - Rotate by _rddrone_to_nwu to get into the NWU frame
-	//  - Rotate by _nwu_to_ned to get into the NED frame
-	_current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init;
-
-	// Now the position is the landing point relative to the vehicle.
-	// so the only thing left is to convert cm to Meters and to add the Initiator offset
-	_rel_pos =  _current_position_ned / 100 + _uwb_init_offset_v3f;
-
-	// The UWB report contains the position of the vehicle relative to the landing point.
-
-	return UWB_OK;
-}
-
-int uwb_sr150_main(int argc, char *argv[])
-{
-	return UWB_SR150::main(argc, argv);
-}
-
-void UWB_SR150::parameters_update()
-{
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// If any parameter updated, call updateParams() to check if
-		// this class attributes need updating (and do so).
-		updateParams();
-	}
-}

src/drivers/uwb/uwb_sr150/uwb_sr150.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -38,101 +38,63 @@
 #include <poll.h>
 #include <sys/select.h>
 #include <sys/time.h>
+#include <perf/perf_counter.h>
+#include <lib/conversion/rotation.h>
 
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/module.h>
-#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
 #include <uORB/Publication.hpp>
-#include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_uwb.h>
+#include <uORB/topics/parameter_update.h>
+
 #include <matrix/math.hpp>
-#include <matrix/Matrix.hpp>
+
+#define UWB_DEFAULT_PORT "/dev/ttyS1"
 
 using namespace time_literals;
 
-#define UWB_CMD  0x8e
-#define UWB_CMD_START  0x01
-#define UWB_CMD_STOP  0x00
-#define UWB_CMD_RANGING  0x0A
-#define STOP_B 0x0A
-
-#define UWB_PRECNAV_APP   0x04
-#define UWB_APP_START     0x10
-#define UWB_APP_STOP      0x11
-#define UWB_SESSION_START 0x22
-#define UWB_SESSION_STOP  0x23
-#define UWB_RANGING_START 0x01
-#define UWB_RANGING_STOP  0x00
-#define UWB_DRONE_CTL     0x0A
-
-#define UWB_CMD_LEN  0x05
-#define UWB_CMD_DISTANCE_LEN 0x21
-#define UWB_MAC_MODE 2
-#define MAX_ANCHORS 12
-#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv"
-
-typedef struct {  //needs higher accuracy?
-	float lat, lon, alt, yaw; //offset to true North
-} gps_pos_t;
-
 typedef struct {
-	int16_t x, y, z; //axis in cm
-} position_t; // Position of a device or target in 3D space
-
-enum UWB_POS_ERROR_CODES {
-	UWB_OK,
-	UWB_ANC_BELOW_THREE,
-	UWB_LIN_DEP_FOR_THREE,
-	UWB_ANC_ON_ONE_LEVEL,
-	UWB_LIN_DEP_FOR_FOUR,
-	UWB_RANK_ZERO
-};
-
-typedef struct {
-	uint8_t MAC[2];		// MAC Adress of UWB device
-	uint8_t status;		// Status of Measurement
-	uint16_t distance; 	// Distance in cm
-	uint8_t nLos; 		// line of sight y/n
-	uint16_t aoaFirst;	// Angle of Arrival of incoming msg
+	uint16_t MAC;					// MAC address of UWB device
+	uint8_t status;					// Status of Measurement
+	uint16_t distance; 				// Distance in cm
+	uint8_t nLos; 					// line of sight y/n
+	int16_t aoa_azimuth;				// AOA of incoming msg for Azimuth antenna pairing
+	int16_t aoa_elevation;				// AOA of incoming msg for Altitude antenna pairing
+	int16_t aoa_dest_azimuth;			// AOA destination Azimuth
+	int16_t aoa_dest_elevation; 			// AOA destination elevation
+	uint8_t aoa_azimuth_FOM;			// AOA Azimuth FOM
+	uint8_t aoa_elevation_FOM;			// AOA Elevation FOM
+	uint8_t aoa_dest_azimuth_FOM;			// AOA Azimuth FOM
+	uint8_t aoa_dest_elevation_FOM;			// AOA Elevation FOM
 } __attribute__((packed)) UWB_range_meas_t;
 
 typedef struct {
-	uint32_t initator_time;  	//timestamp of init
-	uint32_t sessionId;	// Session ID of UWB session
-	uint8_t	num_anchors;	//number of anchors
-	gps_pos_t target_gps; //GPS position of Landing Point
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
-	uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS];
-	position_t target_pos; //target position
-	position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor
-	uint8_t stop; 		// Should be 27
-} grid_msg_t;
-
-typedef struct {
-	uint8_t cmd;      	// Should be 0x8E for distance result message
-	uint16_t len; 		// Should be 0x30 for distance result message
-	uint32_t time_uwb_ms;	// Timestamp of UWB device in ms
-	uint32_t seq_ctr;	// Number of Ranges since last Start of Ranging
-	uint32_t sessionId;	// Session ID of UWB session
-	uint32_t range_interval;	// Time between ranging rounds
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
-	uint8_t  no_measurements;	// MAC adress mode, either 2 Byte or 8 Byte
-	UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9
-	uint8_t stop; 		// Should be 0x1B
+	uint8_t cmd;      				// Should be 0x8E for distance result message
+	uint16_t len; 					// Should be 0x30 for distance result message
+	uint32_t seq_ctr;				// Number of Ranges since last Start of Ranging
+	uint32_t sessionId;				// Session ID of UWB session
+	uint32_t range_interval;			// Time between ranging rounds
+	uint16_t MAC;					// MAC address of UWB device
+	UWB_range_meas_t measurements; 			//Raw anchor_distance distances in CM 2*9
+	uint8_t stop; 					// Should be 0x1B
 } __attribute__((packed)) distance_msg_t;
 
-class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams
+class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug);
-
+	UWB_SR150(const char *port);
 	~UWB_SR150();
 
+	/**
+	 * @see ModuleBase::task_spawn
+	 */
+	static int task_spawn(int argc, char *argv[]);
+
 	/**
 	 * @see ModuleBase::custom_command
 	 */
@@ -143,67 +105,51 @@ public:
 	 */
 	static int print_usage(const char *reason = nullptr);
 
-	/**
-	 * @see ModuleBase::Multilateration
-	 */
-	UWB_POS_ERROR_CODES localization();
+	bool init();
 
-	/**
-	 * @see ModuleBase::Distance Result
-	 */
-	int distance();
+	void start();
 
-	/**
-	 * @see ModuleBase::task_spawn
-	 */
-	static int task_spawn(int argc, char *argv[]);
+	void stop();
 
-	static UWB_SR150 *instantiate(int argc, char *argv[]);
-
-	void run() override;
+	int collectData();
 
 private:
-	static constexpr int64_t sq(int64_t x) { return x * x; }
 
 	void parameters_update();
 
-	void grid_info_read(position_t *grid);
+	void Run() override;
 
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::UWB_INIT_OFF_X>) _uwb_init_off_x,
-		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _uwb_init_off_y,
-		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _uwb_init_off_z,
-		(ParamFloat<px4::params::UWB_INIT_OFF_YAW>) _uwb_init_off_yaw
-	)
+	// Publications
+	uORB::Publication<sensor_uwb_s> _sensor_uwb_pub{ORB_ID(sensor_uwb)};
 
+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	int _uart;
-	fd_set _uart_set;
-	struct timeval _uart_timeout {};
-	bool _uwb_pos_debug;
-
-	uORB::Publication<uwb_grid_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
-	uwb_grid_s _uwb_grid{};
-
-	uORB::Publication<uwb_distance_s> _uwb_distance_pub{ORB_ID(uwb_distance)};
-	uwb_distance_s _uwb_distance{};
-
-	uORB::Publication<landing_target_pose_s> _landing_target_pub{ORB_ID(landing_target_pose)};
-	landing_target_pose_s _landing_target{};
-
-	grid_msg_t _uwb_grid_info{};
-	distance_msg_t _distance_result_msg{};
-	matrix::Vector3f _rel_pos;
-
-	matrix::Dcmf _uwb_init_to_nwu;
-	matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)};
-	matrix::Vector3f _current_position_uwb_init;
-	matrix::Vector3f _current_position_ned;
-	matrix::Vector3f _uwb_init_offset_v3f;
-
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::UWB_PORT_CFG>) 			_uwb_port_cfg,
+		(ParamFloat<px4::params::UWB_INIT_OFF_X>) 		_offset_x,
+		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) 		_offset_y,
+		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) 		_offset_z,
+		(ParamInt<px4::params::UWB_SENS_ROT>) 			_sensor_rot
+	)
+	// Performance (perf) counters
 	perf_counter_t _read_count_perf;
 	perf_counter_t _read_err_perf;
-};
 
+	sensor_uwb_s _sensor_uwb{};
+
+	char _port[20] {};
+	hrt_abstime param_timestamp{0};
+
+	int _uart{-1};
+	fd_set _uart_set;
+	struct timeval _uart_timeout {};
+
+	unsigned _consecutive_fail_count;
+	int _interval{100000};
+
+	distance_msg_t 		_distance_result_msg{};
+};
 #endif //PX4_RDDRONE_H

src/lib/mixer_module/actuator_test.cpp

@@ -75,7 +75,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve)
 					float value = actuator_test.value;
 
 					// handle motors
-					if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) {
+					if ((int)OutputFunction::Motor1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::MotorMax) {
 						actuator_motors_s motors;
 						motors.reversible_flags = 0;
 						_actuator_motors_sub.copy(&motors);
@@ -84,7 +84,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve)
 					}
 
 					// handle servos: add trim
-					if (actuator_test.function >= (int)OutputFunction::Servo1 && actuator_test.function <= (int)OutputFunction::ServoMax) {
+					if ((int)OutputFunction::Servo1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::ServoMax) {
 						actuator_servos_trim_s trim{};
 						_actuator_servos_trim_sub.copy(&trim);
 						int idx = actuator_test.function - (int)OutputFunction::Servo1;

src/lib/mixer_module/mixer_module.cpp

@@ -490,6 +490,24 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
 		output_limit_calc(_throttle_armed || _actuator_test.inTestMode(), _max_num_outputs, outputs);
 	}
 
+	// We must calibrate the PWM and Oneshot ESCs to a consistent range of 1000-2000us (gets mapped to 125-250us for Oneshot)
+	// Doing so makes calibrations consistent among different configurations and hence PWM minimum and maximum have a consistent effect
+	// hence the defaults for these parameters also make most setups work out of the box
+	if (_armed.in_esc_calibration_mode) {
+		static constexpr uint16_t PWM_CALIBRATION_LOW = 1000;
+		static constexpr uint16_t PWM_CALIBRATION_HIGH = 2000;
+
+		for (int i = 0; i < _max_num_outputs; i++) {
+			if (_current_output_value[i] == _min_value[i]) {
+				_current_output_value[i] = PWM_CALIBRATION_LOW;
+			}
+
+			if (_current_output_value[i] == _max_value[i]) {
+				_current_output_value[i] = PWM_CALIBRATION_HIGH;
+			}
+		}
+	}
+
 	/* now return the outputs to the driver */
 	if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) {
 		actuator_outputs_s actuator_outputs{};

src/lib/mixer_module/mixer_module.hpp

@@ -179,6 +179,8 @@ public:
 
 	param_t functionParamHandle(int index) const { return _param_handles[index].function; }
 	param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; }
+	param_t minParamHandle(int index) const { return _param_handles[index].min; }
+	param_t maxParamHandle(int index) const { return _param_handles[index].max; }
 
 	/**
 	 * Returns the actual failsafe value taking into account the assigned function

src/lib/rate_control/rate_control.cpp

@@ -47,13 +47,27 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f
 	_gain_d = D;
 }
 
-void RateControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
-				      const Vector<bool, 3> &saturation_negative)
+void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
+				      const Vector3<bool> &saturation_negative)
 {
 	_control_allocator_saturation_positive = saturation_positive;
 	_control_allocator_saturation_negative = saturation_negative;
 }
 
+void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated)
+{
+	if (axis < 3) {
+		_control_allocator_saturation_positive(axis) = is_saturated;
+	}
+}
+
+void RateControl::setNegativeSaturationFlag(size_t axis, bool is_saturated)
+{
+	if (axis < 3) {
+		_control_allocator_saturation_negative(axis) = is_saturated;
+	}
+}
+
 Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
 			     const float dt, const bool landed)
 {

src/lib/rate_control/rate_control.hpp

@@ -75,8 +75,16 @@ public:
 	 * Set saturation status
 	 * @param control saturation vector from control allocator
 	 */
-	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
-				 const matrix::Vector<bool, 3> &saturation_negative);
+	void setSaturationStatus(const matrix::Vector3<bool> &saturation_positive,
+				 const matrix::Vector3<bool> &saturation_negative);
+
+	/**
+	 * Set individual saturation flags
+	 * @param axis 0 roll, 1 pitch, 2 yaw
+	 * @param is_saturated value to update the flag with
+	 */
+	void setPositiveSaturationFlag(size_t axis, bool is_saturated);
+	void setNegativeSaturationFlag(size_t axis, bool is_saturated);
 
 	/**
 	 * Run one control loop cycle calculation

src/lib/sensor_calibration/Magnetometer.cpp

@@ -70,7 +70,7 @@ void Magnetometer::set_device_id(uint32_t device_id)
 
 bool Magnetometer::set_offset(const Vector3f &offset)
 {
-	if (Vector3f(_offset - offset).longerThan(0.01f)) {
+	if (Vector3f(_offset - offset).longerThan(0.005f)) {
 		if (offset.isAllFinite()) {
 			_offset = offset;
 			_calibration_count++;

src/modules/commander/commander_params.c

@@ -50,8 +50,8 @@
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -66,8 +66,8 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -82,8 +82,8 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */

src/modules/commander/esc_calibration.cpp

@@ -58,18 +58,19 @@ using namespace time_literals;
 
 bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
 {
-	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
-	const battery_status_s &battery = batt_sub.get();
-	batt_sub.update();
+	uORB::SubscriptionData<battery_status_s> battery_status_sub{ORB_ID(battery_status)};
+	battery_status_sub.update();
 
-	if (battery.timestamp == 0) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable");
+	const bool recent_battery_measurement = hrt_absolute_time() < (battery_status_sub.get().timestamp + 1_s);
+
+	if (!recent_battery_measurement) {
+		// We have to send this message for now because "battery unavailable" gets ignored by QGC
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
 		return false;
 	}
 
-	// Make sure battery is disconnected
-	// battery is not connected if the connected flag is not set and we have a recent battery measurement
-	if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) {
+	// Make sure battery is reported to be disconnected
+	if (recent_battery_measurement && !battery_status_sub.get().connected) {
 		return true;
 	}
 
@@ -93,66 +94,80 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, f
 
 int do_esc_calibration(orb_advert_t *mavlink_log_pub)
 {
-	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
+	// 1 Initialization
+	bool calibration_failed = false;
 
-	int	return_code = PX4_OK;
 	uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
 	// since we publish multiple at once, make sure the output driver subscribes before we publish
 	actuator_test_pub.advertise();
-	px4_usleep(10000);
 
-	// set motors to high
+	uORB::SubscriptionData<battery_status_s> battery_status_sub{ORB_ID(battery_status)};
+	battery_status_sub.update();
+	const bool battery_connected_before_calibration = battery_status_sub.get().connected;
+	const float current_before_calibration = battery_status_sub.get().current_a;
+
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
+
+	px4_usleep(10_ms);
+
+	// 2 Set motors to high
 	set_motor_actuators(actuator_test_pub, 1.f, false);
 	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
 
-
-	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
-	const battery_status_s &battery = batt_sub.get();
-	batt_sub.update();
-	bool batt_connected = battery.connected;
 	hrt_abstime timeout_start = hrt_absolute_time();
 
+	// 3 Wait for user to connect power
 	while (true) {
-		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
-		// sit high.
-		static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
-		static constexpr hrt_abstime pwm_high_timeout{3_s};
-		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
+		hrt_abstime now = hrt_absolute_time();
+		battery_status_sub.update();
 
-		if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
-			if (!batt_connected) {
-				calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
-				return_code = PX4_ERROR;
-			}
-
-			// PWM was high long enough
+		if (now > (timeout_start + 1_s) && (battery_status_sub.get().current_a > current_before_calibration + 1.f)) {
+			// Safety termination, current rises immediately, user didn't unplug power before
+			calibration_failed = true;
 			break;
 		}
 
-		if (!batt_connected) {
-			if (batt_sub.update()) {
-				if (battery.connected) {
-					// Battery is connected, signal to user and start waiting again
-					batt_connected = true;
-					timeout_start = hrt_absolute_time();
-					calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
-				}
-			}
+		if (!battery_connected_before_calibration && battery_status_sub.get().connected) {
+			// Battery connection detected we can go to the next step immediately
+			break;
 		}
 
-		px4_usleep(50000);
+		if (now > (timeout_start + 6_s)) {
+			// Timeout, we continue since maybe the battery cannot be detected properly
+			// If we abort here and the ESCs are infact connected and started calibrating
+			// they will measure the disarmed value as the lower limit instead of the fixed 1000us
+			break;
+		}
+
+		px4_usleep(50_ms);
 	}
 
-	if (return_code == PX4_OK) {
-		// set motors to low
+	// 4 Wait for ESCs to measure high signal
+	if (!calibration_failed) {
+		calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
+		px4_usleep(3_s);
+	}
+
+	// 5 Set motors to low
+	if (!calibration_failed) {
 		set_motor_actuators(actuator_test_pub, 0.f, false);
-		px4_usleep(4000000);
-
-		// release control
-		set_motor_actuators(actuator_test_pub, 0.f, true);
-
-		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
 	}
 
-	return return_code;
+	// 6 Wait for ESCs to measure low signal
+	if (!calibration_failed) {
+		px4_usleep(5_s);
+	}
+
+	// 7 release control
+	set_motor_actuators(actuator_test_pub, 0.f, true);
+
+	// 8 Report
+	if (calibration_failed) {
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
+		return PX4_ERROR;
+
+	} else {
+		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
+		return PX4_OK;
+	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp

@@ -84,3 +84,19 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const
 
 	return total_count;
 }
+
+void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
+{
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (stoppable_motors_mask & motor_mask) {
+			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
+				_stopped_motors_mask |= motor_mask;
+
+			} else {
+				_stopped_motors_mask &= ~motor_mask;
+			}
+		}
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -203,7 +203,7 @@ public:
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
-	virtual uint32_t getStoppedMotors() const { return 0; }
+	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
 
 	/**
 	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
@@ -211,6 +211,15 @@ public:
 	 */
 	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
+	/**
+	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
+	 *
+	 * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
+	 * @param actuator_sp outcome of the allocation to determine if the motor should be stopped
+	 */
+	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
+
 protected:
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+	uint32_t _stopped_motors_mask{0};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -48,9 +48,10 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 		return false;
 	}
 
-	// motors
+	// Motors
 	_motors.enablePropellerTorque(false);
 	const bool motors_added_successfully = _motors.addActuators(configuration);
+	_motors_mask = _motors.getMotors();
 
 	// Torque
 	const bool torque_added_successfully = _torque.addActuators(configuration);
@@ -58,3 +59,9 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 	return (motors_added_successfully && torque_added_successfully);
 }
 
+void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -45,9 +45,15 @@ public:
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 	const char *name() const override { return "Custom"; }
 
 protected:
 	ActuatorEffectivenessRotors _motors;
 	ActuatorEffectivenessControlSurfaces _torque;
+
+	uint32_t _motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -53,6 +53,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	// Motors
 	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);
+	_forwards_motors_mask = _rotors.getForwardsMotors();
 
 	// Control Surfaces
 	_first_control_surface_idx = configuration.num_actuators_matrix[0];
@@ -61,6 +62,13 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	return (rotors_added_successfully && surfaces_added_successfully);
 }
 
+void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+}
+
 void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
 		ActuatorVector &actuator_sp)
 {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -51,6 +51,10 @@ public:
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
@@ -59,4 +63,6 @@ private:
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
+
+	uint32_t _forwards_motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -265,6 +265,17 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di
 	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
 }
 
+uint32_t ActuatorEffectivenessRotors::getMotors() const
+{
+	uint32_t motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		motors |= 1u << i;
+	}
+
+	return motors;
+}
+
 uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
 {
 	uint32_t upwards_motors = 0;
@@ -280,6 +291,21 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
 	return upwards_motors;
 }
 
+uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const
+{
+	uint32_t forward_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
+			forward_motors |= 1u << i;
+		}
+	}
+
+	return forward_motors;
+}
+
 bool
 ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -126,7 +126,9 @@ public:
 
 	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
 
+	uint32_t getMotors() const;
 	uint32_t getUpwardsMotors() const;
+	uint32_t getForwardsMotors() const;
 
 private:
 	void updateParams() override;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -45,7 +45,15 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
 	}
 
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
+	_motors_mask = 1u << 0;
 	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
 	return true;
 }
 
+void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}
+

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -43,6 +43,11 @@ public:
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:
+	uint32_t _motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp

@@ -46,6 +46,14 @@ ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &co
 
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
+	_motors_mask = (1u << 0) | (1u << 1);
 	return true;
 }
 
+void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}
+

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp

@@ -43,6 +43,11 @@ public:
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 	const char *name() const override { return "Rover (Differential)"; }
 private:
+	uint32_t _motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -53,7 +53,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 	configuration.selected_matrix = 0;
 	_rotors.enablePropellerTorqueNonUpwards(false);
 	const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
-	_mc_motors_mask = _rotors.getUpwardsMotors();
+	_upwards_motors_mask = _rotors.getUpwardsMotors();
+	_forwards_motors_mask = _rotors.getForwardsMotors();
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
@@ -83,6 +84,15 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
 	}
 }
 
+void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	if (matrix_index == 0) {
+		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+	}
+}
+
 void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {
@@ -94,13 +104,13 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
 	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors = _mc_motors_mask;
+		_stopped_motors_mask |= _upwards_motors_mask;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors = 0;
+		_stopped_motors_mask &= ~_upwards_motors_mask;
 		break;
 	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -75,20 +75,21 @@ public:
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
-	void setFlightPhase(const FlightPhase &flight_phase) override;
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
-	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
-	uint32_t _stopped_motors{}; ///< currently stopped motors
+	uint32_t _upwards_motors_mask{};
+	uint32_t _forwards_motors_mask{};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 
 	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
-
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -86,7 +86,15 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 			_control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp);
 		}
 	}
+}
 
+void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	if (matrix_index == 0) {
+		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+	}
 }
 
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -97,16 +105,17 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	// update stopped motors //TODO: add option to switch off certain motors in FW
+	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors = 0;
+		_forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors = 0;
+		_forwards_motors_mask = 0;
+		_stopped_motors_mask = 0;
 		break;
 	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -72,16 +72,20 @@ public:
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }
 
-	uint32_t getStoppedMotors() const override { return _stopped_motors; }
 protected:
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
-	uint32_t _stopped_motors{}; ///< currently stopped motors
+	uint32_t _forwards_motors_mask{};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -68,10 +68,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
 	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
 			-1.f : _last_collective_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
-			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
+	_untiltable_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
+			     << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
+	_motors = _mc_rotors.getMotors();
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
@@ -118,7 +119,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 {
 	// apply tilt
 	if (matrix_index == 0) {
-
 		tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
 		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
@@ -145,12 +145,15 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 
 			// in FW directly use throttle sp
 			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-
 				for (int i = 0; i < _first_tilt_idx; ++i) {
 					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
 				}
 			}
 		}
+
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
+		}
 	}
 
 	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
@@ -180,13 +183,13 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors = _nontilted_motors;
+		_stopped_motors_mask |= _untiltable_motors;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:
 	case FlightPhase::TRANSITION_FF_TO_HF:
 	case FlightPhase::TRANSITION_HF_TO_FF:
-		_stopped_motors = 0;
+		_stopped_motors_mask = 0;
 		break;
 	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -84,8 +84,6 @@ public:
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
-	uint32_t getStoppedMotors() const override { return _stopped_motors; }
-
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
 protected:
@@ -94,8 +92,8 @@ protected:
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
 
-	uint32_t _nontilted_motors{}; ///< motors that are not tiltable
-	uint32_t _stopped_motors{}; ///< currently stopped motors
+	uint32_t _motors{};
+	uint32_t _untiltable_motors{};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ActuatorEffectivenessUUV.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessUUV::ActuatorEffectivenessUUV(ModuleParams *parent)
+	: ModuleParams(parent),
+	  _rotors(this)
+{
+}
+
+bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	// Motors
+	const bool rotors_added_successfully = _rotors.addActuators(configuration);
+	_motors_mask = _rotors.getMotors();
+
+	return rotors_added_successfully;
+}
+
+void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+
+class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	ActuatorEffectivenessUUV(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessUUV() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	const char *name() const override { return "UUV"; }
+
+protected:
+	ActuatorEffectivenessRotors _rotors;
+
+	uint32_t _motors_mask{};
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -34,6 +34,8 @@
 px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
+	ActuatorEffectivenessUUV.cpp
+	ActuatorEffectivenessUUV.hpp
 	ActuatorEffectivenessControlSurfaces.cpp
 	ActuatorEffectivenessControlSurfaces.hpp
 	ActuatorEffectivenessCustom.cpp

src/modules/control_allocator/ControlAllocator.cpp

@@ -247,7 +247,7 @@ ControlAllocator::update_effectiveness_source()
 			break;
 
 		case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR
-			tmp = new ActuatorEffectivenessRotors(this);
+			tmp = new ActuatorEffectivenessUUV(this);
 			break;
 
 		case EffectivenessSource::MULTIROTOR_WITH_TILT:

src/modules/control_allocator/ControlAllocator.hpp

@@ -51,6 +51,7 @@
 #include <ActuatorEffectivenessFixedWing.hpp>
 #include <ActuatorEffectivenessMCTilt.hpp>
 #include <ActuatorEffectivenessCustom.hpp>
+#include <ActuatorEffectivenessUUV.hpp>
 #include <ActuatorEffectivenessHelicopter.hpp>
 
 #include <ControlAllocation.hpp>

src/modules/ekf2/EKF/common.h

@@ -416,6 +416,7 @@ struct parameters {
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
 	int32_t flow_qual_min{1};               ///< minimum acceptable quality integer from  the flow sensor
+	int32_t flow_qual_min_gnd{0};           ///< minimum acceptable quality integer from  the flow sensor when on ground
 	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)
 
 	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -760,9 +760,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
-#if defined(CONFIG_EKF2_RANGE_FINDER)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
-#endif // CONFIG_EKF2_RANGE_FINDER
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@@ -1026,15 +1024,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
-#if defined(CONFIG_EKF2_RANGE_FINDER)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = _terrain_vpos - _state.pos(2);
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
-		} else
-#endif // CONFIG_EKF2_RANGE_FINDER
-		if (_control_status.flags.gnd_effect) {
+		} else if (_control_status.flags.gnd_effect) {
 			// Turn off ground effect compensation if it times out
 			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 				_control_status.flags.gnd_effect = false;

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -77,6 +77,16 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
 	aid_src.observation_variance[0] = R_LOS;
 	aid_src.observation_variance[1] = R_LOS;
+
+	const Vector24f state_vector = getStateAtFusionHorizonAsVector();
+
+	Vector2f innov_var;
+	Vector24f H;
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	innov_var.copyTo(aid_src.innovation_variance);
+
+	// run the innovation consistency check and record result
+	setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
 }
 
 void Ekf::fuseOptFlow()

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
 	const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
-	if (_delta_time_of < 0.1f) {
+	if (_delta_time_of < 0.2f) {
 		_imu_del_ang_of += delta_angle;
 		_delta_time_of += imu_delayed.delta_ang_dt;
 
@@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 
 	if (_flow_data_ready) {
-		const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
+		int32_t min_quality = _params.flow_qual_min;
+		if (!_control_status.flags.in_air) {
+			min_quality = _params.flow_qual_min_gnd;
+		}
+
+		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
 		const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
 		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
 

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -415,10 +415,12 @@ void Ekf::controlHaglFakeFusion()
 
 bool Ekf::isTerrainEstimateValid() const
 {
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// we have been fusing range finder measurements in the last 5 seconds
 	if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
 		return true;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds

src/modules/ekf2/EKF2.cpp

@@ -154,6 +154,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_of_n_min(_params->flow_noise),
 	_param_ekf2_of_n_max(_params->flow_noise_qual_min),
 	_param_ekf2_of_qmin(_params->flow_qual_min),
+	_param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd),
 	_param_ekf2_of_gate(_params->flow_innov_gate),
 	_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
 	_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
@@ -1295,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
+
+		// set dist bottom to scale flow innovation
+		const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
+		_preflt_checker.setDistBottom(dist_bottom);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -1461,13 +1466,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	// Distance to bottom surface (ground) in meters
-	// constrain the distance to ground to _rng_gnd_clearance
-	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
+	// Distance to bottom surface (ground) in meters, must be positive
+	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
-#endif // CONFIG_EKF2_RANGE_FINDER
 
 	_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
 	_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
@@ -2454,8 +2456,10 @@ void EKF2::UpdateCalibration(const hrt_abstime &timestamp, InFlightCalibration &
 		// consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit
 		const float bias_change_limit = 0.1f * bias_limit;
 
-		if ((cal.last_us != 0) && !(cal.bias - bias).longerThan(bias_change_limit)) {
-			cal.total_time_us += timestamp - cal.last_us;
+		if (!(cal.bias - bias).longerThan(bias_change_limit)) {
+			if (cal.last_us != 0) {
+				cal.total_time_us += timestamp - cal.last_us;
+			}
 
 			if (cal.total_time_us > 30_s) {
 				cal.cal_available = true;
@@ -2515,7 +2519,6 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
 	const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
-				&& _ekf.control_status_flags().mag_aligned_in_flight
 				&& !_ekf.control_status_flags().mag_fault
 				&& !_ekf.control_status_flags().mag_field_disturbed;
 

src/modules/ekf2/EKF2.hpp

@@ -662,7 +662,9 @@ private:
 		(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
 		_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
 		(ParamExtInt<px4::params::EKF2_OF_QMIN>)
-		_param_ekf2_of_qmin, ///< minimum acceptable quality integer from  the flow sensor
+		_param_ekf2_of_qmin, ///< minimum acceptable quality integer from  the flow sensor when in air
+		(ParamExtInt<px4::params::EKF2_OF_QMIN_GND>)
+		_param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from  the flow sensor when on ground
 		(ParamExtFloat<px4::params::EKF2_OF_GATE>)
 		_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
 		(ParamExtFloat<px4::params::EKF2_OF_POS_X>)

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
 	if (_is_using_flow_aiding) {
-		const Vector2f flow_innov = Vector2f(innov.flow);
+		const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom;
 		Vector2f flow_innov_lpf;
 		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
 		flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -77,6 +77,7 @@ public:
 	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
+	void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
 	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
@@ -179,6 +180,7 @@ private:
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	bool _is_using_flow_aiding {};
+	float _flow_dist_bottom {};
 	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
 	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)
 

src/modules/ekf2/ekf2_params.c

@@ -901,7 +901,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f);
 PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
 
 /**
- * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN.
+ * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN.
  *
  * @group EKF2
  * @min 0
@@ -909,6 +909,15 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
  */
 PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
 
+/**
+ * Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 255
+ */
+PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0);
+
 /**
  * Gate size for optical flow fusion
  *

src/modules/fw_att_control/fw_att_control_params.c

@@ -40,11 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-
 /**
  * Attitude Roll Time Constant
  *
@@ -86,7 +81,7 @@ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -98,7 +93,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -110,7 +105,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -122,7 +117,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
  *
  * @unit deg/s
  * @min 0.0
- * @max 90.0
+ * @max 180
  * @decimal 1
  * @increment 0.5
  * @group FW Attitude Control
@@ -140,7 +135,6 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f);
  */
 PARAM_DEFINE_INT32(FW_W_EN, 0);
 
-
 /**
  * Wheel steering rate proportional gain
  *
@@ -149,7 +143,7 @@ PARAM_DEFINE_INT32(FW_W_EN, 0);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Attitude Control
@@ -164,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.5
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Attitude Control
@@ -207,7 +201,7 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 2
  * @increment 0.05
  * @group FW Attitude Control

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -2847,31 +2847,36 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, con
 	// BUT no arbitrary max approach angle, approach entirely determined by generated
 	// bearing vectors
 
-	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
-	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
+	const Vector2f vector_A_to_B = waypoint_B - waypoint_A;
+	const Vector2f vector_B_to_A = -vector_A_to_B;
+	const Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
+	const Vector2f vector_B_to_vehicle = vehicle_pos - waypoint_B;
+	Vector2f desired_path = vector_A_to_B; // this is the default path tangent, but is overridden below
 
 	if (vector_A_to_B.norm() < FLT_EPSILON) {
 		// the waypoints are on top of each other and should be considered as a
 		// single waypoint, fly directly to it
 		if (vector_A_to_vehicle.norm() > FLT_EPSILON) {
-			vector_A_to_B = -vector_A_to_vehicle;
+			desired_path = -vector_A_to_vehicle;
 
 		} else {
 			// Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output.
 			return;
 		}
 
-
 	} else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) {
 		// we are in front of waypoint A, fly directly to it until we are within switch distance.
-
 		if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) {
-			vector_A_to_B = -vector_A_to_vehicle;
+			desired_path = -vector_A_to_vehicle;
 		}
+
+	} else if (vector_B_to_A.dot(vector_B_to_vehicle) < -FLT_EPSILON) {
+		// we are behind waypoint B, fly directly to it
+		desired_path = -vector_B_to_vehicle;
 	}
 
 	// track the line segment
-	Vector2f unit_path_tangent{vector_A_to_B.normalized()};
+	Vector2f unit_path_tangent{desired_path.normalized()};
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
 	_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f);
  * @increment 0.05
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.1f);
+PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.05f);
 
 /**
  * Integrator gain pitch

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -48,6 +48,8 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
 	_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
+	_handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -86,6 +88,10 @@ FixedwingRateControl::parameters_update()
 		// set FF gains to 0 as we add the FF control outside of the rate controller
 		Vector3f(0.f, 0.f, 0.f));
 
+	if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) {
+		param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en);
+	}
+
 
 	return PX4_OK;
 }
@@ -278,26 +284,41 @@ void FixedwingRateControl::Run()
 				_rate_control.resetIntegral();
 			}
 
-			// update saturation status from control allocation feedback
+			// Update saturation status from control allocation feedback
+			// TODO: send the unallocated value directly for better anti-windup
+			Vector3<bool> diffthr_enabled(
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
+			);
+
+			if (_vehicle_status.is_vtol_tailsitter) {
+				// Swap roll and yaw
+				diffthr_enabled.swapRows(0, 2);
+			}
+
+			// saturation handling for axis controlled by differential thrust (VTOL only)
 			control_allocator_status_s control_allocator_status;
 
-			if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
-				Vector<bool, 3> saturation_positive;
-				Vector<bool, 3> saturation_negative;
-
-				if (!control_allocator_status.torque_setpoint_achieved) {
-					for (size_t i = 0; i < 3; i++) {
-						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
-							saturation_positive(i) = true;
-
-						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
-							saturation_negative(i) = true;
-						}
+			// Set saturation flags for VTOL differential thrust feature
+			// If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors.
+			if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
+				for (size_t i = 0; i < 3; i++) {
+					if (diffthr_enabled(i)) {
+						_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
+						_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
 					}
 				}
+			}
 
-				// TODO: send the unallocated value directly for better anti-windup
-				_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
+			// Set saturation flags for control surface controlled axes
+			if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
+				for (size_t i = 0; i < 3; i++) {
+					if (!diffthr_enabled(i)) {
+						_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
+						_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
+					}
+				}
 			}
 
 			/* bi-linear interpolation over airspeed for actuator trim scheduling */

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -144,6 +144,16 @@ private:
 
 	bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
 
+	// enum for bitmask of VT_FW_DIFTHR_EN parameter options
+	enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t {
+		YAW_BIT = (1 << 0),
+		ROLL_BIT = (1 << 1),
+		PITCH_BIT = (1 << 2),
+	};
+
+	param_t _handle_param_vt_fw_difthr_en{PARAM_INVALID};
+	int32_t _param_vt_fw_difthr_en{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
 		(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -34,17 +34,12 @@
 /**
  * @file fw_rate_control_params.c
  *
- * Parameters defined by the fixed-wing attitude control task
+ * Parameters defined by the fixed-wing rate control task
  *
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-
 /**
  * Minimum Airspeed (CAS)
  *
@@ -58,7 +53,6 @@
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -72,7 +66,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -87,7 +80,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
  *
  * @value 0 Use airspeed in controller
  * @value 1 Do not use airspeed in controller
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
 
@@ -99,7 +92,6 @@ PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -115,7 +107,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
  *
  * @unit m/s
  * @min 0.5
- * @max 40
  * @decimal 1
  * @increment 0.5
  * @group FW TECS
@@ -127,7 +118,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -141,7 +132,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -156,7 +147,7 @@ PARAM_DEFINE_FLOAT(FW_PR_D, 0.f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.5
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -182,7 +173,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -197,7 +188,7 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -212,9 +203,9 @@ PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 0.2
- * @decimal 3
- * @increment 0.005
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
  * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f);
@@ -237,7 +228,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -252,7 +243,7 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
  *
  * @unit %/rad/s
  * @min 0.0
- * @max 1.0
+ * @max 10
  * @decimal 3
  * @increment 0.005
  * @group FW Rate Control
@@ -267,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f);
  *
  * @unit %/rad
  * @min 0.0
- * @max 50.0
+ * @max 10
  * @decimal 1
  * @increment 0.5
  * @group FW Rate Control
@@ -338,7 +329,7 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
  * This is the rate the controller is trying to achieve if the user applies full roll
  * stick input in acro mode.
  *
- * @min 45
+ * @min 10
  * @max 720
  * @unit deg
  * @group FW Rate Control
@@ -348,7 +339,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
 /**
  * Acro body pitch max rate setpoint.
  *
- * @min 45
+ * @min 10
  * @max 720
  * @unit deg
  * @group FW Rate Control
@@ -359,7 +350,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
  * Acro body yaw max rate setpoint.
  *
  * @min 10
- * @max 180
+ * @max 720
  * @unit deg
  * @group FW Rate Control
  */
@@ -397,8 +388,8 @@ PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
 * This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -410,8 +401,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f);
 * This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -423,8 +414,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f);
 * This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -436,8 +427,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f);
 * This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -449,8 +440,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f);
 * This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -462,8 +453,8 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
 * This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX.
  *
  * @group FW Rate Control
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -480,7 +471,7 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
 
@@ -494,7 +485,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
  * @min 0.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
 
@@ -508,7 +499,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
  * @min 0.0
  * @decimal 2
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
 
@@ -522,7 +513,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
  * @min 0.0
  * @decimal 1
  * @increment 0.01
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
 
@@ -534,6 +525,6 @@ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
  * @value 0 Disabled
  * @value 1 Flaps channel
  * @value 2 Aux1
- * @group FW Attitude Control
+ * @group FW Rate Control
  */
 PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -100,13 +100,13 @@ void LandingTargetEstimator::update()
 		}
 	}
 
-	if (!_new_sensorReport) {
+	if (!_new_irlockReport) {
 		// nothing to do
 		return;
 	}
 
 	// mark this sensor measurement as consumed
-	_new_sensorReport = false;
+	_new_irlockReport = false;
 
 
 	if (!_estimator_initialized) {
@@ -254,30 +254,7 @@ void LandingTargetEstimator::_update_topics()
 		_target_position_report.rel_pos_x += _params.offset_x;
 		_target_position_report.rel_pos_y += _params.offset_y;
 
-		_new_sensorReport = true;
-
-	} else if (_uwbDistanceSub.update(&_uwbDistance)) {
-		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
-			// don't have the data needed for an update
-			PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
-			return;
-		}
-
-		if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
-			PX4_WARN("Marker position reading invalid!");
-			return;
-		}
-
-		_new_sensorReport = true;
-
-		// The coordinate system is NED (north-east-down)
-		// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
-		// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
-		// To change POV we negate every Axis:
-		_target_position_report.timestamp = _uwbDistance.timestamp;
-		_target_position_report.rel_pos_x = -_uwbDistance.position[0];
-		_target_position_report.rel_pos_y = -_uwbDistance.position[1];
-		_target_position_report.rel_pos_z = -_uwbDistance.position[2];
+		_new_irlockReport = true;
 	}
 }
 

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -54,9 +54,6 @@
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/landing_target_innovations.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -153,14 +150,11 @@ private:
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
-	uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
 
 	vehicle_local_position_s	_vehicleLocalPosition{};
 	vehicle_attitude_s		_vehicleAttitude{};
 	vehicle_acceleration_s		_vehicle_acceleration{};
 	irlock_report_s			_irlockReport{};
-	uwb_grid_s		_uwbGrid{};
-	uwb_distance_s		_uwbDistance{};
 
 	// keep track of which topics we have received
 	bool _vehicleLocalPosition_valid{false};

src/modules/navigator/geofence_params.c

@@ -128,7 +128,9 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
 PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
 
 /**
- * Use Pre-emptive geofence triggering
+ * [EXPERIMENTAL] Use Pre-emptive geofence triggering
+ *
+ * WARNING: This experimental feature may cause flyaways. Use at your own risk.
  *
  * Predict the motion of the vehicle and trigger the breach if it is determined that the current trajectory
  * would result in a breach happening before the vehicle can make evasive maneuvers.
@@ -137,4 +139,4 @@ PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
  * @boolean
  * @group Geofence
  */
-PARAM_DEFINE_INT32(GF_PREDICT, 1);
+PARAM_DEFINE_INT32(GF_PREDICT, 0);

src/modules/navigator/loiter.cpp

@@ -61,6 +61,7 @@ Loiter::on_activation()
 		reposition();
 
 	} else {
+		// this is executed when the flight mode is switched to Hold manually, not through a reposition
 		set_loiter_position();
 	}
 
@@ -109,16 +110,11 @@ Loiter::set_loiter_position()
 		_mission_item.nav_cmd = NAV_CMD_IDLE;
 
 	} else {
-		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
 
 		} else {
-			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-				setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
-
-			} else {
-				setLoiterItemFromCurrentPosition(&_mission_item);
-			}
+			setLoiterItemFromCurrentPosition(&_mission_item);
 		}
 
 	}

src/modules/navigator/mission.cpp

@@ -982,7 +982,10 @@ Mission::set_mission_items()
 					_mission_item.altitude_is_relative = false;
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					_mission_item.autocontinue = true;
-					_mission_item.time_inside = 0.0f;
+
+					// have to reset here because these field were used in set_vtol_transition_item
+					_mission_item.time_inside = 0.f;
+					_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 
 					// make previous setpoint invalid, such that there will be no prev-current line following.
 					// if the vehicle drifted off the path during back-transition it should just go straight to the landing point

src/modules/navigator/rtl.cpp

@@ -544,6 +544,10 @@ void RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 
+			// have to reset here because these field were used in set_vtol_transition_item
+			_mission_item.time_inside = 0.f;
+			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+
 			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
 				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
 
@@ -554,7 +558,6 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _navigator->get_local_position()->heading;
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.origin = ORIGIN_ONBOARD;
 			break;
 		}

src/modules/navigator/vtol_takeoff.cpp

@@ -117,8 +117,9 @@ VtolTakeoff::on_active()
 				// we need the vehicle to loiter indefinitely but also we want this mission item to be reached as soon
 				// as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly,
 				// however it will just continue loitering as there is no next mission item
-				_mission_item.time_inside = 1;
+				_mission_item.time_inside = 1.f;
 				_mission_item.loiter_radius = _navigator->get_loiter_radius();
+				_mission_item.acceptance_radius  = _navigator->get_acceptance_radius();
 				_mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get();
 
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

src/modules/rc_update/rc_update.cpp

@@ -210,8 +210,12 @@ void RCUpdate::parameters_updated()
 		const uint16_t throttle_min = _parameters.min[throttle_channel];
 		const uint16_t throttle_trim = _parameters.trim[throttle_channel];
 		const uint16_t throttle_max = _parameters.max[throttle_channel];
+		const bool throttle_rev = _parameters.rev[throttle_channel];
 
-		if (throttle_min == throttle_trim) {
+		const bool normal_case = !throttle_rev && (throttle_trim == throttle_min);
+		const bool reversed_case = throttle_rev && (throttle_trim == throttle_max);
+
+		if (normal_case || reversed_case) {
 			const uint16_t new_throttle_trim = (throttle_min + throttle_max) / 2;
 			_parameters.trim[throttle_channel] = new_throttle_trim;
 		}
@@ -551,19 +555,16 @@ void RCUpdate::Run()
 	perf_end(_loop_perf);
 }
 
-switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const
+switch_pos_t RCUpdate::getRCSwitchOnOffPosition(uint8_t function, float threshold) const
 {
-	if (_rc.function[func] >= 0) {
-		const bool on_inv = (on_th < 0.f);
+	if (_rc.function[function] >= 0) {
+		float value = 0.5f * _rc.channels[_rc.function[function]] + 0.5f; // Rescale [-1,1] -> [0,1] range
 
-		const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
-
-		if (on_inv ? value < on_th : value > on_th) {
-			return manual_control_switches_s::SWITCH_POS_ON;
-
-		} else {
-			return manual_control_switches_s::SWITCH_POS_OFF;
+		if (threshold < 0.f) {
+			value = -value;
 		}
+
+		return (value > threshold) ? manual_control_switches_s::SWITCH_POS_ON : manual_control_switches_s::SWITCH_POS_OFF;
 	}
 
 	return manual_control_switches_s::SWITCH_POS_NONE;
@@ -630,18 +631,18 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 		}
 	}
 
-	switches.return_switch     = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN,     _param_rc_return_th.get());
-	switches.loiter_switch     = get_rc_sw2pos_position(rc_channels_s::FUNCTION_LOITER,     _param_rc_loiter_th.get());
-	switches.offboard_switch   = get_rc_sw2pos_position(rc_channels_s::FUNCTION_OFFBOARD,   _param_rc_offb_th.get());
-	switches.kill_switch       = get_rc_sw2pos_position(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get());
-	switches.arm_switch        = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH,  _param_rc_armswitch_th.get());
-	switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
-	switches.gear_switch       = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR,       _param_rc_gear_th.get());
-	switches.engage_main_motor_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR,
-					    _param_rc_eng_mot_th.get());
+	switches.return_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get());
+	switches.loiter_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get());
+	switches.offboard_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get());
+	switches.kill_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get());
+	switches.arm_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get());
+	switches.transition_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
+	switches.gear_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get());
+	switches.engage_main_motor_switch =
+		getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR, _param_rc_eng_mot_th.get());
 #if defined(ATL_MANTIS_RC_INPUT_HACKS)
-	switches.photo_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_3, 0.5f);
-	switches.video_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_4, 0.5f);
+	switches.photo_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_3, 0.5f);
+	switches.video_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_4, 0.5f);
 #endif
 
 	// last 2 switch updates identical within 1 second (simple protection from bad RC data)

src/modules/rc_update/rc_update.h

@@ -117,15 +117,16 @@ public:
 	float		get_rc_value(uint8_t func, float min_value, float max_value) const;
 
 	/**
-	 * Get switch position for specified function.
+	 * Get on/off switch position from the RC channel of the specified function
+	 *
+	 * @param function according to rc_channels_s::FUNCTION_XXX
+	 * @param threshold according to RC_XXX_TH parameters, negative means on and off are flipped
 	 */
-	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th) const;
+	switch_pos_t getRCSwitchOnOffPosition(uint8_t function, float threshold) const;
 
 	/**
 	 * Update parameters from RC channels if the functionality is activated and the
 	 * input has changed since the last update
-	 *
-	 * @param
 	 */
 	void		set_params_from_rc();
 

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -138,7 +138,7 @@ RoverPositionControl::manual_control_setpoint_poll()
 					}
 
 					_att_sp.yaw_body = _manual_yaw_sp;
-					_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
+					_att_sp.thrust_body[0] = _manual_control_setpoint.throttle;
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -152,7 +152,7 @@ RoverPositionControl::manual_control_setpoint_poll()
 					// Set heading from the manual roll input channel
 					_yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
 					// Set throttle from the manual throttle channel
-					_throttle_control = (_manual_control_setpoint.throttle + 1.f) * .5f;
+					_throttle_control = _manual_control_setpoint.throttle;
 					_reset_yaw_sp = true;
 				}