new INA231 power_monitor driver

We use lockdown to prevent outputs like motors and servos from being
active in HITL simulation. This means that we can't treat the lockdown
flag as a flight_terminated, otherwise we can't arm in HITL at all.

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -131,7 +131,6 @@ param set-default VT_F_TRANS_DUR 1
 param set-default VT_IDLE_PWM_MC 1025
 param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
-param set-default VT_FW_PERM_STAB 1
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4
 

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -181,6 +181,30 @@ then
 	pcf8583 start -X -a 0x51
 fi
 
+# INA226 digital power monitor
+if param compare SENS_EN_INA226 1
+then
+	ina226 -X start
+fi
+
+# INA228 digital power monitor
+if param compare SENS_EN_INA228 1
+then
+	ina228 -X start
+fi
+
+# INA231 digital power monitor
+if param compare SENS_EN_INA231 1
+then
+	ina231 -X start
+fi
+
+# INA238 digital power monitor
+if param compare SENS_EN_INA238 1
+then
+	ina238 -X start
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then

Tools/setup/ubuntu.sh

@@ -200,13 +200,12 @@ if [[ $INSTALL_SIM == "true" ]]; then
 
 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 		java_version=11
-		gazebo_version=9
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 		java_version=13
-		gazebo_version=11
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		java_version=11
 	else
 		java_version=14
-		gazebo_version=11
 	fi
 	# Java (jmavsim or fastrtps)
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -220,20 +219,30 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
 
 	# Gazebo
+	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+		gazebo_version=9
+		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		gazebo_packages="gazebo libgazebo-dev"
+	else
+		# default and Ubuntu 20.04
+		gazebo_version=11
+		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+	fi
+
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
 	sudo apt-get update -y --quiet
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
-		gazebo$gazebo_version \
+		$gazebo_packages \
 		gstreamer1.0-plugins-bad \
 		gstreamer1.0-plugins-base \
 		gstreamer1.0-plugins-good \
 		gstreamer1.0-plugins-ugly \
 		gstreamer1.0-libav \
 		libeigen3-dev \
-		libgazebo$gazebo_version-dev \
 		libgstreamer-plugins-base1.0-dev \
 		libimage-exiftool-perl \
 		libopencv-dev \

boards/matek/h743-mini/default.px4board

@@ -9,14 +9,15 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=n
 CONFIG_DRIVERS_CAMERA_TRIGGER=n
-CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
+CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_LIGHT=n
-CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_MAGNETOMETER=n
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/nxp/ucans32k146/src/bringup.c

@@ -159,10 +159,12 @@ int s32k1xx_bringup(void)
 	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
 		/* STB high -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
+		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ONE);
 
 	} else {
 		/* STB low -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
+		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ZERO);
 	}
 
 #endif

boards/px4/fmu-v5x/default.px4board

@@ -33,6 +33,7 @@ CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
+CONFIG_DRIVERS_POWER_MONITOR_INA231=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y

msg/sensor_combined.msg

@@ -2,22 +2,24 @@
 # These fields are scaled and offset-compensated where possible and do not
 # change with board revisions and sensor updates.
 
-uint64 timestamp				# time since system start (microseconds)
+uint64 timestamp                                # time since system start (microseconds)
 
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647   # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
 
 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds
+float32[3] gyro_rad                     # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
+uint32 gyro_integral_dt                 # gyro measurement sampling period in microseconds
 
-int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds
+int32 accelerometer_timestamp_relative  # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
+float32[3] accelerometer_m_s2           # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
+uint32 accelerometer_integral_dt        # accelerometer measurement sampling period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
-uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
 
-uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
-uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
+uint8 accelerometer_clipping    # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
+uint8 gyro_clipping             # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
+
+uint8 accel_calibration_count   # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count    # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.

msg/sensor_gyro.msg

@@ -11,6 +11,8 @@ float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
+uint8[3] clip_counter     # clip count per axis in the sample period
+
 uint8 samples             # number of raw samples that went into this message
 
 uint8 ORB_QUEUE_LENGTH = 8

msg/vehicle_attitude.msg

@@ -1,12 +1,12 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
 
-uint64 timestamp		    # time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
 
-uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[4] q			    # Quaternion rotation from the FRD body frame to the NED earth frame
-float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
-uint8 quat_reset_counter	# Quaternion reset counter
+float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth frame
+float32[4] delta_q_reset        # Amount by which quaternion has changed during last reset
+uint8 quat_reset_counter        # Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
 # TOPICS estimator_attitude

msg/vehicle_imu.msg

@@ -8,12 +8,15 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that
 
 float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
 float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
+
 uint16 delta_angle_dt           # integration period in microseconds
 uint16 delta_velocity_dt        # integration period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
+
+uint8 delta_angle_clipping     # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
 
 uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.

msg/vehicle_imu_status.msg

@@ -4,6 +4,7 @@ uint32 accel_device_id          # unique device ID for the sensor that does not
 uint32 gyro_device_id           # unique device ID for the sensor that does not change between power cycles
 
 uint32[3] accel_clipping        # total clipping per axis
+uint32[3] gyro_clipping         # total clipping per axis
 
 uint32 accel_error_count
 uint32 gyro_error_count

msg/vehicle_status.msg

@@ -1,5 +1,3 @@
-# If you change the order, add or remove arming_state_t states make sure to update the arrays
-# in state_machine_helper.cpp as well.
 uint64 timestamp				# time since system start (microseconds)
 
 uint8 ARMING_STATE_INIT = 0
@@ -69,13 +67,12 @@ uint8 system_type			# system type, contains mavlink MAV_TYPE
 uint8 system_id			# system id, contains MAVLink's system ID field
 uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
 
-uint8 vehicle_type          # Type of vehicle (fixed-wing, rotary wing, ground)
+uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
                             # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
                             # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
 
 bool is_vtol				# True if the system is VTOL capable
 bool is_vtol_tailsitter                 # True if the system performs a 90° pitch down rotation during transition from MC to FW
-bool vtol_fw_permanent_stab		# True if VTOL should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
 

msg/vtol_vehicle_status.msg

@@ -7,8 +7,6 @@ uint8 VEHICLE_VTOL_STATE_FW = 4
 
 uint64 timestamp			# time since system start (microseconds)
 
-bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
-bool vtol_in_trans_mode
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
+uint8 vehicle_vtol_state		# current state of the vtol, see VEHICLE_VTOL_STATE
+
 bool vtol_transition_failsafe		# vtol in transition failsafe mode
-bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode

src/drivers/drv_sensor.h

@@ -149,9 +149,6 @@
 #define DRV_DIST_DEVTYPE_SRF02        0x74
 #define DRV_DIST_DEVTYPE_TERARANGER   0x75
 #define DRV_DIST_DEVTYPE_VL53L0X      0x76
-#define DRV_POWER_DEVTYPE_INA226      0x77
-#define DRV_POWER_DEVTYPE_INA228      0x78
-#define DRV_POWER_DEVTYPE_VOXLPM      0x79
 
 #define DRV_LED_DEVTYPE_RGBLED        0x7a
 #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
@@ -188,7 +185,6 @@
 #define DRV_DIST_DEVTYPE_GY_US42       0x9C
 
 #define DRV_BAT_DEVTYPE_BATMON_SMBUS   0x9d
-#define DRV_POWER_DEVTYPE_INA238       0x9E
 #define DRV_GPIO_DEVTYPE_MCP23009      0x9F
 
 #define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@@ -208,6 +204,12 @@
 
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1
 
+#define DRV_POWER_DEVTYPE_INA226 0xC0
+#define DRV_POWER_DEVTYPE_INA228 0xC1
+#define DRV_POWER_DEVTYPE_INA231 0xC2
+#define DRV_POWER_DEVTYPE_INA238 0xC3
+#define DRV_POWER_DEVTYPE_VOXLPM 0xC4
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/dshot/DShot.cpp

@@ -455,29 +455,44 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	} else {
 		int telemetry_index = 0;
 
-		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
-
 		for (int i = 0; i < (int)num_outputs; i++) {
 
 			uint16_t output = outputs[i];
 
-			// DShot 3D splits the throttle ranges in two.
-			// This is in terms of DShot values, code below is in terms of actuator_output
-			// Direction 1) 48 is the slowest, 1047 is the fastest.
-			// Direction 2) 1049 is the slowest, 2047 is the fastest.
-			if (_param_dshot_3d_enable.get() || (reversible_outputs & (1u << i))) {
-				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
-					output = DSHOT_DISARM_VALUE;
-
-				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
-					output = 999 - output;
-				}
-			}
-
 			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
 
 			} else {
+
+				// DShot 3D splits the throttle ranges in two.
+				// This is in terms of DShot values, code below is in terms of actuator_output
+				// Direction 1) 48 is the slowest, 1047 is the fastest.
+				// Direction 2) 1049 is the slowest, 2047 is the fastest.
+				if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
+					if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
+						output = DSHOT_DISARM_VALUE;
+
+					} else {
+						bool upper_range = output >= 1000;
+
+						if (upper_range) {
+							output -= 1000;
+
+						} else {
+							output = 999 - output; // lower range is inverted
+						}
+
+						float max_output = 999.f;
+						float min_output = max_output * _param_dshot_min.get();
+						output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
+
+						if (upper_range) {
+							output += 1000;
+						}
+
+					}
+				}
+
 				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							telemetry_index == requested_telemetry_index);
 			}
@@ -556,6 +571,13 @@ void DShot::Run()
 
 	handle_vehicle_commands();
 
+	if (!_mixing_output.armed().armed) {
+		if (_reversible_outputs != _mixing_output.reversibleOutputs()) {
+			_reversible_outputs = _mixing_output.reversibleOutputs();
+			update_params();
+		}
+	}
+
 	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
 	_mixing_output.updateSubscriptions(true);
 
@@ -649,6 +671,13 @@ void DShot::update_params()
 	_mixing_output.setAllMinValues(math::constrain(static_cast<int>((_param_dshot_min.get() *
 				       static_cast<float>(DSHOT_MAX_THROTTLE))),
 				       DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE));
+
+	// Do not use the minimum parameter for reversible outputs
+	for (unsigned i = 0; i < _num_outputs; ++i) {
+		if ((1 << i) & _reversible_outputs) {
+			_mixing_output.minValue(i) = DSHOT_MIN_THROTTLE;
+		}
+	}
 }
 
 int DShot::ioctl(file *filp, int cmd, unsigned long arg)

src/drivers/dshot/DShot.h

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

src/drivers/imu/invensense/icm42670p/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021-2022 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

src/drivers/imu/invensense/icm42670p/ICM42670P.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
-	if (_drdy_gpio != 0) {
+	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
@@ -123,7 +123,7 @@ void ICM42670P::RunImpl()
 
 	switch (_state) {
 	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
+		// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
 		RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG);
 		_reset_timestamp = now;
 		_failure_count = 0;
@@ -133,7 +133,7 @@ void ICM42670P::RunImpl()
 
 	case STATE::WAIT_FOR_RESET:
 		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
+		    && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
 		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
 
 			// Wakeup accel and gyro and schedule remaining configuration
@@ -190,15 +190,19 @@ void ICM42670P::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -209,8 +213,6 @@ void ICM42670P::RunImpl()
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
 
-				// PX4_WARN("fifo_count = %d ",fifo_count);
-
 				if (fifo_count >= FIFO::SIZE) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
@@ -222,12 +224,17 @@ void ICM42670P::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
 						perf_count(_fifo_overflow_perf);
 						samples = 0;
-						// PX4_WARN("samples 1  = %d ",samples);
 					}
 				}
 			}
@@ -235,12 +242,9 @@ void ICM42670P::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				// PX4_WARN("samples 2  = %d ",samples);
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
-					// PX4_WARN("success  = %d ",success);
-
 					if (_failure_count > 0) {
 						_failure_count--;
 					}
@@ -260,9 +264,11 @@ void ICM42670P::RunImpl()
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+				    && RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1])
 				   ) {
 					_last_config_check_timestamp = now;
 					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg;
 
 				} else {
 					// register check failed, force reset
@@ -283,68 +289,8 @@ void ICM42670P::RunImpl()
 	}
 }
 
-void ICM42670P::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM42670P::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM42670P::ConfigureSampleRate(int sample_rate)
 {
-	if (sample_rate == 0) {
-		sample_rate = 800; // default to 800 Hz
-	}
-
 	// round down to nearest FIFO sample dt
 	const float min_interval = FIFO_SAMPLE_DT;
 	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
@@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
 	}
 }
 
-
-
 bool ICM42670P::Configure()
 {
 	// first set and clear all configured register bits
@@ -383,7 +327,9 @@ bool ICM42670P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
-	Mreg1Config();
+	for (const auto &reg_cfg : _register_mreg1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
 
 	// now check that all are configured
 	bool success = true;
@@ -394,11 +340,17 @@ bool ICM42670P::Configure()
 		}
 	}
 
-	success = Mreg1Check();
+	for (const auto &reg_cfg : _register_mreg1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
 
+	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 
-	ConfigureAccel();
-	ConfigureGyro();
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	_px4_gyro.set_range(math::radians(2000.f));
 
 	return success;
 }
@@ -411,11 +363,8 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM42670P::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM42670P::DataReadyInterruptConfigure()
@@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T &reg_cfg)
 	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
 	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
 		success = false;
 	}
 
 	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
 		success = false;
 	}
 
 	return success;
 }
 
-template <typename T>
-uint8_t ICM42670P::RegisterRead(T reg)
+uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
 {
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
@@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg)
 	return cmd[1];
 }
 
-template <typename T>
-void ICM42670P::RegisterWrite(T reg, uint8_t value)
+uint8_t ICM42670P::RegisterRead(Register::MREG1 reg)
+{
+	// BLK_SEL_R must be set to 0
+	RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00);
+
+	// MADDR_R must be set to 0x14 (address of the MREG1 register being accessed)
+	RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg);
+
+	// Wait for 10 µs
+	px4_udelay(10);
+
+	// Read register M_R to access the value in MREG1 register 0x14
+	uint8_t value = RegisterRead(Register::BANK_0::M_R);
+
+	// Wait for 10 µs
+	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
+	px4_udelay(10);
+
+	return value;
+}
+
+void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
 {
 	uint8_t cmd[2] { (uint8_t)reg, value };
-
 	transfer(cmd, cmd, sizeof(cmd));
 }
 
+void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
+{
+	// BLK_SEL_W must be set to 0
+	RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00);
+
+	// MADDR_W must be set to address of the MREG1 register being accessed
+	RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg);
+
+	// M_W must be set to the desired value
+	RegisterWrite(Register::BANK_0::M_W, value);
+
+	// Wait for 10 µs
+	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
+	px4_udelay(10);
+}
+
+
 template <typename T>
 void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
 {
@@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount()
 	return combine(fifo_count_buf[1], fifo_count_buf[2]);
 }
 
-
-
 bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 
 	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
 
-
 	if (fifo_count_bytes >= FIFO::SIZE) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
@@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
 			// gyro bit not set
 			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
 		}
 
 		if (valid) {
@@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		}
 	}
 
-	// PX4_WARN("valid_samples  = %d ",valid_samples);
 	if (valid_samples > 0) {
 		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
 		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
@@ -577,18 +571,30 @@ void ICM42670P::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);
 
+	// FIFO flush requires the following programming sequence:
+	//  Write FIFO_FLUSH = 1
+	//  Wait for 1.5 µs
+	//  Read FIFO_FLUSH, it should now be 0
+
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+	px4_udelay(2); // Wait for 1.5 µs
+
+	const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
+
+	if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
+		PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
+	}
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
 {
 	sensor_accel_fifo_s accel{};
 	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
+	accel.samples = samples;
 	accel.dt = FIFO_SAMPLE_DT;
 
 	for (int i = 0; i < samples; i++) {
@@ -598,18 +604,15 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
+		accel.x[i] = accel_x;
+		accel.y[i] = math::negate(accel_y);
+		accel.z[i] = math::negate(accel_z);
 	}
 
 	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
 				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
+	_px4_accel.updateFIFO(accel);
 }
 
 void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
 		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		gyro.y[i] = math::negate(gyro_y);
+		gyro.z[i] = math::negate(gyro_z);
 	}
 
 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature()
 		_px4_gyro.set_temperature(TEMP_degC);
 	}
 }
-
-
-uint8_t ICM42670P::RegisterReadBank1(uint8_t reg)
-{
-	uint8_t value;
-
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
-	RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg);
-	ScheduleDelayed(10_us);
-	value = RegisterRead((uint8_t)Register::BANK_0::M_R);
-	ScheduleDelayed(10_us);
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
-
-	return value;
-}
-
-void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value)
-{
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
-	RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg);
-	RegisterWrite((uint8_t)Register::BANK_0::M_W, value);
-	ScheduleDelayed(10_us);
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
-}
-
-void ICM42670P::Mreg1Config()
-{
-	uint8_t data;
-	uint8_t set_bits;
-	uint8_t clear_bits;
-
-	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
-	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
-		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
-
-	data = RegisterReadBank1(0x01);
-	data &= ~clear_bits;
-	data |= set_bits;
-	RegisterWriteBank1(0x01, data);
-
-
-	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
-	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
-	data = RegisterReadBank1(0x04);
-	data &= ~clear_bits;
-	data |= set_bits;
-	RegisterWriteBank1(0x04, data);
-}
-
-bool ICM42670P::Mreg1Check()
-{
-	uint8_t set_bits;
-	uint8_t clear_bits;
-	uint8_t reg_value;
-	bool success;
-	success = true;
-
-	reg_value = RegisterReadBank1(0x01);
-
-	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
-	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
-		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
-
-	if (set_bits && ((reg_value & set_bits) != set_bits)) {
-		success = false;
-	}
-
-	if (clear_bits && ((reg_value & clear_bits) != 0)) {
-		success = false;
-	}
-
-	reg_value = RegisterReadBank1(0x04);
-	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
-	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
-
-	if (set_bits && ((reg_value & set_bits) != set_bits)) {
-		success = false;
-	}
-
-	if (clear_bits && ((reg_value & clear_bits) != 0)) {
-		success = false;
-	}
-
-	return success;
-}

src/drivers/imu/invensense/icm42670p/ICM42670P.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 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
@@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
 public:
 	ICM42670P(const I2CSPIDriverConfig &config);
 	~ICM42670P() override;
+
 	static void print_usage();
 
 	void RunImpl();
@@ -68,12 +69,12 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 Hz accel & gyro ODR configured
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -85,10 +86,8 @@ private:
 		uint8_t FIFO_COUNTL{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
-
-
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -96,13 +95,17 @@ private:
 		uint8_t clear_bits{0};
 	};
 
+	struct register_mreg1_config_t {
+		Register::MREG1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
 	int probe() override;
 
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -111,9 +114,13 @@ private:
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();
 
+	uint8_t RegisterRead(Register::BANK_0 reg);
+	uint8_t RegisterRead(Register::MREG1 reg);
+
+	void RegisterWrite(Register::BANK_0 reg, uint8_t value);
+	void RegisterWrite(Register::MREG1 reg, uint8_t value);
+
 	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
 	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
 	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
 	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
@@ -126,12 +133,6 @@ private:
 	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	void UpdateTemperature();
 
-	uint8_t RegisterReadBank1(uint8_t reg);
-	void RegisterWriteBank1(uint8_t reg, uint8_t value);
-
-	void Mreg1Config();
-	bool Mreg1Check();
-
 	const spi_drdy_gpio_t _drdy_gpio;
 
 	PX4Accelerometer _px4_accel;
@@ -149,9 +150,7 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
-
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -159,26 +158,32 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
 	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		// Register                        | Set bits, Clear bits
-		{ Register::BANK_0::INT_CONFIG,    INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::FIFO_CONFIG1,   FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 },
-		{ Register::BANK_0::PWR_MGMT0,     PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
-		{ Register::BANK_0::GYRO_CONFIG0,  GYRO_CONFIG0_BIT::GYRO_ODR_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 },
-		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 },
-		{ Register::BANK_0::GYRO_CONFIG1,  GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
-		{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },
-		{ Register::BANK_0::FIFO_CONFIG2,  0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,  0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_SOURCE0,   INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_mreg1{0};
+	static constexpr uint8_t size_register_mreg1_cfg{2};
+	register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::MREG1::FIFO_CONFIG5,          FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::MREG1::INT_CONFIG0,           INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
 	};
 };

src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 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
@@ -54,7 +54,7 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-static constexpr uint32_t SPI_SPEED = 12 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x67;
@@ -66,21 +66,13 @@ namespace Register
 {
 
 enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG     = 0x01,
+	SIGNAL_PATH_RESET = 0x02,
 
 	INT_CONFIG        = 0x06,
 
-
 	TEMP_DATA1        = 0x09,
 	TEMP_DATA0        = 0x0A,
 
-	INT_STATUS        = 0x3A,
-	FIFO_COUNTH       = 0x3D,
-	FIFO_COUNTL       = 0x3E,
-	FIFO_DATA         = 0x3F,
-
-	SIGNAL_PATH_RESET = 0x02,
-
 	PWR_MGMT0         = 0x1F,
 	GYRO_CONFIG0      = 0x20,
 	ACCEL_CONFIG0     = 0x21,
@@ -90,87 +82,93 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG1      = 0x28,
 	FIFO_CONFIG2      = 0x29,
 	FIFO_CONFIG3      = 0x2A,
-
 	INT_SOURCE0       = 0x2B,
 
+	INT_STATUS        = 0x3A,
+	FIFO_COUNTH       = 0x3D,
+	FIFO_COUNTL       = 0x3E,
+	FIFO_DATA         = 0x3F,
+
 	WHO_AM_I          = 0x75,
-	// REG_BANK_SEL      = 0x76,
 
 	BLK_SEL_W         = 0x79,
 	MADDR_W           = 0x7A,
 	M_W               = 0x7B,
-
 	BLK_SEL_R         = 0x7C,
 	MADDR_R           = 0x7D,
 	M_R               = 0x7E,
-
 };
 
-enum class MREG_1 : uint8_t {
-	FIFO_CONFIG5_MREG1      = 0x01,
-	INT_CONFIG0_MREG1       = 0x04,
+enum class MREG1 : uint8_t {
+	FIFO_CONFIG5 = 0x01,
+
+	INT_CONFIG0  = 0x04,
 };
 
-enum class MREG_2 : uint8_t {
-	OTP_CTRL7_MREG2  = 0x06,
-};
-
-enum class MREG_3 : uint8_t {
-	XA_ST_DATA_MREG3 = 0x00,
-	YA_ST_DATA_MREG3 = 0x01,
-	ZA_ST_DATA_MREG3 = 0x02,
-	XG_ST_DATA_MREG3 = 0x03,
-	YG_ST_DATA_MREG3 = 0x04,
-	ZG_ST_DATA_MREG3 = 0x05,
-};
-
-
-
 };
 
 //---------------- BANK0 Register bits
 
 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	SOFT_RESET_DEVICE_CONFIG  = Bit4, //
-	FIFO_FLUSH                = Bit2,
+	SOFT_RESET_DEVICE_CONFIG  = Bit4, // 1: Software reset enabled
+	FIFO_FLUSH                = Bit2, // When set to 1, FIFO will get flushed
 };
 
 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
+	INT1_MODE          = Bit2, // INT1 interrupt mode 1: Latched mode
+	INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull
+	INT1_POLARITY      = Bit0, // INT1 interrupt polarity 0: Active low
 };
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	// 3:2 GYRO_MODE
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	// 1:0 ACCEL_MODE
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 6:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS_SET   = 0,           // 0b000 = ±2000dps
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5,
+
+	// 3:0 GYRO_ODR
+	GYRO_ODR_1600HZ_SET        = Bit2 | Bit0, // 0101: 1600Hz
+	GYRO_ODR_1600HZ_CLEAR      = Bit3 | Bit1,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 6:5 ACCEL_UI_FS_SEL
+	ACCEL_UI_FS_SEL_16G_SET   = 0,           // 000: ±16g
+	ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5,
+
+	// 3:0 ACCEL_ODR
+	ACCEL_ODR_1600HZ_SET      = Bit2 | Bit0, // 0101: 1600Hz
+	ACCEL_ODR_1600HZ_CLEAR    = Bit3 | Bit1,
+};
+
 // GYRO_CONFIG1
 enum GYRO_CONFIG1_BIT : uint8_t {
-
-	// 2:0 GYRO_ODR
-	GYRO_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
-	GYRO_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
-	GYRO_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
-	GYRO_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
-	GYRO_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
-	GYRO_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
-	GYRO_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
+	// 2:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-
-	// 2:0 ACCEL_ODR
-	ACCEL_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
-	ACCEL_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
-	ACCEL_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
-	ACCEL_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
-	ACCEL_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
-	ACCEL_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
-	ACCEL_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
+	// 2:0 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };
+
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	// 1 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
+	// FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
+	FIFO_BYPASS            = Bit0, // 0: FIFO is not bypassed
 };
 
 // INT_STATUS
@@ -180,113 +178,63 @@ enum INT_STATUS_BIT : uint8_t {
 	FIFO_FULL_INT  = Bit1,
 };
 
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
 };
 
-// GYRO_CONFIG0
-enum GYRO_CONFIG0_BIT : uint8_t {
-	// 6:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS = 0,            // 0b000 = ±2000dps
-	GYRO_FS_SEL_1000_DPS = Bit5,         // 0b001 = ±1000 dps
-	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps
 
-	// 3:0 GYRO_ODR
-	GYRO_ODR_1600Hz      = Bit2 | Bit0,         // 0101: 1600Hz
-	GYRO_ODR_800Hz       = Bit2 | Bit1,         // 0110: 800Hz
-	GYRO_ODR_400Hz       = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
-	GYRO_ODR_200Hz       = Bit3,         // 1000: 200Hz
-};
-
-// ACCEL_CONFIG0
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 6:5 ACCEL_FS_SEL
-	ACCEL_FS_SEL_16G = 0,           // 000: ±16g
-	ACCEL_FS_SEL_8G  = Bit5,        // 001: ±8g
-	ACCEL_FS_SEL_4G  = Bit6,        // 010: ±4g
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5, // 011: ±2g
-
-	// 3:0 ACCEL_ODR
-	ACCEL_ODR_1600Hz  = Bit2 | Bit0,         // 0101: 1600Hz
-	ACCEL_ODR_800Hz  = Bit2 | Bit1,         // 0110: 800Hz
-	ACCEL_ODR_400Hz   = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
-	ACCEL_ODR_200Hz   = Bit3,         	// 1000: 200Hz
-};
+//---------------- USER BANK MREG1 Register bits
 
 // FIFO_CONFIG5
 enum FIFO_CONFIG5_BIT : uint8_t {
-	FIFO_RESUME_PARTIAL_RD = Bit4,
-	FIFO_WM_GT_TH          = Bit5,
-	FIFO_HIRES_EN          = Bit3,
-	FIFO_TMST_FSYNC_EN     = Bit2,
-	FIFO_GYRO_EN           = Bit1,
-	FIFO_ACCEL_EN          = Bit0,
+	FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM
+	FIFO_GYRO_EN  = Bit1, // 1: Enables Gyro packets to go to FIFO
+	FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO
 };
 
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
-};
-
-// INT_SOURCE0
-enum INT_SOURCE0_BIT : uint8_t {
-	ST_INT1_END        = Bit7,
-	FSYNC_INT1_EN      = Bit6,
-	PLL_RDY_INT1_EN    = Bit5,
-	RESET_DONE_INT1_EN = Bit4,
-	DRDY_INT1_EN       = Bit3,
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
-	FIFO_FULL_INT1_EN  = Bit1,
-	AGC_RDY_INT1_EN    = Bit0,
-};
-
-// REG_BANK_SEL
-enum REG_BANK_SEL_BIT : uint8_t {
-	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
-	USER_BANK_1 = Bit4,        // 1: Select USER BANK 1.
-	USER_BANK_2 = Bit5,        // 2: Select USER BANK 2.
-	USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
+	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
 };
 
 namespace FIFO
 {
-static constexpr size_t SIZE = 2048;
+static constexpr size_t SIZE = 1024;
 
 // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
 
 // Packet 3
 struct DATA {
 	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1;
-	uint8_t ACCEL_DATA_X0;
-	uint8_t ACCEL_DATA_Y1;
-	uint8_t ACCEL_DATA_Y0;
-	uint8_t ACCEL_DATA_Z1;
-	uint8_t ACCEL_DATA_Z0;
-	uint8_t GYRO_DATA_X1;
-	uint8_t GYRO_DATA_X0;
-	uint8_t GYRO_DATA_Y1;
-	uint8_t GYRO_DATA_Y0;
-	uint8_t GYRO_DATA_Z1;
-	uint8_t GYRO_DATA_Z0;
-	uint8_t temperature;  // Temperature[7:0]
-	uint8_t timestamp_l;
-	uint8_t timestamp_h;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t temperature;   // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
 enum FIFO_HEADER_BIT : uint8_t {
 	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6,
-	HEADER_GYRO            = Bit5,
-	HEADER_20              = Bit4,
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	HEADER_ODR_ACCEL       = Bit1,
-	HEADER_ODR_GYRO        = Bit0,
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
 };
 
 }

src/drivers/power_monitor/ina231/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2022 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__power_monitor__ina231
+	MAIN ina231
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		ina231_main.cpp
+		ina231.cpp
+	DEPENDS
+		battery
+		px4_work_queue
+	)

src/drivers/power_monitor/ina231/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_POWER_MONITOR_INA231
+	bool "ina231"
+	default n
+	---help---
+		Enable support for ina231

src/drivers/power_monitor/ina231/ina231.cpp

@@ -0,0 +1,322 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "ina231.h"
+
+INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) :
+	I2C(config),
+	ModuleParams(nullptr),
+	I2CSPIDriver(config),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ina231_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ina231_com_err")),
+	_collection_errors(perf_alloc(PC_COUNT, "ina231_collection_err")),
+	_measure_errors(perf_alloc(PC_COUNT, "ina231_measurement_err")),
+	_battery(battery_index, this, INA231_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
+{
+	float fvalue = MAX_CURRENT;
+	_max_current = fvalue;
+	param_t ph = param_find("INA231_CURRENT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_max_current = fvalue;
+	}
+
+	fvalue = INA231_SHUNT;
+	_rshunt = fvalue;
+	ph = param_find("INA231_SHUNT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_rshunt = fvalue;
+	}
+
+	ph = param_find("INA231_CONFIG");
+	int32_t value = INA231_CONFIG;
+	_config = (uint16_t)value;
+
+	if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
+		_config = (uint16_t)value;
+	}
+
+	_mode_triggered = ((_config & INA231_MODE_MASK) >> INA231_MODE_SHIFTS) <=
+			  ((INA231_MODE_SHUNT_BUS_TRIG & INA231_MODE_MASK) >>
+			   INA231_MODE_SHIFTS);
+
+	_current_lsb = _max_current / DN_MAX;
+	_power_lsb = 25 * _current_lsb;
+
+	// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
+	_battery.setConnected(false);
+	_battery.updateVoltage(0.f);
+	_battery.updateCurrent(0.f);
+	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+}
+
+INA231::~INA231()
+{
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_collection_errors);
+	perf_free(_measure_errors);
+}
+
+int INA231::read(uint8_t address, int16_t &data)
+{
+	// read desired little-endian value via I2C
+	uint16_t received_bytes;
+	int ret = PX4_ERROR;
+
+	for (size_t i = 0; i < 3; i++) {
+		ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
+
+		if (ret == PX4_OK) {
+			data = swap16(received_bytes);
+			break;
+
+		} else {
+			perf_count(_comms_errors);
+			PX4_DEBUG("i2c::transfer returned %d", ret);
+		}
+	}
+
+	return ret;
+}
+
+int INA231::write(uint8_t address, uint16_t value)
+{
+	uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
+	return transfer(data, sizeof(data), nullptr, 0);
+}
+
+int
+INA231::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != PX4_OK) {
+		return ret;
+	}
+
+	write(INA231_REG_CONFIGURATION, INA231_RST);
+
+	_cal = INA231_CONST / (_current_lsb * _rshunt);
+
+	if (write(INA231_REG_CALIBRATION, _cal) < 0) {
+		return -3;
+	}
+
+	// If we run in continuous mode then start it here
+
+	if (!_mode_triggered) {
+		ret = write(INA231_REG_CONFIGURATION, _config);
+
+	} else {
+		ret = PX4_OK;
+	}
+
+	start();
+	_sensor_ok = true;
+
+	_initialized = ret == PX4_OK;
+	return ret;
+}
+
+int
+INA231::force_init()
+{
+	int ret = init();
+
+	start();
+
+	return ret;
+}
+
+int
+INA231::probe()
+{
+	int16_t read1 = 0;
+
+	if (read(INA231_REG_CONFIGURATION, read1) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	int16_t read2 = 0;
+
+	if (read(INA231_REG_CONFIGURATION, read2) != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	if (read1 != read2) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int
+INA231::measure()
+{
+	int ret = PX4_OK;
+
+	if (_mode_triggered) {
+		ret = write(INA231_REG_CONFIGURATION, _config);
+
+		if (ret < 0) {
+			perf_count(_comms_errors);
+			PX4_DEBUG("i2c::transfer returned %d", ret);
+		}
+	}
+
+	return ret;
+}
+
+int
+INA231::collect()
+{
+	perf_begin(_sample_perf);
+
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+
+		updateParams();
+	}
+
+	// read from the sensor
+	// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
+	bool success{true};
+	success = success && (read(INA231_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
+	// success = success && (read(INA231_REG_POWER, _power) == PX4_OK);
+	success = success && (read(INA231_REG_CURRENT, _current) == PX4_OK);
+	// success = success && (read(INA231_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
+
+	if (!success) {
+		PX4_DEBUG("error reading from sensor");
+		_bus_voltage = _power = _current = _shunt = 0;
+	}
+
+	_battery.setConnected(success);
+	_battery.updateVoltage(static_cast<float>(_bus_voltage * INA231_VSCALE));
+	_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
+	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+
+	perf_end(_sample_perf);
+
+	if (success) {
+		return PX4_OK;
+
+	} else {
+		return PX4_ERROR;
+	}
+}
+
+void
+INA231::start()
+{
+	ScheduleClear();
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	_measure_interval = INA231_CONVERSION_INTERVAL;
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(5);
+}
+
+void
+INA231::RunImpl()
+{
+	if (_initialized) {
+		if (_collect_phase) {
+			/* perform collection */
+			if (collect() != PX4_OK) {
+				perf_count(_collection_errors);
+				/* if error restart the measurement state machine */
+				start();
+				return;
+			}
+
+			/* next phase is measurement */
+			_collect_phase = !_mode_triggered;
+
+			if (_measure_interval > INA231_CONVERSION_INTERVAL) {
+				/* schedule a fresh cycle call when we are ready to measure again */
+				ScheduleDelayed(_measure_interval - INA231_CONVERSION_INTERVAL);
+				return;
+			}
+		}
+
+		/* Measurement  phase */
+
+		/* Perform measurement */
+		if (measure() != PX4_OK) {
+			perf_count(_measure_errors);
+		}
+
+		/* next phase is collection */
+		_collect_phase = true;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		ScheduleDelayed(INA231_CONVERSION_INTERVAL);
+
+	} else {
+		_battery.setConnected(false);
+		_battery.updateVoltage(0.f);
+		_battery.updateCurrent(0.f);
+		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+
+		if (init() != PX4_OK) {
+			ScheduleDelayed(INA231_INIT_RETRY_INTERVAL_US);
+		}
+	}
+}
+
+void
+INA231::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	if (_initialized) {
+		perf_print_counter(_sample_perf);
+		perf_print_counter(_comms_errors);
+
+		printf("poll interval:  %u \n", _measure_interval);
+
+	} else {
+		PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
+			 INA231_INIT_RETRY_INTERVAL_US / 1000);
+	}
+}

src/drivers/power_monitor/ina231/ina231.h

@@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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 ina231.h
+ *
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <drivers/device/i2c.h>
+#include <lib/perf/perf_counter.h>
+#include <battery/battery.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace time_literals;
+
+/* Configuration Constants */
+#define INA231_BASEADDR 	                    0x40 /* 7-bit address. 8-bit address is 0x44 */
+// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to
+// connect to the INA231 every this many microseconds
+#define INA231_INIT_RETRY_INTERVAL_US			500000
+
+/* INA231 Registers addresses */
+#define INA231_REG_CONFIGURATION             (0x00)
+#define INA231_REG_SHUNTVOLTAGE              (0x01)
+#define INA231_REG_BUSVOLTAGE                (0x02)
+#define INA231_REG_POWER                     (0x03)
+#define INA231_REG_CURRENT                   (0x04)
+#define INA231_REG_CALIBRATION               (0x05)
+#define INA231_REG_MASKENABLE                (0x06)
+#define INA231_REG_ALERTLIMIT                (0x07)
+#define INA231_MFG_ID                        (0xfe)
+#define INA231_MFG_DIEID                     (0xff)
+
+/* INA231 Configuration Register */
+#define INA231_MODE_SHIFTS                   (0)
+#define INA231_MODE_MASK                     (7 << INA231_MODE_SHIFTS)
+#define INA231_MODE_SHUTDOWN                 (0 << INA231_MODE_SHIFTS)
+#define INA231_MODE_SHUNT_TRIG               (1 << INA231_MODE_SHIFTS)
+#define INA231_MODE_BUS_TRIG                 (2 << INA231_MODE_SHIFTS)
+#define INA231_MODE_SHUNT_BUS_TRIG           (3 << INA231_MODE_SHIFTS)
+#define INA231_MODE_ADC_OFF                  (4 << INA231_MODE_SHIFTS)
+#define INA231_MODE_SHUNT_CONT               (5 << INA231_MODE_SHIFTS)
+#define INA231_MODE_BUS_CONT                 (6 << INA231_MODE_SHIFTS)
+#define INA231_MODE_SHUNT_BUS_CONT           (7 << INA231_MODE_SHIFTS)
+
+#define INA231_VSHCT_SHIFTS                  (3)
+#define INA231_VSHCT_MASK                    (7 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_140US                   (0 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_204US                   (1 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_332US                   (2 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_588US                   (3 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_1100US                  (4 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_2116US                  (5 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_4156US                  (6 << INA231_VSHCT_SHIFTS)
+#define INA231_VSHCT_8244US                  (7 << INA231_VSHCT_SHIFTS)
+
+#define INA231_VBUSCT_SHIFTS                 (6)
+#define INA231_VBUSCT_MASK                   (7 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_140US                  (0 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_204US                  (1 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_332US                  (2 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_588US                  (3 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_1100US                 (4 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_2116US                 (5 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_4156US                 (6 << INA231_VBUSCT_SHIFTS)
+#define INA231_VBUSCT_8244US                 (7 << INA231_VBUSCT_SHIFTS)
+
+#define INA231_AVERAGES_SHIFTS                (9)
+#define INA231_AVERAGES_MASK                  (7 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_1                     (0 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_4                     (1 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_16                    (2 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_64                    (3 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_128                   (4 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_256                   (5 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_512                   (6 << INA231_AVERAGES_SHIFTS)
+#define INA231_AVERAGES_1024                  (7 << INA231_AVERAGES_SHIFTS)
+
+#define INA231_CONFIG (INA231_MODE_SHUNT_BUS_CONT | INA231_VSHCT_588US | INA231_VBUSCT_588US | INA231_AVERAGES_64)
+
+#define INA231_RST                            (1 << 15)
+
+/* INA231 Enable / Mask Register */
+
+#define INA231_LEN                           (1 << 0)
+#define INA231_APOL                          (1 << 1)
+#define INA231_OVF                           (1 << 2)
+#define INA231_CVRF                          (1 << 3)
+#define INA231_AFF                           (1 << 4)
+
+#define INA231_CNVR                          (1 << 10)
+#define INA231_POL                           (1 << 11)
+#define INA231_BUL                           (1 << 12)
+#define INA231_BOL                           (1 << 13)
+#define INA231_SUL                           (1 << 14)
+#define INA231_SOL                           (1 << 15)
+
+#define INA231_SAMPLE_FREQUENCY_HZ            10
+#define INA231_SAMPLE_INTERVAL_US             (1_s / INA231_SAMPLE_FREQUENCY_HZ)
+#define INA231_CONVERSION_INTERVAL            (INA231_SAMPLE_INTERVAL_US - 7)
+#define MAX_CURRENT                           164.0f    /* 164 Amps */
+#define DN_MAX                                32768.0f  /* 2^15 */
+#define INA231_CONST                          0.00512f  /* is an internal fixed value used to ensure scaling is maintained properly  */
+#define INA231_SHUNT                          0.0005f   /* Shunt is 500 uOhm */
+#define INA231_VSCALE                         0.00125f  /* LSB of voltage is 1.25 mV  */
+
+#define swap16(w)                       __builtin_bswap16((w))
+
+class INA231 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA231>
+{
+public:
+	INA231(const I2CSPIDriverConfig &config, int battery_index);
+	virtual ~INA231();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	void	RunImpl();
+
+	int 		  init() override;
+
+	/**
+	 * Tries to call the init() function. If it fails, then it will schedule to retry again in
+	 * INA231_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
+	 *
+	 * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
+	 */
+	int force_init();
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				      print_status() override;
+
+protected:
+	int	  		probe() override;
+
+private:
+	bool			        _sensor_ok{false};
+	unsigned                        _measure_interval{0};
+	bool			        _collect_phase{false};
+	bool 					_initialized{false};
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t 		_collection_errors;
+	perf_counter_t 		_measure_errors;
+
+	int16_t           _bus_voltage{0};
+	int16_t           _power{0};
+	int16_t           _current{0};
+	int16_t           _shunt{0};
+	int16_t           _cal{0};
+	bool              _mode_triggered{false};
+
+	float             _max_current{MAX_CURRENT};
+	float             _rshunt{INA231_SHUNT};
+	uint16_t          _config{INA231_CONFIG};
+	float             _current_lsb{_max_current / DN_MAX};
+	float             _power_lsb{25.0f * _current_lsb};
+
+	Battery 		  _battery;
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	int read(uint8_t address, int16_t &data);
+	int write(uint8_t address, uint16_t data);
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				      start();
+
+	int					     measure();
+	int					     collect();
+
+};

src/drivers/power_monitor/ina231/ina231_main.cpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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 <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "ina231.h"
+
+I2CSPIDriverBase *INA231::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	INA231 *instance = new INA231(config, config.custom1);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (config.keep_running) {
+		if (instance->force_init() != PX4_OK) {
+			PX4_INFO("Failed to init INA231 on bus %d, but will try again periodically.", config.bus);
+		}
+
+	} else if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	return instance;
+}
+
+void
+INA231::print_usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for the INA231 power monitor.
+
+Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address.
+
+For example, one instance can run on Bus 2, address 0x44, and one can run on Bus 2, address 0x45.
+
+If the INA231 module is not powered, then by default, initialization of the driver will fail. To change this, use
+the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again
+every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without
+this flag set, the battery must be plugged in before starting the driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ina231", "driver");
+
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x40);
+	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
+	PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int
+ina231_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = INA231;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = INA231_BASEADDR;
+	cli.default_i2c_frequency = 100000;
+	cli.support_keep_running = true;
+	cli.custom1 = 1;
+
+	while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) {
+		switch (ch) {
+		case 't': // battery index
+			cli.custom1 = (int)strtol(cli.optArg(), NULL, 0);
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA231);
+
+	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/power_monitor/ina231/ina231_params.c

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable INA231 Power Monitor
+ *
+ * For systems a INA231 Power Monitor, this should be set to true
+ *
+ * @group Sensors
+ * @boolean
+ * @reboot_required true
+*/
+PARAM_DEFINE_INT32(SENS_EN_INA231, 0);
+
+/**
+ * INA231 Power Monitor Config
+ *
+ * @group Sensors
+ * @min 0
+ * @max 65535
+ * @decimal 1
+ * @increment 1
+*/
+PARAM_DEFINE_INT32(INA231_CONFIG, 18139);
+
+/**
+ * INA231 Power Monitor Max Current
+ *
+ * @group Sensors
+ * @min 0.1
+ * @max 200.0
+ * @decimal 2
+ * @increment 0.1
+ */
+PARAM_DEFINE_FLOAT(INA231_CURRENT, 164.0f);
+
+/**
+ * INA231 Power Monitor Shunt
+ *
+ * @group Sensors
+ * @min 0.000000001
+ * @max 0.1
+ * @decimal 10
+ * @increment .000000001
+ */
+PARAM_DEFINE_FLOAT(INA231_SHUNT, 0.0005f);

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -50,6 +50,22 @@ static constexpr int32_t sum(const int16_t samples[], uint8_t len)
 	return sum;
 }
 
+static constexpr uint8_t clipping(const int16_t samples[], uint8_t len)
+{
+	unsigned clip_count = 0;
+
+	for (int n = 0; n < len; n++) {
+		// - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1
+		// - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1)
+		//   and sensors that may re-use the lowest bit for other purposes (sync indicator, etc)
+		if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
 	_device_id{device_id},
 	_rotation{rotation}
@@ -90,6 +106,8 @@ void PX4Gyroscope::set_scale(float scale)
 		}
 
 		_scale = scale;
+
+		UpdateClipLimit();
 	}
 }
 
@@ -107,6 +125,9 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
 	report.x = x * _scale;
 	report.y = y * _scale;
 	report.z = z * _scale;
+	report.clip_counter[0] = (fabsf(x) >= _clip_limit);
+	report.clip_counter[1] = (fabsf(y) >= _clip_limit);
+	report.clip_counter[2] = (fabsf(z) >= _clip_limit);
 	report.samples = 1;
 	report.timestamp = hrt_absolute_time();
 
@@ -145,8 +166,17 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 	_last_sample[1] = sample.y[N - 1];
 	_last_sample[2] = sample.z[N - 1];
 
+	report.clip_counter[0] = clipping(sample.x, N);
+	report.clip_counter[1] = clipping(sample.y, N);
+	report.clip_counter[2] = clipping(sample.z, N);
 	report.samples = N;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
 }
+
+void PX4Gyroscope::UpdateClipLimit()
+{
+	// 99.9% of potential max
+	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
+}

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -53,7 +52,7 @@ public:
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
-	void set_range(float range) { _range = range; }
+	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
@@ -64,6 +63,8 @@ public:
 	int get_instance() { return _sensor_pub.get_instance(); };
 
 private:
+	void UpdateClipLimit();
+
 	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
 
@@ -76,6 +77,8 @@ private:
 	float			_scale{1.f};
 	float			_temperature{NAN};
 
+	float			_clip_limit{_range / _scale};
+
 	uint32_t		_error_count{0};
 
 	int16_t			_last_sample[3] {};

src/lib/parameters/param_translation.cpp

@@ -146,6 +146,25 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-04-11: translate VT_PTCH_MIN to VT_PITCH_MIN
+	{
+		if (strcmp("VT_PTCH_MIN", node->name) == 0) {
+			strcpy(node->name, "VT_PITCH_MIN");
+			PX4_INFO("copying %s -> %s", "VT_PTCH_MIN", "VT_PITCH_MIN");
+			return true;
+		}
+	}
+
+	// 2022-04-11: translate VT_LND_PTCH_MIN to VT_LND_PITCH_MIN
+	{
+		if (strcmp("VT_LND_PTCH_MIN", node->name) == 0) {
+			strcpy(node->name, "VT_LND_PITCH_MIN");
+			PX4_INFO("copying %s -> %s", "VT_LND_PTCH_MIN", "VT_LND_PITCH_MIN");
+			return true;
+		}
+	}
+
+
 	// 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R
 	{
 		if (strcmp("NAV_GPSF_LT", node->name) == 0) {

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -513,7 +513,8 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	// update wind and airspeed estimator
 	_wind_estimator_sideslip.update(_time_now_usec);
 
-	if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
+	if (_vehicle_local_position_valid
+	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.q);
 

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -38,28 +38,6 @@
 constexpr bool
 ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
 
-events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arming_state)
-{
-	switch (arming_state) {
-	case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init;
-
-	case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby;
-
-	case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed;
-
-	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error;
-
-	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown;
-
-	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore;
-	}
-
-	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore,
-		      "enum def mismatch");
-
-	return events::px4::enums::arming_state_t::init;
-}
-
 transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
 		const vehicle_control_mode_s &control_mode, const safety_s &safety,
 		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
@@ -73,13 +51,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		      "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
 
 	transition_result_t ret = TRANSITION_DENIED;
-	arming_state_t current_arming_state = status.arming_state;
 	bool feedback_provided = false;
 
 	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == current_arming_state) {
+	if (new_arming_state == _arm_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
@@ -113,7 +90,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			if ((_last_preflight_check == 0) || (hrt_elapsed_time(&_last_preflight_check) > 1000 * 1000)) {
 
 				status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
-						status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
+						status_flags, control_mode, false, !isArmed(),
 						time_since_boot);
 
 				_last_preflight_check = hrt_absolute_time();
@@ -121,7 +98,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		}
 
 		// Check that we have a valid state transition
-		bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
+		bool valid_transition = arming_transitions[new_arming_state][_arm_state];
 
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
@@ -129,7 +106,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 				//      Do not perform pre-arm checks if coming from in air restore
 				//      Allow if vehicle_status_s::HIL_STATE_ON
-				if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
+				if (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
 
 					bool prearm_check_ret = true;
 
@@ -154,8 +131,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			status_flags.system_sensors_initialized = true;
 
 			/* recover from a prearm fail */
-			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+			if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+				_arm_state = vehicle_status_s::ARMING_STATE_STANDBY;
 			}
 
 			// HIL can always go to standby
@@ -166,7 +143,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 		if (!hil_enabled &&
 		    (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-		    (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
+		    (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
 
 			// Sensors need to be initialized for STANDBY state, except for HIL
 			if (!status_flags.system_sensors_initialized) {
@@ -177,21 +154,20 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 		// Finish up the state transition
 		if (valid_transition) {
-			bool was_armed = armed.armed;
-			armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-					     || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
 			ret = TRANSITION_CHANGED;
-			status.arming_state = new_arming_state;
 
-			if (was_armed && !armed.armed) { // disarm transition
+			// Record arm/disarm reason
+			if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition
 				status.latest_disarming_reason = (uint8_t)calling_reason;
 
-			} else if (!was_armed && armed.armed) { // arm transition
+			} else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition
 				status.latest_arming_reason = (uint8_t)calling_reason;
 			}
 
-			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			// Switch state
+			_arm_state = new_arming_state;
+
+			if (isArmed()) {
 				status.armed_time = hrt_absolute_time();
 
 			} else {
@@ -205,13 +181,58 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		if (!feedback_provided) {
 			// FIXME: this catch-all does not provide helpful information to the user
 			mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t",
-					     arming_state_names[status.arming_state], arming_state_names[new_arming_state]);
+					     getArmStateName(_arm_state), getArmStateName(new_arming_state));
 			events::send<events::px4::enums::arming_state_t, events::px4::enums::arming_state_t>(
 				events::ID("commander_transition_denied"), events::Log::Critical,
 				"Arming state transition denied: {1} to {2}",
-				eventArmingState(status.arming_state), eventArmingState(new_arming_state));
+				getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state));
 		}
 	}
 
 	return ret;
 }
+
+const char *ArmStateMachine::getArmStateName(uint8_t arming_state)
+{
+	switch (arming_state) {
+
+	case vehicle_status_s::ARMING_STATE_INIT: return "Init";
+
+	case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby";
+
+	case vehicle_status_s::ARMING_STATE_ARMED: return "Armed";
+
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error";
+
+	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown";
+
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore";
+
+	default: return "Unknown";
+	}
+
+	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE,
+		      "enum def mismatch");
+}
+
+events::px4::enums::arming_state_t ArmStateMachine::getArmStateEvent(uint8_t arming_state)
+{
+	switch (arming_state) {
+	case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init;
+
+	case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby;
+
+	case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed;
+
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error;
+
+	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown;
+
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore;
+	}
+
+	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore,
+		      "enum def mismatch");
+
+	return events::px4::enums::arming_state_t::init;
+}

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -52,6 +52,8 @@ public:
 	ArmStateMachine() = default;
 	~ArmStateMachine() = default;
 
+	void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
+
 	transition_result_t
 	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
 				const arming_state_t new_arming_state,
@@ -59,19 +61,21 @@ public:
 				vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
 				const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
 
-	// You can index into the array with an arming_state_t in order to get its textual representation
-	const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
-		"INIT",
-		"STANDBY",
-		"ARMED",
-		"STANDBY_ERROR",
-		"SHUTDOWN",
-		"IN_AIR_RESTORE",
-	};
+	// Getters
+	uint8_t getArmState() const { return _arm_state; }
+
+	bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); }
+	bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); }
+	bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); }
+	bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); }
+
+	static const char *getArmStateName(uint8_t arming_state);
+	const char *getArmStateName() const { return getArmStateName(_arm_state); }
 
 private:
-	static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);
+	static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state);
 
+	uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT};
 	hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately
 
 	// This array defines the arming state transitions. The rows are the new state, and the columns

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -44,7 +44,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 	typedef struct {
 		arming_state_t  arming_state;   // vehicle_status_s.arming_state
 		bool            armed;          // actuator_armed_s.armed
-		bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
 	} ArmingTransitionVolatileState_t;
 
 	// This structure represents a test case for arming_state_transition. It contains the machine
@@ -66,8 +65,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 	// We use these defines so that our test cases are more readable
 #define ATT_ARMED true
 #define ATT_DISARMED false
-#define ATT_READY_TO_ARM true
-#define ATT_NOT_READY_TO_ARM false
 #define ATT_SENSORS_INITIALIZED true
 #define ATT_SENSORS_NOT_INITIALIZED false
 #define ATT_SAFETY_AVAILABLE true
@@ -81,9 +78,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: identical states",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED
 		},
 
 		// TRANSITION_CHANGED tests
@@ -92,97 +89,97 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"transition: init to standby",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: init to standby error",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: init to reboot",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to init",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to standby error",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to reboot",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: armed to standby",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby error to reboot",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: in air restore to armed",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: in air restore to reboot",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		// hil on tests, standby error to standby not normally allowed
 
 		{
 			"transition: standby error to standby, hil on",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
 		},
 
 		// Safety switch arming tests
 
 		{
 			"transition: standby to armed, no safety switch",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to armed, safety switch off",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
 		},
 
 		// TRANSITION_DENIED tests
@@ -191,51 +188,51 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: init to armed",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: armed to init",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: armed to reboot",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: standby error to armed",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: standby error to standby",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: reboot to armed",
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: in air restore to standby",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED
 		},
 
 		// Sensor tests
@@ -249,9 +246,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: init to armed, safety switch on",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
 			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
 		},
 	};
 
@@ -268,7 +265,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		PreFlightCheck::arm_requirements_t arm_req{};
 
 		// Setup initial machine state
-		status.arming_state = test->current_state.arming_state;
+		arm_state_machine.forceArmState(test->current_state.arming_state);
 		status_flags.system_sensors_initialized = test->system_sensors_initialized;
 		status.hil_state = test->hil_state;
 		// The power status of the test unit is not relevant for the unit test
@@ -276,9 +273,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		safety.safety_switch_available = test->safety_switch_available;
 		safety.safety_off = test->safety_off;
 
-		armed.armed = test->current_state.armed;
-		armed.ready_to_arm = test->current_state.ready_to_arm;
-
 		vehicle_control_mode_s control_mode{};
 
 		// Attempt transition
@@ -297,8 +291,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		// Validate result of transition
 		EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg;
-		EXPECT_EQ(status.arming_state, test->expected_state.arming_state) << test->assertMsg;
-		EXPECT_EQ(armed.armed, test->expected_state.armed) << test->assertMsg;
-		EXPECT_EQ(armed.ready_to_arm, test->expected_state.ready_to_arm) << test->assertMsg;
+		EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg;
+		EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg;
 	}
 }

src/modules/commander/Commander.cpp

@@ -91,6 +91,20 @@ typedef enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_ENUM_END = 129, /*  | */
 } VEHICLE_MODE_FLAG;
 
+// TODO: generate
+static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b)
+{
+	return (a.armed == b.armed &&
+		a.prearmed == b.prearmed &&
+		a.ready_to_arm == b.ready_to_arm &&
+		a.lockdown == b.lockdown &&
+		a.manual_lockdown == b.manual_lockdown &&
+		a.force_failsafe == b.force_failsafe &&
+		a.in_esc_calibration_mode == b.in_esc_calibration_mode &&
+		a.soft_stop == b.soft_stop);
+}
+static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
+
 #if defined(BOARD_HAS_POWER_CONTROL)
 static orb_advert_t tune_control_pub = nullptr;
 static void play_power_button_down_tune()
@@ -465,7 +479,7 @@ int Commander::custom_command(int argc, char *argv[])
 
 int Commander::print_status()
 {
-	PX4_INFO("arming: %s", _arm_state_machine.arming_state_names[_status.arming_state]);
+	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
 	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
 	perf_print_counter(_loop_perf);
 	perf_print_counter(_preflight_check_perf);
@@ -688,7 +702,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		run_preflight_checks = false;
 	}
 
-	if (run_preflight_checks && !_armed.armed) {
+	if (run_preflight_checks && !_arm_state_machine.isArmed()) {
 		if (_vehicle_control_mode.flag_control_manual_enabled) {
 			if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
 			    !_status.rc_signal_lost && _is_throttle_above_center) {
@@ -815,7 +829,6 @@ Commander::Commander() :
 	// We want to accept RC inputs as default
 	_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	_status.nav_state_timestamp = hrt_absolute_time();
-	_status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
 
 	/* mark all signals lost as long as they haven't been found */
 	_status.rc_signal_lost = true;
@@ -827,7 +840,7 @@ Commander::Commander() :
 	_status_flags.rc_calibration_valid = true;
 
 	// default for vtol is rotary wing
-	_vtol_status.vtol_in_rw_mode = true;
+	_vtol_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
 	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
 	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
@@ -1013,12 +1026,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
 				const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
 
-				if (!forced) {
-					// Flick to in-air restore first if this comes from an onboard system and from IO
-					if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
-					    && cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
-						_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
-					}
+				// Flick to in-air restore first if this comes from an onboard system and from IO
+				if (!forced && cmd_from_io
+				    && (cmd.source_system == _status.system_id)
+				    && (cmd.source_component == _status.component_id)
+				    && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
+					// TODO: replace with a proper allowed transition
+					_arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE);
 				}
 
 				transition_result_t arming_res = TRANSITION_DENIED;
@@ -1372,8 +1386,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1492,9 +1505,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
 			// Magnetometer quick calibration using world magnetic model and known heading
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-			    || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
-			    || _worker_thread.isBusy()) {
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1529,9 +1540,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN
-			    || _worker_thread.isBusy()) {
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1600,6 +1609,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
+	case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -1621,7 +1631,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 unsigned
 Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 {
-	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
+	if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
 		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
@@ -1669,7 +1679,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 unsigned
 Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
 {
-	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
+	if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
 		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
@@ -1749,7 +1759,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
 
 	case action_request_s::ACTION_TOGGLE_ARMING:
-		if (_armed.armed) {
+		if (_arm_state_machine.isArmed()) {
 			disarm(arm_disarm_reason);
 
 		} else {
@@ -1794,7 +1804,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 		// if there's never been a mode change force RC switch as initial state
 		if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT
-		    && !_armed.armed && (_internal_state.main_state_changes == 0)
+		    && !_arm_state_machine.isArmed() && (_internal_state.main_state_changes == 0)
 		    && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL
 			|| action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) {
 			_internal_state.main_state = action_request.mode;
@@ -2083,8 +2093,10 @@ void Commander::updateParameters()
 	_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
 	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
 
-	const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
-	const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
+	const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status)
+			       && _vtol_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
+	const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status)
+			      && _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	const bool is_ground = is_ground_rover(_status);
 
 	/* disable manual override for all systems that rely on electronic stabilization */
@@ -2124,8 +2136,6 @@ void Commander::updateParameters()
 void
 Commander::run()
 {
-	bool sensor_fail_tune_played = false;
-
 	/* initialize */
 	led_init();
 	buzzer_init();
@@ -2169,6 +2179,9 @@ Commander::run()
 
 		perf_begin(_loop_perf);
 
+		const actuator_armed_s actuator_armed_prev{_armed};
+		const vehicle_status_flags_s vehicle_status_flags_prev{_status_flags};
+
 		/* update parameters */
 		const bool params_updated = _parameter_update_sub.updated();
 
@@ -2178,7 +2191,7 @@ Commander::run()
 			_parameter_update_sub.copy(&update);
 
 			/* update parameters */
-			if (!_armed.armed) {
+			if (!_arm_state_machine.isArmed()) {
 				updateParameters();
 
 				_status_changed = true;
@@ -2232,7 +2245,7 @@ Commander::run()
 			_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
 
 			// Only take actions if armed
-			if (_armed.armed) {
+			if (_arm_state_machine.isArmed()) {
 				if (!was_landed && _vehicle_land_detected.landed) {
 					mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
 					events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
@@ -2276,7 +2289,7 @@ Commander::run()
 						 _safety.safety_switch_available, _status);
 
 				// disarm if safety is now on and still armed
-				if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off
+				if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off
 				    && (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
 					disarm(arm_disarm_reason_t::safety_button);
 				}
@@ -2300,30 +2313,33 @@ Commander::run()
 		if (_vtol_vehicle_status_sub.updated()) {
 			/* vtol status changed */
 			_vtol_vehicle_status_sub.copy(&_vtol_status);
-			_status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab;
 
 			/* Make sure that this is only adjusted if vehicle really is of type vtol */
 			if (is_vtol(_status)) {
 
-				// Check if there has been any change while updating the flags
-				const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ?
-							      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
-							      vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+				// Check if there has been any change while updating the flags (transition = rotary wing status)
+				const auto new_vehicle_type = _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ?
+							      vehicle_status_s::VEHICLE_TYPE_FIXED_WING :
+							      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 
 				if (new_vehicle_type != _status.vehicle_type) {
-					_status.vehicle_type = _vtol_status.vtol_in_rw_mode ?
-							       vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
-							       vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+					_status.vehicle_type = new_vehicle_type;
 					_status_changed = true;
 				}
 
-				if (_status.in_transition_mode != _vtol_status.vtol_in_trans_mode) {
-					_status.in_transition_mode = _vtol_status.vtol_in_trans_mode;
+				const bool new_in_transition = _vtol_status.vehicle_vtol_state ==
+							       vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW
+							       || _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
+
+				if (_status.in_transition_mode != new_in_transition) {
+					_status.in_transition_mode = new_in_transition;
 					_status_changed = true;
 				}
 
-				if (_status.in_transition_to_fw != _vtol_status.in_transition_to_fw) {
-					_status.in_transition_to_fw = _vtol_status.in_transition_to_fw;
+				if (_status.in_transition_to_fw != (_vtol_status.vehicle_vtol_state ==
+								    vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW)) {
+					_status.in_transition_to_fw = (_vtol_status.vehicle_vtol_state ==
+								       vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW);
 					_status_changed = true;
 				}
 
@@ -2372,7 +2388,7 @@ Commander::run()
 		estimator_check();
 
 		// Auto disarm when landed or kill switch engaged
-		if (_armed.armed) {
+		if (_arm_state_machine.isArmed()) {
 
 			// Check for auto-disarm on landing or pre-flight
 			if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
@@ -2435,7 +2451,7 @@ Commander::run()
 		}
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
 
 			_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 					vehicle_status_s::ARMING_STATE_STANDBY, _armed,
@@ -2490,7 +2506,7 @@ Commander::run()
 			}
 
 			// Transition main state to loiter or auto-mission after takeoff is completed.
-			if (_armed.armed && !_vehicle_land_detected.landed
+			if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
 			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
 				_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 			    && (mission_result.timestamp >= _status.nav_state_timestamp)
@@ -2514,7 +2530,7 @@ Commander::run()
 		const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
 
 		// Geofence actions
-		if (_armed.armed
+		if (_arm_state_machine.isArmed()
 		    && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
 		    && !in_low_battery_failsafe_delay) {
 
@@ -2614,7 +2630,7 @@ Commander::run()
 		}
 
 		/* Check for mission flight termination */
-		if (_armed.armed && _mission_result_sub.get().flight_termination &&
+		if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination &&
 		    !_status_flags.circuit_breaker_flight_termination_disabled) {
 
 
@@ -2664,7 +2680,8 @@ Commander::run()
 
 				const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
 
-				if (!_armed.armed && (is_mavlink || !_mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
+				if (!_arm_state_machine.isArmed() && (is_mavlink || !_mode_switch_mapped)
+				    && (_internal_state.main_state_changes == 0)) {
 					// if there's never been a mode change force position control as initial state
 					_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
 					_internal_state.main_state_changes++;
@@ -2698,7 +2715,7 @@ Commander::run()
 			// but only if actually in air.
 			if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
 			    && !in_low_battery_failsafe_delay && !_geofence_warning_action_on
-			    && _armed.armed
+			    && _arm_state_machine.isArmed()
 			    && !_status_flags.rc_calibration_in_progress
 			    && manual_control_setpoint.valid
 			    && manual_control_setpoint.sticks_moving
@@ -2735,7 +2752,7 @@ Commander::run()
 		avoidance_check();
 
 		/* check if we are disarmed and there is a better mode to wait in */
-		if (!_armed.armed) {
+		if (!_arm_state_machine.isArmed()) {
 			/* if there is no radio control but GPS lock the user might want to fly using
 			 * just a tablet. Since the RC will force its mode switch setting on connecting
 			 * we can as well just wait in a hold mode which enables tablet control.
@@ -2785,7 +2802,7 @@ Commander::run()
 			auto fd_status_flags = _failure_detector.getStatusFlags();
 			_status_changed = true;
 
-			if (_armed.armed) {
+			if (_arm_state_machine.isArmed()) {
 				if (fd_status_flags.arm_escs) {
 					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
 					if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
@@ -2912,7 +2929,7 @@ Commander::run()
 			checkWindSpeedThresholds();
 		}
 
-		_status_flags.flight_terminated = _armed.force_failsafe || _armed.lockdown || _armed.manual_lockdown;
+		_status_flags.flight_terminated = _armed.force_failsafe || _armed.manual_lockdown;
 
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
@@ -2937,7 +2954,7 @@ Commander::run()
 
 		// automatically set or update home position
 		if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
-			if (!_armed.armed && _vehicle_land_detected.landed) {
+			if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) {
 				const bool can_set_home_lpos_first_time = (!_home_pub.get().valid_lpos && _status_flags.local_position_valid);
 				const bool can_set_home_gpos_first_time = ((!_home_pub.get().valid_hpos || !_home_pub.get().valid_alt)
 						&& (_status_flags.global_position_valid || _status_flags.gps_position_valid));
@@ -2953,10 +2970,10 @@ Commander::run()
 		}
 
 		// check for arming state change
-		if (_was_armed != _armed.armed) {
+		if (_was_armed != _arm_state_machine.isArmed()) {
 			_status_changed = true;
 
-			if (_armed.armed) {
+			if (_arm_state_machine.isArmed()) {
 				if (!_vehicle_land_detected.landed) { // check if takeoff already detected upon arming
 					_have_taken_off_since_arming = true;
 				}
@@ -2970,7 +2987,7 @@ Commander::run()
 			}
 		}
 
-		if (!_armed.armed) {
+		if (!_arm_state_machine.isArmed()) {
 			/* Reset the flag if disarmed. */
 			_have_taken_off_since_arming = false;
 			_imbalanced_propeller_check_triggered = false;
@@ -3013,54 +3030,49 @@ Commander::run()
 			_failsafe_old = _status.failsafe;
 		}
 
-		/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed */
-		if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) {
 
-			update_control_mode();
+		// prearm mode
+		switch ((PrearmedMode)_param_com_prearm_mode.get()) {
+		case PrearmedMode::DISABLED:
+			/* skip prearmed state  */
+			_armed.prearmed = false;
+			break;
 
-			_status.timestamp = hrt_absolute_time();
-			_status_pub.publish(_status);
+		case PrearmedMode::ALWAYS:
+			/* safety is not present, go into prearmed
+			* (all output drivers should be started / unlocked last in the boot process
+			* when the rest of the system is fully initialized)
+			*/
+			_armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s);
+			break;
 
-			switch ((PrearmedMode)_param_com_prearm_mode.get()) {
-			case PrearmedMode::DISABLED:
-				/* skip prearmed state  */
+		case PrearmedMode::SAFETY_BUTTON:
+			if (_safety.safety_switch_available) {
+				/* safety switch is present, go into prearmed if safety is off */
+				_armed.prearmed = _safety.safety_off;
+
+			} else {
+				/* safety switch is not present, do not go into prearmed */
 				_armed.prearmed = false;
-				break;
-
-			case PrearmedMode::ALWAYS:
-				/* safety is not present, go into prearmed
-				* (all output drivers should be started / unlocked last in the boot process
-				* when the rest of the system is fully initialized)
-				*/
-				_armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s);
-				break;
-
-			case PrearmedMode::SAFETY_BUTTON:
-				if (_safety.safety_switch_available) {
-					/* safety switch is present, go into prearmed if safety is off */
-					_armed.prearmed = _safety.safety_off;
-
-				} else {
-					/* safety switch is not present, do not go into prearmed */
-					_armed.prearmed = false;
-				}
-
-				break;
-
-			default:
-				_armed.prearmed = false;
-				break;
 			}
 
-			_armed.timestamp = hrt_absolute_time();
-			_armed_pub.publish(_armed);
+			break;
 
-			/* publish internal state for logging purposes */
-			_internal_state.timestamp = hrt_absolute_time();
-			_commander_state_pub.publish(_internal_state);
+		default:
+			_armed.prearmed = false;
+			break;
+		}
+
+
+		// publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed
+		if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed
+		    || !(_armed == actuator_armed_prev)) {
+
+			// Evaluate current prearm status (skip during arm -> disarm transition)
+			if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) {
+
+				_status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
 
-			// Evaluate current prearm status
-			if (!_armed.armed && !_status_flags.calibration_enabled) {
 				perf_begin(_preflight_check_perf);
 				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode,
 							   false, true, hrt_elapsed_time(&_boot_timestamp));
@@ -3072,17 +3084,34 @@ Commander::run()
 				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req,
 							_status, false);
 
-				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
-						 && prearm_check_res), _status);
+				const bool prearm_check_ok = preflight_check_res && prearm_check_res;
+				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _status);
 			}
 
-			/* publish vehicle_status_flags */
+			// publish actuator_armed first (used by output modules)
+			_armed.armed = _arm_state_machine.isArmed();
+			_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
+			_armed.timestamp = hrt_absolute_time();
+			_armed_pub.publish(_armed);
+
+			// update and publish vehicle_control_mode
+			update_control_mode();
+
+			// vehicle_status publish (after prearm/preflight updates above)
+			_status.arming_state = _arm_state_machine.getArmState();
+			_status.timestamp = hrt_absolute_time();
+			_status_pub.publish(_status);
+
+			// publish vehicle_status_flags (after prearm/preflight updates above)
 			_status_flags.timestamp = hrt_absolute_time();
 			_vehicle_status_flags_pub.publish(_status_flags);
 
-			/* publish failure_detector data */
+			// commander_state publish internal state for logging purposes
+			_internal_state.timestamp = hrt_absolute_time();
+			_commander_state_pub.publish(_internal_state);
+
+			// failure_detector_status publish
 			failure_detector_status_s fd_status{};
-			fd_status.timestamp = hrt_absolute_time();
 			fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
 			fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch;
 			fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
@@ -3093,11 +3122,12 @@ Commander::run()
 			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
 			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
+			fd_status.timestamp = hrt_absolute_time();
 			_failure_detector_status_pub.publish(fd_status);
 		}
 
 		/* play arming and battery warning tunes */
-		if (!_arm_tune_played && _armed.armed &&
+		if (!_arm_tune_played && _arm_state_machine.isArmed() &&
 		    (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
 
 			/* play tune when armed */
@@ -3115,7 +3145,7 @@ Commander::run()
 			/* play tune on battery warning */
 			set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
 
-		} else if (_status.failsafe && _armed.armed) {
+		} else if (_status.failsafe && _arm_state_machine.isArmed()) {
 			tune_failsafe(true);
 
 		} else {
@@ -3123,7 +3153,7 @@ Commander::run()
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (!_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
+		if (!_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
 
 			// Notify the user that it is safe to approach the vehicle
 			if (_arm_tune_played) {
@@ -3134,14 +3164,10 @@ Commander::run()
 		}
 
 		/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
-		_status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
-
-		if (!sensor_fail_tune_played && (!_status_flags.system_sensors_initialized
-						 && _status_flags.system_hotplug_timeout)) {
+		if (!_status_flags.system_sensors_initialized &&
+		    !vehicle_status_flags_prev.system_hotplug_timeout && _status_flags.system_hotplug_timeout) {
 
 			set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING);
-			sensor_fail_tune_played = true;
-			_status_changed = true;
 		}
 
 		// check if the worker has finished
@@ -3170,11 +3196,11 @@ Commander::run()
 		_last_local_position_valid = _status_flags.local_position_valid;
 		_last_global_position_valid = _status_flags.global_position_valid;
 
-		_was_armed = _armed.armed;
+		_was_armed = _arm_state_machine.isArmed();
 
 		arm_auth_update(now, params_updated);
 
-		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
+		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
 
 		perf_end(_loop_perf);
 
@@ -3250,14 +3276,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 		uint8_t led_color = led_control_s::COLOR_WHITE;
 		bool set_normal_color = false;
 
-		uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
+		uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms;
 
 		// set mode
 		if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_PURPLE;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		} else if (_arm_state_machine.isArmed()) {
 			led_mode = led_control_s::MODE_ON;
 			set_normal_color = true;
 
@@ -3265,7 +3291,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_RED;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+		} else if (_arm_state_machine.isStandby()) {
 			led_mode = led_control_s::MODE_BREATHE;
 			set_normal_color = true;
 
@@ -3273,7 +3299,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BREATHE;
 			set_normal_color = true;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		} else if (_arm_state_machine.isInit()) {
 			// if in init status it should not be in the error state
 			led_mode = led_control_s::MODE_OFF;
 
@@ -3313,7 +3339,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 
 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
 
-	if (_armed.armed) {
+	if (_arm_state_machine.isArmed()) {
 		if (_status.failsafe) {
 			BOARD_ARMED_LED_OFF();
 
@@ -3329,7 +3355,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			BOARD_ARMED_LED_ON();
 		}
 
-	} else if (_armed.ready_to_arm) {
+	} else if (_arm_state_machine.isStandby()) {
 		BOARD_ARMED_LED_OFF();
 
 		// ready to arm, blink at 1Hz
@@ -3404,7 +3430,7 @@ Commander::update_control_mode()
 	_vehicle_control_mode = {};
 
 	/* set vehicle_control_mode according to set_navigation_state */
-	_vehicle_control_mode.flag_armed = _armed.armed;
+	_vehicle_control_mode.flag_armed = _arm_state_machine.isArmed();
 
 	switch (_status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
@@ -3536,11 +3562,7 @@ Commander::update_control_mode()
 bool
 Commander::stabilization_required()
 {
-	return (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ||		// is a rotary wing, or
-		_status.vtol_fw_permanent_stab || 	// is a VTOL in fixed wing mode and stabilisation is on, or
-		(_vtol_status.vtol_in_trans_mode && 	// is currently a VTOL transitioning AND
-		 _status.vehicle_type ==
-		 vehicle_status_s::VEHICLE_TYPE_FIXED_WING));	// is a fixed wing, ie: transitioning back to rotary wing mode
+	return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 }
 
 void
@@ -3557,7 +3579,7 @@ Commander::print_reject_mode(uint8_t main_state)
 
 		/* only buzz if armed, because else we're driving people nuts indoors
 		they really need to look at the leds as well. */
-		tune_negative(_armed.armed);
+		tune_negative(_arm_state_machine.isArmed());
 
 		_last_print_mode_reject_time = hrt_absolute_time();
 	}
@@ -3689,7 +3711,7 @@ void Commander::data_link_check()
 						events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
 					}
 
-					if (!_armed.armed && !_status_flags.calibration_enabled) {
+					if (!_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) {
 						// make sure to report preflight check failures to a connecting GCS
 						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
 									       true, false, hrt_elapsed_time(&_boot_timestamp));
@@ -3863,7 +3885,7 @@ void Commander::battery_status_check()
 			battery_required_count++;
 		}
 
-		if (_armed.armed) {
+		if (_arm_state_machine.isArmed()) {
 
 			if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
 				mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
@@ -3909,9 +3931,11 @@ void Commander::battery_status_check()
 					     fault_index++) {
 						if (battery.faults & (1 << fault_index)) {
 							mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1,
-									      battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)),  _armed.armed ? "Land now!" : "");
+									      battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)),
+									      _arm_state_machine.isArmed() ? "Land now!" : "");
 
-							events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
+							events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ?
+									events::px4::enums::suggested_action_t::land :
 									events::px4::enums::suggested_action_t::none;
 
 							/* EVENT
@@ -3945,7 +3969,7 @@ void Commander::battery_status_check()
 	if (_rtl_time_estimate_sub.copy(&rtl_time_estimate)
 	    && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s
 	    && rtl_time_estimate.valid
-	    && _armed.armed
+	    && _arm_state_machine.isArmed()
 	    && !_vehicle_land_detected.ground_contact // not in any landing stage
 	    && !_rtl_time_actions_done
 	    && PX4_ISFINITE(worst_battery_time_s)
@@ -3971,7 +3995,7 @@ void Commander::battery_status_check()
 	bool battery_warning_level_increased_while_armed = false;
 	bool update_internal_battery_state = false;
 
-	if (_armed.armed) {
+	if (_arm_state_machine.isArmed()) {
 		if (worst_warning > _battery_warning) {
 			battery_warning_level_increased_while_armed = true;
 			update_internal_battery_state = true;
@@ -4144,7 +4168,7 @@ void Commander::estimator_check()
 
 			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+				if (!_arm_state_machine.isArmed()) {
 					_nav_test_failed = false;
 					_nav_test_passed = false;
 
@@ -4208,7 +4232,7 @@ void Commander::estimator_check()
 		bool xy_valid = lpos.xy_valid && !_nav_test_failed;
 		bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
 
-		if (!_armed.armed) {
+		if (!_arm_state_machine.isArmed()) {
 			if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
 				xy_valid = false;
 			}
@@ -4399,7 +4423,8 @@ void Commander::esc_status_check()
 				if ((esc_status.esc_online_flags & (1 << index)) == 0) {
 					snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
 					esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
-					events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
+					events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ?
+							events::px4::enums::suggested_action_t::land :
 							events::px4::enums::suggested_action_t::none;
 					// TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default
 					events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
@@ -4407,7 +4432,8 @@ void Commander::esc_status_check()
 				}
 			}
 
-			mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
+			mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg,
+					     _arm_state_machine.isArmed() ? "Land now!" : "");
 
 			_last_esc_online_flags = esc_status.esc_online_flags;
 			_status_flags.escs_error = true;
@@ -4441,7 +4467,7 @@ void Commander::esc_status_check()
 						}
 
 						mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1,
-								      esc_fault_reason_str(fault_reason_index),  _armed.armed ? user_action : "");
+								      esc_fault_reason_str(fault_reason_index), _arm_state_machine.isArmed() ? user_action : "");
 
 						events::send<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>
 						(events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning},

src/modules/commander/commander_params.c

@@ -296,7 +296,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @max 25.0
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 10.0f);
+PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f);
 
 /**
  * Imbalanced propeller failsafe mode

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -43,11 +43,11 @@
 
 #pragma once
 
+#include <lib/hysteresis/hysteresis.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <matrix/matrix/math.hpp>
-#include <mathlib/mathlib.h>
 #include <px4_platform_common/module_params.h>
-#include <hysteresis/hysteresis.h>
 
 // subscriptions
 #include <uORB/Subscription.hpp>

src/modules/commander/state_machine_helper.h

@@ -100,7 +100,6 @@ enum class position_nav_loss_actions_t {
 	LAND_TERMINATE = 1,	// Land/Terminate.  Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
 };
 
-extern const char *const arming_state_names[];
 extern const char *const nav_state_names[];
 
 enum RCLossExceptionBits {

src/modules/ekf2/EKF/common.h

@@ -56,26 +56,81 @@ using matrix::Vector2f;
 using matrix::Vector3f;
 using matrix::wrap_pi;
 
-enum class velocity_frame_t : uint8_t {
-	LOCAL_FRAME_FRD,
-	BODY_FRAME_FRD
+// maximum sensor intervals in usec
+#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
+#define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
+#define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
+
+// bad accelerometer detection and mitigation
+#define BADACC_PROBATION  (uint64_t)10e6        ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
+#define BADACC_BIAS_PNOISE      4.9f    ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
+
+// ground effect compensation
+#define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+
+enum class VelocityFrame : uint8_t {
+	LOCAL_FRAME_FRD = 0,
+	BODY_FRAME_FRD  = 1
 };
 
-struct gps_message {
+enum GeoDeclinationMask : uint8_t {
+	// Bit locations for mag_declination_source
+	USE_GEO_DECL  = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
+	SAVE_GEO_DECL = (1<<1), ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
+	FUSE_DECL     = (1<<2)  ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
+};
+
+enum MagFuseType : uint8_t {
+	// Integer definitions for mag_fusion_type
+	AUTO    = 0,   	///< The selection of either heading or 3D magnetometer fusion will be automatic
+	HEADING = 1,   	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
+	MAG_3D  = 2,   	///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
+	INDOOR  = 3,   	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
+	NONE    = 4    	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
+};
+
+enum TerrainFusionMask : uint8_t {
+	TerrainFuseRangeFinder = (1 << 0),
+	TerrainFuseOpticalFlow = (1 << 1)
+};
+
+enum VerticalHeightSensor : uint8_t {
+	// Integer definitions for vdist_sensor_type
+	BARO  = 0,   	///< Use baro height
+	GPS   = 1,   	///< Use GPS height
+	RANGE = 2,   	///< Use range finder height
+	EV    = 3    	///< Use external vision
+};
+
+enum SensorFusionMask : uint16_t {
+	// Bit locations for fusion_mode
+	USE_GPS          = (1<<0),      ///< set to true to use GPS data
+	USE_OPT_FLOW     = (1<<1),      ///< set to true to use optical flow data
+	INHIBIT_ACC_BIAS = (1<<2),      ///< set to true to inhibit estimation of accelerometer delta velocity bias
+	USE_EXT_VIS_POS  = (1<<3),      ///< set to true to use external vision position data
+	USE_EXT_VIS_YAW  = (1<<4),      ///< set to true to use external vision quaternion data for yaw
+	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
+	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
+	USE_GPS_YAW      = (1<<7),      ///< set to true to use GPS yaw data if available
+	USE_EXT_VIS_VEL  = (1<<8)       ///< set to true to use external vision velocity data
+};
+
+struct gpsMessage {
 	uint64_t    time_usec{};
-	int32_t     lat{};                 ///< Latitude in 1E-7 degrees
-	int32_t     lon{};                 ///< Longitude in 1E-7 degrees
-	int32_t     alt{};                 ///< Altitude in 1E-3 meters (millimeters) above MSL
-	float       yaw{};                 ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
-	float       yaw_offset{};          ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
-	uint8_t     fix_type{};            ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
-	float       eph{};                 ///< GPS horizontal position accuracy in m
-	float       epv{};                 ///< GPS vertical position accuracy in m
-	float       sacc{};                ///< GPS speed accuracy in m/s
-	float       vel_m_s{};             ///< GPS ground speed (m/sec)
-	Vector3f    vel_ned{};             ///< GPS ground speed NED
-	bool        vel_ned_valid{};       ///< GPS ground speed is valid
-	uint8_t     nsats{};               ///< number of satellites used
+	int32_t     lat{};              ///< Latitude in 1E-7 degrees
+	int32_t     lon{};              ///< Longitude in 1E-7 degrees
+	int32_t     alt{};              ///< Altitude in 1E-3 meters (millimeters) above MSL
+	float       yaw{};              ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
+	float       yaw_offset{};       ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
+	uint8_t     fix_type{};         ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
+	float       eph{};              ///< GPS horizontal position accuracy in m
+	float       epv{};              ///< GPS vertical position accuracy in m
+	float       sacc{};             ///< GPS speed accuracy in m/s
+	float       vel_m_s{};          ///< GPS ground speed (m/sec)
+	Vector3f    vel_ned{};          ///< GPS ground speed NED
+	bool        vel_ned_valid{};    ///< GPS ground speed is valid
+	uint8_t     nsats{};            ///< number of satellites used
 	float       pdop{};             ///< position dilution of precision
 };
 
@@ -151,7 +206,7 @@ struct extVisionSample {
 	Vector3f    posVar{};      ///< XYZ position variances (m**2)
 	Matrix3f    velCov{};      ///< XYZ velocity covariances ((m/sec)**2)
 	float       angVar{};      ///< angular heading variance (rad**2)
-	velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
 };
 
@@ -177,61 +232,13 @@ struct stateSample {
 	Vector2f wind_vel{};            ///< horizontal wind velocity in earth frame in m/s
 };
 
-// Integer definitions for vdist_sensor_type
-#define VDIST_SENSOR_BARO  0    ///< Use baro height
-#define VDIST_SENSOR_GPS   1    ///< Use GPS height
-#define VDIST_SENSOR_RANGE 2    ///< Use range finder height
-#define VDIST_SENSOR_EV    3    ///< Use external vision
-
-// Bit locations for mag_declination_source
-#define MASK_USE_GEO_DECL     (1<<0)    ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
-#define MASK_SAVE_GEO_DECL    (1<<1)    ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
-#define MASK_FUSE_DECL        (1<<2)    ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
-
-// Bit locations for fusion_mode
-#define MASK_USE_GPS          (1<<0)    ///< set to true to use GPS data
-#define MASK_USE_OF           (1<<1)    ///< set to true to use optical flow data
-#define MASK_INHIBIT_ACC_BIAS (1<<2)    ///< set to true to inhibit estimation of accelerometer delta velocity bias
-#define MASK_USE_EVPOS        (1<<3)    ///< set to true to use external vision position data
-#define MASK_USE_EVYAW        (1<<4)    ///< set to true to use external vision quaternion data for yaw
-#define MASK_USE_DRAG         (1<<5)    ///< set to true to use the multi-rotor drag model to estimate wind
-#define MASK_ROTATE_EV        (1<<6)    ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
-#define MASK_USE_GPSYAW       (1<<7)    ///< set to true to use GPS yaw data if available
-#define MASK_USE_EVVEL        (1<<8)    ///< set to true to use external vision velocity data
-
-enum TerrainFusionMask : int32_t {
-	TerrainFuseRangeFinder = (1 << 0),
-	TerrainFuseOpticalFlow = (1 << 1)
-};
-
-// Integer definitions for mag_fusion_type
-#define MAG_FUSE_TYPE_AUTO    0         ///< The selection of either heading or 3D magnetometer fusion will be automatic
-#define MAG_FUSE_TYPE_HEADING 1         ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
-#define MAG_FUSE_TYPE_3D      2         ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
-#define MAG_FUSE_TYPE_UNUSED  3         ///< Not implemented
-#define MAG_FUSE_TYPE_INDOOR  4         ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
-#define MAG_FUSE_TYPE_NONE    5         ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
-
-// Maximum sensor intervals in usec
-#define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
-#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
-
-// bad accelerometer detection and mitigation
-#define BADACC_PROBATION  (uint64_t)10e6        ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
-#define BADACC_BIAS_PNOISE      4.9f    ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
-
-// ground effect compensation
-#define GNDEFFECT_TIMEOUT       10E6    ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
-
 struct parameters {
 
 	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
 
 	// measurement source control
-	int32_t fusion_mode{MASK_USE_GPS};              ///< bitmasked integer that selects which aiding sources will be used
-	int32_t vdist_sensor_type{VDIST_SENSOR_BARO};   ///< selects the primary source for height data
+	int32_t fusion_mode{SensorFusionMask::USE_GPS};         ///< bitmasked integer that selects which aiding sources will be used
+	int32_t vdist_sensor_type{VerticalHeightSensor::BARO};  ///< selects the primary source for height data
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -501,7 +508,7 @@ union filter_control_status_u {
 };
 
 // Mavlink bitmask containing state of estimator solution
-union ekf_solution_status {
+union ekf_solution_status_u {
 	struct {
 		uint16_t attitude           : 1; ///< 0 - True if the attitude estimate is good
 		uint16_t velocity_horiz     : 1; ///< 1 - True if the horizontal velocity estimate is good

src/modules/ekf2/EKF/control.cpp

@@ -215,8 +215,8 @@ void Ekf::controlExternalVisionFusion()
 
 		// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
 		// needs to be calculated and the observations rotated into the EKF frame of reference
-		if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS)
-				|| (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) {
+		if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
+				|| (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) {
 
 			// rotate EV measurements into the EKF Navigation frame
 			calcExtVisRotMat();
@@ -228,19 +228,19 @@ void Ekf::controlExternalVisionFusion()
 			// check for a external vision measurement that has fallen behind the fusion time horizon
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				// turn on use of external vision measurements for position
-				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
+				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
 					startEvPosFusion();
 				}
 
 				// turn on use of external vision measurements for velocity
-				if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
+				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
 					startEvVelFusion();
 				}
 			}
 		}
 
 		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
+		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
 		    && _control_status.flags.tilt_align) {
 
 			// don't start using EV data unless data is arriving frequently
@@ -268,7 +268,7 @@ void Ekf::controlExternalVisionFusion()
 			_ev_sample_delayed.pos -= pos_offset_earth;
 
 			// Use an incremental position fusion method for EV position data if GPS is also used
-			if (_params.fusion_mode & MASK_USE_GPS) {
+			if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
 				_fuse_hpos_as_odom = true;
 
 			} else {
@@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion()
 				Vector3f ev_pos_meas = _ev_sample_delayed.pos;
 				Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
 
-				if (_params.fusion_mode & MASK_ROTATE_EV) {
+				if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
 					ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
 					ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
 				}
@@ -471,7 +471,7 @@ void Ekf::controlOpticalFlowFusion()
 
 		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
-			if (!(_params.fusion_mode & MASK_USE_OF)
+			if (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
 			    || _inhibit_flow_use) {
 
 				stopFlowFusion();
@@ -480,11 +480,11 @@ void Ekf::controlOpticalFlowFusion()
 		}
 
 		// optical flow fusion mode selection logic
-		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
+		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
 		    && !_control_status.flags.opt_flow // we are not yet using flow data
 		    && !_inhibit_flow_use) {
 			// If the heading is valid and use is not inhibited , start using optical flow aiding
-			if (_control_status.flags.yaw_align || _params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
+			if (_control_status.flags.yaw_align || _params.mag_fusion_type == MagFuseType::NONE) {
 				// set the flag and reset the fusion timeout
 				ECL_INFO("starting optical flow fusion");
 				_control_status.flags.opt_flow = true;
@@ -555,7 +555,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
 
 void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 {
-	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
+	if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_YAW)
 	    || _control_status.flags.gps_yaw_fault) {
 
 		stopGpsYawFusion();
@@ -842,7 +842,7 @@ void Ekf::controlHeightFusion()
 		ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
 
 	// FALLTHROUGH
-	case VDIST_SENSOR_BARO:
+	case VerticalHeightSensor::BARO:
 		if (do_range_aid) {
 			if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
 				startRngAidHgtFusion();
@@ -862,7 +862,7 @@ void Ekf::controlHeightFusion()
 
 		break;
 
-	case VDIST_SENSOR_RANGE:
+	case VerticalHeightSensor::RANGE:
 
 		// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
 		// and are on the ground, then synthesise a measurement at the expected on ground value
@@ -883,7 +883,7 @@ void Ekf::controlHeightFusion()
 
 		break;
 
-	case VDIST_SENSOR_GPS:
+	case VerticalHeightSensor::GPS:
 
 		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
 		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
@@ -909,7 +909,7 @@ void Ekf::controlHeightFusion()
 
 		break;
 
-	case VDIST_SENSOR_EV:
+	case VerticalHeightSensor::EV:
 
 		// don't start using EV data unless data is arriving frequently
 		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
@@ -986,7 +986,7 @@ void Ekf::controlAirDataFusion()
 	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
 
-	if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
+	if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
 		_control_status.flags.wind = false;
 	}
 
@@ -1053,7 +1053,7 @@ void Ekf::controlBetaFusion()
 
 void Ekf::controlDragFusion()
 {
-	if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer &&
+	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
 	    !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
 
 		if (!_control_status.flags.wind) {

src/modules/ekf2/EKF/covariance.cpp

@@ -142,7 +142,7 @@ void Ekf::predictCovariance()
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
 
-	const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)
+	const bool do_inhibit_all_axes = (_params.fusion_mode & SensorFusionMask::INHIBIT_ACC_BIAS)
 					 || is_manoeuvre_level_high
 					 || _fault_status.flags.bad_acc_vertical;
 

src/modules/ekf2/EKF/ekf.cpp

@@ -174,7 +174,7 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
 		if (_mag_counter < _obs_buffer_length) {
 			// not enough mag samples accumulated
 			return false;
@@ -202,7 +202,7 @@ bool Ekf::initialiseFilter()
 	initialiseCovariance();
 
 	// update the yaw angle variance using the variance of the measurement
-	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
 		// using magnetic heading tuning parameter
 		increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
 	}

src/modules/ekf2/EKF/ekf.h

@@ -160,7 +160,7 @@ public:
 	matrix::SquareMatrix<float, 3> position_covariances() const { return P.slice<3, 3>(7, 7); }
 
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
-	bool collect_gps(const gps_message &gps) override;
+	bool collect_gps(const gpsMessage &gps) override;
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
@@ -814,7 +814,7 @@ private:
 	Vector3f calcEarthRateNED(float lat_rad) const;
 
 	// return true id the GPS quality is good enough to set an origin and start aiding
-	bool gps_is_good(const gps_message &gps);
+	bool gps_is_good(const gpsMessage &gps);
 
 	// Control the filter fusion modes
 	void controlFusionModes();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -158,7 +158,7 @@ void Ekf::resetHorizontalPositionToVision()
 	ECL_INFO("reset position to ev position");
 	Vector3f _ev_pos = _ev_sample_delayed.pos;
 
-	if (_params.fusion_mode & MASK_ROTATE_EV) {
+	if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
 		_ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos;
 	}
 
@@ -467,7 +467,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
 
 	const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos;
 
-	if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) {
+	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
 
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
 		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
@@ -480,7 +480,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
 			yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
 		}
 
-	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) {
+	} else if (_params.mag_fusion_type == MagFuseType::INDOOR) {
 		// we are operating temporarily without knowing the earth frame yaw angle
 		return true;
 
@@ -522,7 +522,7 @@ float Ekf::getMagDeclination()
 		// Use value consistent with earth field state
 		return atan2f(_state.mag_I(1), _state.mag_I(0));
 
-	} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
 		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
 			return _mag_declination_gps;
@@ -953,7 +953,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 // return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
-	ekf_solution_status soln_status;
+	ekf_solution_status_u soln_status;
 	// TODO: Is this accurate enough?
 	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
@@ -1398,14 +1398,14 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
 
 	// rotate measurement into correct earth frame if required
 	switch (_ev_sample_delayed.vel_frame) {
-	case velocity_frame_t::BODY_FRAME_FRD:
+	case VelocityFrame::BODY_FRAME_FRD:
 		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
 		break;
 
-	case velocity_frame_t::LOCAL_FRAME_FRD:
+	case VelocityFrame::LOCAL_FRAME_FRD:
 		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
-		if (_params.fusion_mode & MASK_ROTATE_EV) {
+		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
 			vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth;
 
 		} else {
@@ -1424,12 +1424,12 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
 
 	// rotate measurement into correct earth frame if required
 	switch (_ev_sample_delayed.vel_frame) {
-	case velocity_frame_t::BODY_FRAME_FRD:
+	case VelocityFrame::BODY_FRAME_FRD:
 		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
 		break;
 
-	case velocity_frame_t::LOCAL_FRAME_FRD:
-		if (_params.fusion_mode & MASK_ROTATE_EV) {
+	case VelocityFrame::LOCAL_FRAME_FRD:
+		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
 			ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
 		}
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -129,7 +129,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 	}
 }
 
-void EstimatorInterface::setGpsData(const gps_message &gps)
+void EstimatorInterface::setGpsData(const gpsMessage &gps)
 {
 	if (!_initialised) {
 		return;
@@ -382,7 +382,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if ((_params.fusion_mode & MASK_USE_DRAG)) {
+	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG)) {
 
 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
@@ -447,24 +447,24 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	// mag mode
-	if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+	if (_params.mag_fusion_type != MagFuseType::NONE) {
 		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
 	}
 
 	// range aid or range height
-	if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) {
+	if (_params.range_aid || (_params.vdist_sensor_type == VerticalHeightSensor::RANGE)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & MASK_USE_GPS) {
+	if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & MASK_USE_OF) {
+	if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
+	if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -78,13 +78,13 @@ class EstimatorInterface
 {
 public:
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
-	virtual bool collect_gps(const gps_message &gps) = 0;
+	virtual bool collect_gps(const gpsMessage &gps) = 0;
 
 	void setIMUData(const imuSample &imu_sample);
 
 	void setMagData(const magSample &mag_sample);
 
-	void setGpsData(const gps_message &gps);
+	void setGpsData(const gpsMessage &gps);
 
 	void setBaroData(const baroSample &baro_sample);
 
@@ -206,7 +206,7 @@ public:
 	// At the next startup, set param.mag_declination_deg to the value saved
 	bool get_mag_decl_deg(float *val) const
 	{
-		if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
+		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
 			*val = math::degrees(_mag_declination_gps);
 			return true;
 

src/modules/ekf2/EKF/gps_checks.cpp

@@ -55,7 +55,7 @@
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
 
-bool Ekf::collect_gps(const gps_message &gps)
+bool Ekf::collect_gps(const gpsMessage &gps)
 {
 	// Run GPS checks always
 	_gps_checks_passed = gps_is_good(gps);
@@ -92,7 +92,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 		_mag_strength_gps = get_mag_strength_gauss(lat, lon);
 
 		// request a reset of the yaw using the new declination
-		if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE)
+		if ((_params.mag_fusion_type != MagFuseType::NONE)
 		     && !declination_was_valid) {
 			_mag_yaw_reset_req = true;
 		}
@@ -120,7 +120,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 			_mag_strength_gps = get_mag_strength_gauss(lat, lon);
 
 			// request mag yaw reset if there's a mag declination for the first time
-			if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+			if (_params.mag_fusion_type != MagFuseType::NONE) {
 				if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
 					_mag_yaw_reset_req = true;
 				}
@@ -141,7 +141,7 @@ bool Ekf::collect_gps(const gps_message &gps)
  * Checks are activated using the EKF2_GPS_CHECK bitmask parameter
  * Checks are adjusted using the EKF2_REQ_* parameters
 */
-bool Ekf::gps_is_good(const gps_message &gps)
+bool Ekf::gps_is_good(const gpsMessage &gps)
 {
 	// Check the fix type
 	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);

src/modules/ekf2/EKF/gps_control.cpp

@@ -41,7 +41,7 @@
 
 void Ekf::controlGpsFusion()
 {
-	if (!(_params.fusion_mode & MASK_USE_GPS)) {
+	if (!(_params.fusion_mode & SensorFusionMask::USE_GPS)) {
 		stopGpsFusion();
 		return;
 	}
@@ -109,7 +109,7 @@ void Ekf::controlGpsFusion()
 
 					// TODO: move this to EV control logic
 					// Reset position state to external vision if we are going to use absolute values
-					if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
+					if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) {
 						resetHorizontalPositionToVision();
 					}
 				}
@@ -136,7 +136,7 @@ void Ekf::controlGpsFusion()
 					startGpsFusion();
 				}
 
-			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
+			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
 				// If no mag is used, align using the yaw estimator (if available)
 				if (resetYawToEKFGSF()) {
 					_information_events.flags.yaw_aligned_to_imu_gps = true;

src/modules/ekf2/EKF/mag_control.cpp

@@ -54,7 +54,7 @@ void Ekf::controlMagFusion()
 
 			// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 			// this is useful if there is a lot of interference on the sensor measurement.
-			if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
+			if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
 			    && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
 			   ) {
 				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
@@ -82,7 +82,7 @@ void Ekf::controlMagFusion()
 	// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
 	// ground and to prevent uncontrolled yaw variance growth
 	// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
-	if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
+	if (_params.mag_fusion_type >= MagFuseType::NONE
 	    || _control_status.flags.mag_fault
 	    || !_control_status.flags.tilt_align) {
 
@@ -111,18 +111,18 @@ void Ekf::controlMagFusion()
 		default:
 
 		/* fallthrough */
-		case MAG_FUSE_TYPE_AUTO:
+		case MagFuseType::AUTO:
 			selectMagAuto();
 			break;
 
-		case MAG_FUSE_TYPE_INDOOR:
+		case MagFuseType::INDOOR:
 
 		/* fallthrough */
-		case MAG_FUSE_TYPE_HEADING:
+		case MagFuseType::HEADING:
 			startMagHdgFusion();
 			break;
 
-		case MAG_FUSE_TYPE_3D:
+		case MagFuseType::MAG_3D:
 			startMag3DFusion();
 			break;
 		}
@@ -188,7 +188,7 @@ void Ekf::runOnGroundYawReset()
 
 bool Ekf::canResetMagHeading() const
 {
-	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
+	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MagFuseType::NONE);
 }
 
 void Ekf::runInAirYawReset(const Vector3f &mag_sample)
@@ -280,7 +280,7 @@ void Ekf::checkMagDeclRequired()
 	// then the declination must be fused as an observation to prevent long term heading drift
 	// fusing declination when gps aiding is available is optional, but recommended to prevent
 	// problem if the vehicle is static for extended periods of time
-	const bool user_selected = (_params.mag_declination_source & MASK_FUSE_DECL);
+	const bool user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
 	const bool not_using_ne_aiding = !_control_status.flags.gps;
 	_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
 }
@@ -306,7 +306,7 @@ bool Ekf::shouldInhibitMag() const
 	// is available, assume that we are operating indoors and the magnetometer should not be used.
 	// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
 	// has explicitly stopped magnetometer use.
-	const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
+	const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
 
 	const bool heading_not_required_for_navigation = !_control_status.flags.gps
 			&& !_control_status.flags.ev_pos
@@ -319,7 +319,7 @@ bool Ekf::shouldInhibitMag() const
 void Ekf::checkMagFieldStrength(const Vector3f &mag_sample)
 {
 	if (_params.check_mag_strength
-	    && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
+	    && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
 
 		if (PX4_ISFINITE(_mag_strength_gps)) {
 			constexpr float wmm_gate_size = 0.2f; // +/- Gauss

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -852,7 +852,7 @@ void Ekf::limitDeclination()
 	float decl_reference;
 	float h_field_min = 0.001f;
 
-	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+	if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
 		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
 			decl_reference = _mag_declination_gps;

src/modules/ekf2/EKF2.cpp

@@ -308,7 +308,7 @@ void EKF2::Run()
 		}
 
 		// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
-		if (_params->mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+		if (_params->mag_fusion_type != MagFuseType::NONE) {
 			float sens_mag_rate = 0.f;
 
 			if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
@@ -1153,7 +1153,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			_last_gyro_bias_published = gyro_bias;
 		}
 
-		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
+		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS)) {
 			bias.accel_device_id = _device_id_accel;
 			accel_bias.copyTo(bias.accel_bias);
 			bias.accel_bias_limit = _params->acc_bias_lim;
@@ -1596,10 +1596,10 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 			ev_data.vel(2) = ev_odom.vz;
 
 			if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
-				ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+				ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
 
 			} else {
-				ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 			}
 
 			// velocity measurement error from ev_data or parameters
@@ -1735,7 +1735,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			perf_count(_msg_missed_gps_perf);
 		}
 
-		gps_message gps_msg{
+		gpsMessage gps_msg{
 			.time_usec = vehicle_gps_position.timestamp,
 			.lat = vehicle_gps_position.lat,
 			.lon = vehicle_gps_position.lon,
@@ -1878,7 +1878,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 
 void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 {
-	if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
+	if (_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS) {
 		_accel_cal.cal_available = false;
 		return;
 	}

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -12,7 +12,7 @@ EkfWrapper::~EkfWrapper()
 
 void EkfWrapper::setBaroHeight()
 {
-	_ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO;
+	_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
 }
 
 bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -22,7 +22,7 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
 
 void EkfWrapper::setGpsHeight()
 {
-	_ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS;
+	_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
 }
 
 bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -32,7 +32,7 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
 
 void EkfWrapper::setRangeHeight()
 {
-	_ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE;
+	_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
 }
 
 bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -42,7 +42,7 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
 
 void EkfWrapper::setVisionHeight()
 {
-	_ekf_params->vdist_sensor_type = VDIST_SENSOR_EV;
+	_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
 }
 
 bool EkfWrapper::isIntendingVisionHeightFusion() const
@@ -52,12 +52,12 @@ bool EkfWrapper::isIntendingVisionHeightFusion() const
 
 void EkfWrapper::enableGpsFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_GPS;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS;
 }
 
 void EkfWrapper::disableGpsFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_GPS;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS;
 }
 
 bool EkfWrapper::isIntendingGpsFusion() const
@@ -67,12 +67,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
 
 void EkfWrapper::enableGpsHeadingFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_YAW;
 }
 
 void EkfWrapper::disableGpsHeadingFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_GPSYAW;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_YAW;
 }
 
 bool EkfWrapper::isIntendingGpsHeadingFusion() const
@@ -82,12 +82,12 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const
 
 void EkfWrapper::enableFlowFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_OF;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_OPT_FLOW;
 }
 
 void EkfWrapper::disableFlowFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_OF;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_OPT_FLOW;
 }
 
 bool EkfWrapper::isIntendingFlowFusion() const
@@ -102,12 +102,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
 
 void EkfWrapper::enableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_EVPOS;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
 }
 
 void EkfWrapper::disableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_EVPOS;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
 }
 
 bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@@ -117,12 +117,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
 
 void EkfWrapper::enableExternalVisionVelocityFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_EVVEL;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_VEL;
 }
 
 void EkfWrapper::disableExternalVisionVelocityFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_EVVEL;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_VEL;
 }
 
 bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
@@ -132,12 +132,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
 
 void EkfWrapper::enableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode |= MASK_USE_EVYAW;
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
 }
 
 void EkfWrapper::disableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode &= ~MASK_USE_EVYAW;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
 }
 
 bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@@ -147,12 +147,12 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
 
 void EkfWrapper::enableExternalVisionAlignment()
 {
-	_ekf_params->fusion_mode |= MASK_ROTATE_EV;
+	_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
 }
 
 void EkfWrapper::disableExternalVisionAlignment()
 {
-	_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
+	_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
 }
 
 bool EkfWrapper::isIntendingMagHeadingFusion() const
@@ -167,7 +167,7 @@ bool EkfWrapper::isIntendingMag3DFusion() const
 
 void EkfWrapper::setMagFuseTypeNone()
 {
-	_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE;
+	_ekf_params->mag_fusion_type = MagFuseType::NONE;
 }
 
 bool EkfWrapper::isWindVelocityEstimated() const

src/modules/ekf2/test/sensor_simulator/gps.cpp

@@ -30,7 +30,7 @@ void Gps::send(const uint64_t time)
 	_ekf->setGpsData(_gps_data);
 }
 
-void Gps::setData(const gps_message &gps)
+void Gps::setData(const gpsMessage &gps)
 {
 	_gps_data = gps;
 }
@@ -108,9 +108,9 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
 	_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
 }
 
-gps_message Gps::getDefaultGpsData()
+gpsMessage Gps::getDefaultGpsData()
 {
-	gps_message gps_data{};
+	gpsMessage gps_data{};
 	gps_data.time_usec = 0;
 	gps_data.lat = 473566094;
 	gps_data.lon = 85190237;

src/modules/ekf2/test/sensor_simulator/gps.h

@@ -51,7 +51,7 @@ public:
 	Gps(std::shared_ptr<Ekf> ekf);
 	~Gps();
 
-	void setData(const gps_message &gps);
+	void setData(const gpsMessage &gps);
 	void stepHeightByMeters(const float hgt_change);
 	void stepHorizontalPositionByMeters(const Vector2f hpos_change);
 	void setPositionRateNED(const Vector3f &rate);
@@ -65,12 +65,12 @@ public:
 	void setNumberOfSatellites(const int num_satellites);
 	void setPdop(const float pdop);
 
-	gps_message getDefaultGpsData();
+	gpsMessage getDefaultGpsData();
 
 private:
 	void send(uint64_t time) override;
 
-	gps_message _gps_data{};
+	gpsMessage _gps_data{};
 	Vector3f _gps_pos_rate{};
 };
 

src/modules/ekf2/test/sensor_simulator/vio.cpp

@@ -61,12 +61,12 @@ void Vio::setOrientation(const Quatf &quat)
 
 void Vio::setVelocityFrameToBody()
 {
-	_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+	_vio_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
 }
 
 void Vio::setVelocityFrameToLocal()
 {
-	_vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+	_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 }
 
 extVisionSample Vio::dataAtRest()
@@ -78,7 +78,7 @@ extVisionSample Vio::dataAtRest()
 	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
 	vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
 	vio_data.angVar = 0.05f;
-	vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+	vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
 	return vio_data;
 }
 

src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp

@@ -67,6 +67,10 @@ public:
 	float getRoll() const { return _positions(1); }
 	float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame
 	float getYaw() const { return _positions(3); }
+	float getPitchExpo() const { return _positions_expo(0); }
+	float getRollExpo() const { return _positions_expo(1); }
+	float getThrottleZeroCenteredExpo() const { return -_positions_expo(2); } // Convert Z-axis(down) command to Up-axis frame
+	float getYawExpo() const { return _positions_expo(3); }
 
 	/**
 	 * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -33,8 +33,6 @@
 
 #include "FixedwingAttitudeControl.hpp"
 
-#include <vtol_att_control/vtol_type.h>
-
 using namespace time_literals;
 using math::constrain;
 using math::gradual;
@@ -48,15 +46,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
-	// check if VTOL first
-	if (vtol) {
-		int32_t vt_type = -1;
-
-		if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
-			_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
-		}
-	}
-
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -128,7 +117,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	if (_vehicle_status.is_vtol) {
 		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					 && !_vehicle_status.in_transition_mode;
-		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
+		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _vehicle_status.is_vtol_tailsitter;
 
 		if (is_hovering || is_tailsitter_transition) {
 			_vcontrol_mode.flag_control_attitude_enabled = false;
@@ -140,7 +129,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 void
 FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 {
-	const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
+	const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode;
 	const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
 	if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
@@ -212,7 +201,7 @@ void
 FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
 {
 	if (_rates_sp_sub.update(&_rates_sp)) {
-		if (_is_tailsitter) {
+		if (_vehicle_status.is_vtol_tailsitter) {
 			float tmp = _rates_sp.roll;
 			_rates_sp.roll = -_rates_sp.yaw;
 			_rates_sp.yaw = tmp;
@@ -311,7 +300,7 @@ void FixedwingAttitudeControl::Run()
 		float pitchspeed = angular_velocity.xyz[1];
 		float yawspeed = angular_velocity.xyz[2];
 
-		if (_is_tailsitter) {
+		if (_vehicle_status.is_vtol_tailsitter) {
 			/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 			 *
 			 * Since the VTOL airframe is initialized as a multicopter we need to
@@ -379,7 +368,7 @@ void FixedwingAttitudeControl::Run()
 		// lock integrator if no rate control enabled, or in RW mode (but not transitioning VTOL or tailsitter), or for long intervals (> 20 ms)
 		bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
 				       || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
-					   !_vehicle_status.in_transition_mode && !_is_tailsitter)
+					   !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)
 				       || (dt > 0.02f);
 
 		/* if we are in rotary wing mode, do nothing */
@@ -417,7 +406,7 @@ void FixedwingAttitudeControl::Run()
 			 */
 			if (_landed
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-				&& !_vehicle_status.in_transition_mode && !_is_tailsitter)) {
+				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
 
 				_roll_ctrl.reset_integrator();
 				_pitch_ctrl.reset_integrator();

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -151,8 +151,6 @@ private:
 
 	bool _flag_control_attitude_enabled_last{false};
 
-	bool _is_tailsitter{false};
-
 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};
 	float _control_prev[3] {};

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -33,7 +33,6 @@
 
 #include "FixedwingPositionControl.hpp"
 
-#include <vtol_att_control/vtol_type.h>
 #include <px4_platform_common/events.h>
 
 using math::constrain;
@@ -59,12 +58,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 {
 	if (vtol) {
 		_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
-
-		// VTOL parameter VTOL_TYPE
-		int32_t vt_type = -1;
-		param_get(param_find("VT_TYPE"), &vt_type);
-
-		_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
 	}
 
 	// limit to 50 Hz
@@ -370,7 +363,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 
 		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
 		// between multirotor and fixed wing flight
-		if (_vtol_tailsitter) {
+		if (_vehicle_status.is_vtol_tailsitter) {
 			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
 			R = R * R_offset;
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -95,7 +95,6 @@
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
 #include <uORB/uORB.h>
-#include <vtol_att_control/vtol_type.h>
 
 using namespace launchdetection;
 using namespace runwaytakeoff;
@@ -527,7 +526,6 @@ private:
 		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
 
 		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad
-
 	)
 
 };

src/modules/mavlink/mavlink_mission.cpp

@@ -298,19 +298,21 @@ void
 MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 	mission_item_s mission_item{};
-	int read_result = 0;
+	int16_t bytes_read = 0;
+	bool read_success = false;
 
 	switch (_mission_type) {
 
 	case MAV_MISSION_TYPE_MISSION: {
-			read_result = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s);
+			bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s));
+			read_success = (bytes_read == sizeof(mission_item_s));
 		}
 		break;
 
 	case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
 			mission_fence_point_s mission_fence_point;
-			read_result = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) ==
-				      sizeof(mission_fence_point_s);
+			bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s));
+			read_success = (bytes_read == sizeof(mission_fence_point_s));
 
 			mission_item.nav_cmd = mission_fence_point.nav_cmd;
 			mission_item.frame = mission_fence_point.frame;
@@ -330,8 +332,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 
 	case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
 			mission_safe_point_s mission_safe_point;
-			read_result = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) ==
-				      sizeof(mission_safe_point_s);
+			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
+			read_success = (bytes_read == sizeof(mission_safe_point_s));
 
 			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
 			mission_item.frame = mission_safe_point.frame;
@@ -348,7 +350,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		break;
 	}
 
-	if (read_result > 0) {
+	if (read_success) {
 		_time_last_sent = hrt_absolute_time();
 
 		if (_int_mode) {
@@ -382,9 +384,14 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
 
 		if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
-			_mavlink->send_statustext_critical("Mission storage: Unable to read from microSD\t");
-			events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
-				     "Mission: Unable to read from storage");
+			mavlink_log_critical(_mavlink->get_mavlink_log_pub(),
+					     "Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t",
+					     (uint8_t)_mission_type, bytes_read);
+			/* EVENT
+			 * @description Mission type: {1}. Number of bytes read: {2}
+			 */
+			events::send<uint8_t, int16_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
+						       "Mission: Unable to read from storage", _mission_type, bytes_read);
 		}
 
 		PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id);

src/modules/mavlink/mavlink_receiver.cpp

@@ -2323,6 +2323,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_battery_status.discharged_mah = -1.0f;
 		hil_battery_status.connected = true;
 		hil_battery_status.remaining = 0.70;
+		hil_battery_status.time_remaining_s = NAN;
 
 		_battery_pub.publish(hil_battery_status);
 	}
@@ -2752,6 +2753,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 		hil_battery_status.timestamp = hrt_absolute_time();
+		hil_battery_status.time_remaining_s = NAN;
 		_battery_pub.publish(hil_battery_status);
 	}
 }

src/modules/mc_att_control/mc_att_control.hpp

@@ -54,7 +54,6 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <vtol_att_control/vtol_type.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 #include <AttitudeControl.hpp>

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -58,13 +58,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
 	_vtol(vtol)
 {
-	if (_vtol) {
-		int32_t vt_type = -1;
-
-		if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
-			_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
-		}
-	}
 
 	parameters_updated();
 }
@@ -301,6 +294,8 @@ MulticopterAttitudeControl::Run()
 				_vehicle_type_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 				_vtol = vehicle_status.is_vtol;
 				_vtol_in_transition_mode = vehicle_status.in_transition_mode;
+				_vtol_tailsitter = vehicle_status.is_vtol_tailsitter;
+
 			}
 		}
 

src/modules/rc_update/CMakeLists.txt

@@ -38,6 +38,9 @@ px4_add_module(
 		rc_update.cpp
 		rc_update.h
 	DEPENDS
+		hysteresis
 		mathlib
 		px4_work_queue
 	)
+
+px4_add_functional_gtest(SRC RCUpdateTest.cpp LINKLIBS modules__rc_update)

src/modules/rc_update/RCUpdateTest.cpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 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.
+ *
+ ****************************************************************************/
+
+#define MODULE_NAME "rc_update"
+
+#include <gtest/gtest.h>
+#include "rc_update.h"
+
+using namespace rc_update;
+
+TEST(RCUpdateTest, ModeSlotUnassigned)
+{
+	RCUpdate rc_update;
+	// GIVEN: Default configuration with no assigned mode switch
+	EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 0);
+
+	// WHEN: we update the switches two times to pass the simple outlier protection
+	rc_update.UpdateManualSwitches(0);
+	rc_update.UpdateManualSwitches(0);
+
+	// THEN: we receive no mode slot
+	uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	manual_control_switches_sub.update();
+
+	EXPECT_EQ(manual_control_switches_sub.get().mode_slot, 0); // manual_control_switches_s::MODE_SLOT_NONE
+}
+
+void checkModeSlotSwitch(float channel_value, uint8_t expected_slot)
+{
+	RCUpdate rc_update;
+
+	// GIVEN: First channel is configured as mode switch
+	rc_update._param_rc_map_fltmode.set(1);
+	EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 1);
+	// GIVEN: First channel has some value
+	rc_update._rc.channels[0] = channel_value;
+
+	// WHEN: we update the switches two times to pass the simple outlier protection
+	rc_update.UpdateManualSwitches(0);
+	rc_update.UpdateManualSwitches(0);
+
+	// THEN: we receive the expected mode slot
+	uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	manual_control_switches_sub.update();
+
+	EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
+}
+
+TEST(RCUpdateTest, ModeSlotSwitchAllValues)
+{
+	checkModeSlotSwitch(-1.f, 1); // manual_control_switches_s::MODE_SLOT_1
+	checkModeSlotSwitch(-.5f, 2); // manual_control_switches_s::MODE_SLOT_2
+	checkModeSlotSwitch(-.1f, 3); // manual_control_switches_s::MODE_SLOT_3
+	checkModeSlotSwitch(0.f, 4); // manual_control_switches_s::MODE_SLOT_4
+	checkModeSlotSwitch(.5f, 5); // manual_control_switches_s::MODE_SLOT_5
+	checkModeSlotSwitch(1.f, 6); // manual_control_switches_s::MODE_SLOT_6
+}
+
+void checkModeSlotButton(uint8_t button_configuration, uint8_t channel, float channel_value, uint8_t expected_slot)
+{
+	RCUpdate rc_update;
+
+	// GIVEN: Buttons are configured
+	rc_update._param_rc_map_fltm_btn.set(button_configuration);
+	EXPECT_EQ(rc_update._param_rc_map_fltm_btn.get(), button_configuration);
+	// GIVEN: buttons are mapped
+	rc_update.update_rc_functions();
+	// GIVEN: First channel has some value
+	rc_update._rc.channels[channel - 1] = channel_value;
+
+	// WHEN: we update the switches 4 times:
+	// - initiate the button press
+	// - keep the same button pressed
+	// - hold the button for 50ms
+	// - pass the simple outlier protection
+	rc_update.UpdateManualSwitches(0);
+	rc_update.UpdateManualSwitches(0);
+	rc_update.UpdateManualSwitches(51_ms);
+	rc_update.UpdateManualSwitches(51_ms);
+
+	// THEN: we receive the expected mode slot
+	uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	manual_control_switches_sub.update();
+
+	EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot);
+}
+
+TEST(RCUpdateTest, ModeSlotButtonAllValues)
+{
+	checkModeSlotButton(1, 1, -1.f, 0); // button not pressed -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(1, 1, 0.f, 0); // button not pressed over threshold -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(1, 1, 1.f, 1); // button 1 pressed -> manual_control_switches_s::MODE_SLOT_1
+	checkModeSlotButton(1, 2, 1.f, 0); // button 2 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(3, 2, 1.f, 2); // button 2 pressed -> manual_control_switches_s::MODE_SLOT_2
+	checkModeSlotButton(3, 3, 1.f, 0); // button 3 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(7, 3, 1.f, 3); // button 3 pressed -> manual_control_switches_s::MODE_SLOT_3
+	checkModeSlotButton(7, 4, 1.f, 0); // button 4 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(15, 4, 1.f, 4); // button 4 pressed -> manual_control_switches_s::MODE_SLOT_4
+	checkModeSlotButton(15, 5, 1.f, 0); // button 5 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(31, 5, 1.f, 5); // button 5 pressed -> manual_control_switches_s::MODE_SLOT_5
+	checkModeSlotButton(31, 6, 1.f, 0); // button 6 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE
+	checkModeSlotButton(63, 6, 1.f, 6); // button 6 pressed -> manual_control_switches_s::MODE_SLOT_6
+}

src/modules/rc_update/rc_update.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -320,7 +320,7 @@ RCUpdate::map_flight_modes_buttons()
 	}
 
 	// If the functionality is disabled we don't need to map channels
-	const int flightmode_buttons = _param_rc_map_flightmode_buttons.get();
+	const int flightmode_buttons = _param_rc_map_fltm_btn.get();
 
 	if (flightmode_buttons == 0) {
 		return;
@@ -605,7 +605,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 			switches.mode_slot = num_slots;
 		}
 
-	} else if (_param_rc_map_flightmode_buttons.get() > 0) {
+	} else if (_param_rc_map_fltm_btn.get() > 0) {
 		switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
 		bool is_consistent_button_press = false;
 
@@ -629,7 +629,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 			}
 		}
 
-		_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, hrt_absolute_time());
+		_button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, timestamp_sample);
 
 		if (_button_pressed_hysteresis.get_state()) {
 			switches.mode_slot = _potential_button_press_slot;

src/modules/rc_update/rc_update.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -46,6 +46,7 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <drivers/drv_hrt.h>
+#include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <uORB/Publication.hpp>
@@ -59,7 +60,6 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/parameter_update.h>
-#include <hysteresis/hysteresis.h>
 
 using namespace time_literals;
 
@@ -90,8 +90,6 @@ public:
 
 	int print_status() override;
 
-private:
-
 	static constexpr uint64_t VALID_DATA_MIN_INTERVAL_US{1_s / 3}; // assume valid RC input is at least 3 Hz
 
 	void Run() override;
@@ -219,7 +217,7 @@ private:
 		(ParamInt<px4::params::RC_MAP_ARM_SW>) _param_rc_map_arm_sw,
 		(ParamInt<px4::params::RC_MAP_TRANS_SW>) _param_rc_map_trans_sw,
 		(ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
-		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,
+		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_fltm_btn,
 
 		(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
 		(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -407,11 +407,11 @@ bool VehicleIMU::UpdateAccel()
 
 			_publish_status = true;
 
-			if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) {
+			if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) {
 				// start notifying the user periodically if there's significant continuous clipping
 				const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2];
 
-				if (clipping_total > _last_clipping_notify_total_count + 1000) {
+				if (clipping_total > _last_accel_clipping_notify_total_count + 1000) {
 					mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance);
 					/* EVENT
 					 * @description Land now, and check the vehicle setup.
@@ -419,8 +419,8 @@ bool VehicleIMU::UpdateAccel()
 					 */
 					events::send<uint8_t>(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical,
 							      "Accel {1} clipping, not safe to fly!", _instance);
-					_last_clipping_notify_time = accel.timestamp_sample;
-					_last_clipping_notify_total_count = clipping_total;
+					_last_accel_clipping_notify_time = accel.timestamp_sample;
+					_last_accel_clipping_notify_total_count = clipping_total;
 				}
 			}
 		}
@@ -534,6 +534,52 @@ bool VehicleIMU::UpdateGyro()
 		_gyro_integrator.put(gyro_raw, dt);
 
 		updated = true;
+
+		if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) {
+			// rotate sensor clip counts into vehicle body frame
+			const Vector3f clipping{_gyro_calibration.rotation() *
+						Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}};
+
+			// round to get reasonble clip counts per axis (after board rotation)
+			const uint8_t clip_x = roundf(fabsf(clipping(0)));
+			const uint8_t clip_y = roundf(fabsf(clipping(1)));
+			const uint8_t clip_z = roundf(fabsf(clipping(2)));
+
+			_status.gyro_clipping[0] += clip_x;
+			_status.gyro_clipping[1] += clip_y;
+			_status.gyro_clipping[2] += clip_z;
+
+			if (clip_x > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_X;
+			}
+
+			if (clip_y > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y;
+			}
+
+			if (clip_z > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z;
+			}
+
+			_publish_status = true;
+
+			if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) {
+				// start notifying the user periodically if there's significant continuous clipping
+				const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2];
+
+				if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) {
+					mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance);
+					/* EVENT
+					 * @description Land now, and check the vehicle setup.
+					 * Clipping can lead to fly-aways.
+					 */
+					events::send<uint8_t>(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical,
+							      "Gyro {1} clipping, not safe to fly!", _instance);
+					_last_gyro_clipping_notify_time = gyro.timestamp_sample;
+					_last_gyro_clipping_notify_total_count = clipping_total;
+				}
+			}
+		}
 	}
 
 	return updated;

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -148,10 +148,15 @@ private:
 	float _coning_norm_accum{0};
 	float _coning_norm_accum_total_time_s{0};
 
-	uint8_t _delta_velocity_clipping{0};
+	uint8_t     _delta_angle_clipping{0};
+	uint8_t     _delta_velocity_clipping{0};
+
+	hrt_abstime _last_accel_clipping_notify_time{0};
+	hrt_abstime _last_gyro_clipping_notify_time{0};
+
+	uint64_t    _last_accel_clipping_notify_total_count{0};
+	uint64_t    _last_gyro_clipping_notify_total_count{0};
 
-	hrt_abstime _last_clipping_notify_time{0};
-	uint64_t _last_clipping_notify_total_count{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	uint32_t _backup_schedule_timeout_us{20000};

src/modules/sensors/voted_sensors_update.cpp

@@ -172,6 +172,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 			_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
 			_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
 			_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
+			_last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping;
 			_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
 			_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
 
@@ -229,11 +230,14 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
 		       sizeof(raw.accelerometer_m_s2));
 		memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
+
 		raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
-		raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
-		raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
-		raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count;
-		raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count;
+		raw.accelerometer_clipping    = _last_sensor_data[accel_best_index].accelerometer_clipping;
+		raw.accel_calibration_count   = _last_sensor_data[accel_best_index].accel_calibration_count;
+
+		raw.gyro_integral_dt          = _last_sensor_data[gyro_best_index].gyro_integral_dt;
+		raw.gyro_clipping             = _last_sensor_data[gyro_best_index].gyro_clipping;
+		raw.gyro_calibration_count    = _last_sensor_data[gyro_best_index].gyro_calibration_count;
 
 		if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
 			_accel.last_best_vote = (uint8_t)accel_best_index;

src/modules/vtol_att_control/standard.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -54,47 +54,20 @@ Standard::Standard(VtolAttitudeControl *attc) :
 {
 	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
 	_vtol_schedule.transition_start = 0;
-	_pusher_active = false;
 
 	_mc_roll_weight = 1.0f;
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
 	_mc_throttle_weight = 1.0f;
-
-	_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
-	_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
-	_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
-	_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
-	_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
 }
 
 void
 Standard::parameters_update()
 {
-	float v;
-
-	/* duration of a forwards transition to fw mode */
-	param_get(_params_handles_standard.pusher_ramp_dt, &v);
-	_params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f);
-
-	/* MC ramp up during back transition to mc mode */
-	param_get(_params_handles_standard.back_trans_ramp, &v);
-	_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration);
-
-	_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
-
-	/* pitch setpoint offset */
-	param_get(_params_handles_standard.pitch_setpoint_offset, &v);
-	_params_standard.pitch_setpoint_offset = math::radians(v);
-
-	/* reverse output */
-	param_get(_params_handles_standard.reverse_output, &v);
-	_params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f);
-
-	/* reverse output */
-	param_get(_params_handles_standard.reverse_delay, &v);
-	_params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f);
+	VtolType::updateParams();
 
+	// make sure that pusher ramp in backtransition is smaller than back transition (max) duration
+	_param_vt_b_trans_ramp.set(math::min(_param_vt_b_trans_ramp.get(), _param_vt_b_trans_dur.get()));
 }
 
 void Standard::update_vtol_state()
@@ -132,7 +105,7 @@ void Standard::update_vtol_state()
 			// Regular backtransition
 			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
 			_vtol_schedule.transition_start = hrt_absolute_time();
-			_reverse_output = _params_standard.reverse_output;
+			_reverse_output = _param_vt_b_rev_out.get();
 
 		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
 			// failsafe back to mc mode
@@ -148,13 +121,13 @@ void Standard::update_vtol_state()
 			if (_local_pos->v_xy_valid) {
 				const Dcmf R_to_body(Quatf(_v_att->q).inversed());
 				const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
-				exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
+				exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get();
 
 			} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
-				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
+				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
 			}
 
-			const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
+			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
 
 			if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
 				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
@@ -179,14 +152,14 @@ void Standard::update_vtol_state()
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 
 			const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
-					&& !_params->airspeed_disabled;
+					&& !_param_fw_arsp_mode.get();
 			const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
 
 			bool transition_to_fw = false;
 
 			if (minimum_trans_time_elapsed) {
 				if (airspeed_triggers_transition) {
-					transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
+					transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();
 
 				} else {
 					transition_to_fw = true;
@@ -248,41 +221,43 @@ void Standard::update_transition_state()
 	}
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
-		if (_params_standard.pusher_ramp_dt <= 0.0f) {
+		if (_param_vt_psher_rmp_dt.get() <= 0.0f) {
 			// just set the final target throttle value
-			_pusher_throttle = _params->front_trans_throttle;
+			_pusher_throttle = _param_vt_f_trans_thr.get();
 
-		} else if (_pusher_throttle <= _params->front_trans_throttle) {
+		} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
 			// ramp up throttle to the target throttle value
-			_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
+			_pusher_throttle = _param_vt_f_trans_thr.get() * time_since_trans_start / _param_vt_psher_rmp_dt.get();
 		}
 
+		_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();
+
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
 		    PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
 		    _airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
-		    _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend &&
+		    _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
 		    time_since_trans_start > getMinimumFrontTransitionTime()) {
 
-			mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
+			mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
-		} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
+		} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
 			mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 
 		}
 
 		// ramp up FW_PSP_OFF
-		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
+		_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
 
 		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 
 		// check front transition timeout
-		if (_params->front_trans_timeout > FLT_EPSILON) {
-			if (time_since_trans_start > _params->front_trans_timeout) {
+		if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
+			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
 				// transition timeout occured, abort transition
 				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
 			}
@@ -304,17 +279,16 @@ void Standard::update_transition_state()
 
 		_pusher_throttle = 0.0f;
 
-		if (time_since_trans_start >= _params_standard.reverse_delay) {
+		if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
 			// Handle throttle reversal for active breaking
-			float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
+			float thrscale = (time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
 			thrscale = math::constrain(thrscale, 0.0f, 1.0f);
-			_pusher_throttle = thrscale * _params->back_trans_throttle;
+			_pusher_throttle = thrscale * _param_vt_b_trans_thr.get();
 		}
 
 		// continually increase mc attitude control as we transition back to mc mode
-		if (_params_standard.back_trans_ramp > FLT_EPSILON) {
-			mc_weight = time_since_trans_start / _params_standard.back_trans_ramp;
-
+		if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
+			mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
 		}
 
 		set_all_motor_state(motor_state::ENABLED);
@@ -357,8 +331,6 @@ void Standard::fill_actuator_outputs()
 	auto &mc_out = _actuators_out_0->control;
 	auto &fw_out = _actuators_out_1->control;
 
-	const bool elevon_lock = (_params->elevons_mc_lock == 1);
-
 	switch (_vtol_schedule.flight_mode) {
 	case vtol_mode::MC_MODE:
 
@@ -370,8 +342,10 @@ void Standard::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 		// FW out = 0, other than roll and pitch depending on elevon lock
-		fw_out[actuator_controls_s::INDEX_ROLL]         = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH];
+		fw_out[actuator_controls_s::INDEX_ROLL]         = _param_vt_elev_mc_lock.get() ? 0 :
+				fw_in[actuator_controls_s::INDEX_ROLL];
+		fw_out[actuator_controls_s::INDEX_PITCH]        = _param_vt_elev_mc_lock.get() ? 0 :
+				fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]          = 0;
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;
 		fw_out[actuator_controls_s::INDEX_FLAPS]        = _flaps_setpoint_with_slewrate.getState();

src/modules/vtol_att_control/standard.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -46,8 +46,6 @@
 #ifndef STANDARD_H
 #define STANDARD_H
 #include "vtol_type.h"
-#include <parameters/param.h>
-#include <drivers/drv_hrt.h>
 
 class Standard : public VtolType
 {
@@ -67,22 +65,6 @@ public:
 
 private:
 
-	struct {
-		float pusher_ramp_dt;
-		float back_trans_ramp;
-		float pitch_setpoint_offset;
-		float reverse_output;
-		float reverse_delay;
-	} _params_standard;
-
-	struct {
-		param_t pusher_ramp_dt;
-		param_t back_trans_ramp;
-		param_t pitch_setpoint_offset;
-		param_t reverse_output;
-		param_t reverse_delay;
-	} _params_handles_standard;
-
 	enum class vtol_mode {
 		MC_MODE = 0,
 		TRANSITION_TO_FW,
@@ -100,5 +82,13 @@ private:
 	float _airspeed_trans_blend_margin{0.0f};
 
 	void parameters_update() override;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
+					(ParamFloat<px4::params::VT_PSHER_RMP_DT>) _param_vt_psher_rmp_dt,
+					(ParamFloat<px4::params::VT_B_TRANS_RAMP>) _param_vt_b_trans_ramp,
+					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
+					(ParamFloat<px4::params::VT_B_REV_OUT>) _param_vt_b_rev_out,
+					(ParamFloat<px4::params::VT_B_REV_DEL>) _param_vt_b_rev_del
+				       )
 };
 #endif

src/modules/vtol_att_control/tailsitter.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -55,21 +55,14 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
 	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
 	_vtol_schedule.transition_start = 0;
 
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-
 	_flag_was_in_trans_mode = false;
-	_params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
 }
 
 void
 Tailsitter::parameters_update()
 {
-	float v;
+	VtolType::updateParams();
 
-	param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
-	_params_tailsitter.fw_pitch_sp_offset = math::radians(v);
 }
 
 void Tailsitter::update_vtol_state()
@@ -111,7 +104,7 @@ void Tailsitter::update_vtol_state()
 			float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 			// check if we have reached pitch angle to switch to MC mode
-			if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
+			if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
 				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
 			}
 
@@ -136,13 +129,13 @@ void Tailsitter::update_vtol_state()
 
 
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
-						&& !_params->airspeed_disabled;
+						&& !_param_fw_arsp_mode.get() ;
 
 				bool transition_to_fw = false;
 
 				if (pitch <= PITCH_TRANSITION_FRONT_P1) {
 					if (airspeed_triggers_transition) {
-						transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
+						transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
 
 					} else {
 						transition_to_fw = true;
@@ -156,8 +149,8 @@ void Tailsitter::update_vtol_state()
 				}
 
 				// check front transition timeout
-				if (_params->front_trans_timeout > FLT_EPSILON) {
-					if (time_since_trans_start > _params->front_trans_timeout) {
+				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
+					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
 						// transition timeout occured, abort transition
 						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
 					}
@@ -177,24 +170,20 @@ void Tailsitter::update_vtol_state()
 	switch (_vtol_schedule.flight_mode) {
 	case vtol_mode::MC_MODE:
 		_vtol_mode = mode::ROTARY_WING;
-		_vtol_vehicle_status->vtol_in_trans_mode = false;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::FW_MODE:
 		_vtol_mode = mode::FIXED_WING;
-		_vtol_vehicle_status->vtol_in_trans_mode = false;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::TRANSITION_FRONT_P1:
 		_vtol_mode = mode::TRANSITION_TO_FW;
-		_vtol_vehicle_status->vtol_in_trans_mode = true;
 		break;
 
 	case vtol_mode::TRANSITION_BACK:
 		_vtol_mode = mode::TRANSITION_TO_MC;
-		_vtol_vehicle_status->vtol_in_trans_mode = true;
 		break;
 	}
 }
@@ -245,16 +234,17 @@ void Tailsitter::update_transition_state()
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
 
-		const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
+		// calculate pitching rate - and constrain to at least 0.1s transition time
+		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_f_trans_dur.get(), 0.1f);
 
-		if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
+		if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
 			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
 						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 
 		// check front transition timeout
-		if (_params->front_trans_timeout > FLT_EPSILON) {
-			if (time_since_trans_start > _params->front_trans_timeout) {
+		if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
+			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
 				// transition timeout occured, abort transition
 				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
 			}
@@ -262,7 +252,8 @@ void Tailsitter::update_transition_state()
 
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 
-		const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
+		// calculate pitching rate - and constrain to at least 0.1s transition time
+		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
 
 		if (!_flag_idle_mc) {
 			_flag_idle_mc = set_idle_mc();
@@ -276,10 +267,6 @@ void Tailsitter::update_transition_state()
 
 	_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
 
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-
 	_v_att_sp->timestamp = hrt_absolute_time();
 
 	const Eulerf euler_sp(_q_trans_sp);
@@ -338,9 +325,9 @@ void Tailsitter::fill_actuator_outputs()
 	_thrust_setpoint_1->xyz[2] = 0.f;
 
 
-	mc_out[actuator_controls_s::INDEX_ROLL]  = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
-	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
+	mc_out[actuator_controls_s::INDEX_ROLL]  = mc_in[actuator_controls_s::INDEX_ROLL];
+	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
+	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW];
 
 	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
@@ -349,15 +336,15 @@ void Tailsitter::fill_actuator_outputs()
 		_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
-		if (_params->diff_thrust == 1) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
-			_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
+		if (_param_vt_fw_difthr_en.get()) {
+			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
+			_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
 		}
 
 	} else {
-		_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL]  * _mc_roll_weight;
-		_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-		_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
+		_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL];
+		_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH];
+		_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW];
 
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
 		_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
@@ -371,7 +358,7 @@ void Tailsitter::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
 	}
 
-	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 

src/modules/vtol_att_control/tailsitter.h

@@ -62,15 +62,6 @@ public:
 	void waiting_on_tecs() override;
 
 private:
-
-	struct {
-		float fw_pitch_sp_offset;
-	} _params_tailsitter{};
-
-	struct {
-		param_t fw_pitch_sp_offset;
-	} _params_handles_tailsitter{};
-
 	enum class vtol_mode {
 		MC_MODE = 0,			/**< vtol is in multicopter mode */
 		TRANSITION_FRONT_P1,	/**< vtol is in front transition part 1 mode */
@@ -89,5 +80,10 @@ private:
 
 	void parameters_update() override;
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
+					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
+				       )
+
+
 };
 #endif

src/modules/vtol_att_control/tiltrotor.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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,38 +63,12 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_mc_yaw_weight = 1.0f;
 
 	_flag_was_in_trans_mode = false;
-
-	_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
-	_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
-	_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
-	_params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP");
-	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
 }
 
 void
 Tiltrotor::parameters_update()
 {
-	float v;
-
-	/* vtol tilt mechanism position in mc mode */
-	param_get(_params_handles_tiltrotor.tilt_mc, &v);
-	_params_tiltrotor.tilt_mc = v;
-
-	/* vtol tilt mechanism position in transition mode */
-	param_get(_params_handles_tiltrotor.tilt_transition, &v);
-	_params_tiltrotor.tilt_transition = v;
-
-	/* vtol tilt mechanism position in fw mode */
-	param_get(_params_handles_tiltrotor.tilt_fw, &v);
-	_params_tiltrotor.tilt_fw = v;
-
-	/* vtol tilt mechanism position during motor spinup */
-	param_get(_params_handles_tiltrotor.tilt_spinup, &v);
-	_params_tiltrotor.tilt_spinup = v;
-
-	/* vtol front transition phase 2 duration */
-	param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
-	_params_tiltrotor.front_trans_dur_p2 = v;
+	VtolType::updateParams();
 }
 
 void Tiltrotor::update_vtol_state()
@@ -137,7 +111,7 @@ void Tiltrotor::update_vtol_state()
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
-			const bool exit_backtransition_tilt_condition = _tilt_control <= (_params_tiltrotor.tilt_mc + 0.01f);
+			const bool exit_backtransition_tilt_condition = _tilt_control <= (_param_vt_tilt_mc.get() + 0.01f);
 
 			// speed exit condition: use ground if valid, otherwise airspeed
 			bool exit_backtransition_speed_condition = false;
@@ -145,14 +119,14 @@ void Tiltrotor::update_vtol_state()
 			if (_local_pos->v_xy_valid) {
 				const Dcmf R_to_body(Quatf(_v_att->q).inversed());
 				const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
-				exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
+				exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get() ;
 
 			} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
-				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
+				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
 			}
 
 			const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-			const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
+			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
 
 			if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
 				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
@@ -178,16 +152,16 @@ void Tiltrotor::update_vtol_state()
 				float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
-						&& !_params->airspeed_disabled;
+						&& !_param_fw_arsp_mode.get() ;
 
 				bool transition_to_p2 = false;
 
 				if (time_since_trans_start > getMinimumFrontTransitionTime()) {
 					if (airspeed_triggers_transition) {
-						transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
+						transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
 
 					} else {
-						transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
+						transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
 								   time_since_trans_start > getOpenLoopFrontTransitionTime();
 					}
 				}
@@ -200,8 +174,8 @@ void Tiltrotor::update_vtol_state()
 				}
 
 				// check front transition timeout
-				if (_params->front_trans_timeout > FLT_EPSILON) {
-					if (time_since_trans_start > _params->front_trans_timeout) {
+				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
+					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
 						// transition timeout occured, abort transition
 						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
 					}
@@ -213,9 +187,9 @@ void Tiltrotor::update_vtol_state()
 		case vtol_mode::TRANSITION_FRONT_P2:
 
 			// if the rotors have been tilted completely we switch to fw mode
-			if (_tilt_control >= _params_tiltrotor.tilt_fw) {
+			if (_tilt_control >= _param_vt_tilt_fw.get()) {
 				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
-				_tilt_control = _params_tiltrotor.tilt_fw;
+				_tilt_control = _param_vt_tilt_fw.get();
 			}
 
 			break;
@@ -263,7 +237,7 @@ void Tiltrotor::update_mc_state()
 	// reset this timestamp while disarmed
 	if (!_v_control_mode->flag_armed) {
 		_last_timestamp_disarmed = hrt_absolute_time();
-		_tilt_motors_for_startup = _params_tiltrotor.tilt_spinup > 0.01f; // spinup phase only required if spinup tilt > 0
+		_tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0
 
 	} else if (_tilt_motors_for_startup) {
 		// leave motors tilted forward after arming to allow them to spin up easier
@@ -274,12 +248,12 @@ void Tiltrotor::update_mc_state()
 
 	if (_tilt_motors_for_startup) {
 		if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) {
-			_tilt_control = _params_tiltrotor.tilt_spinup;
+			_tilt_control = _param_vt_tilt_spinup.get();
 
 		} else {
 			// duration phase 2: begin to adapt tilt to multicopter tilt
-			float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup);
-			_tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
+			float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get());
+			_tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
 					(_last_timestamp_disarmed + spin_up_duration_p1));
 		}
 
@@ -287,11 +261,11 @@ void Tiltrotor::update_mc_state()
 
 	} else {
 		// normal operation
-		_tilt_control = VtolType::pusher_assist() + _params_tiltrotor.tilt_mc;
+		_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
 		_mc_yaw_weight = 1.0f;
 
 		// do thrust compensation only for legacy (static) allocation
-		if (_params->ctrl_alloc != 1) {
+		if (!_param_sys_ctrl_alloc.get()) {
 			_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
 		}
 	}
@@ -307,7 +281,7 @@ void Tiltrotor::update_fw_state()
 	_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
 
 	// make sure motors are tilted forward
-	_tilt_control = _params_tiltrotor.tilt_fw;
+	_tilt_control = _param_vt_tilt_fw.get();
 }
 
 void Tiltrotor::update_transition_state()
@@ -338,10 +312,10 @@ void Tiltrotor::update_transition_state()
 		set_all_motor_state(motor_state::ENABLED);
 
 		// tilt rotors forward up to certain angle
-		if (_tilt_control <= _params_tiltrotor.tilt_transition) {
-			const float ramped_up_tilt = _params_tiltrotor.tilt_mc +
-						     fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
-						     time_since_trans_start / _params->front_trans_duration;
+		if (_tilt_control <= _param_vt_tilt_trans.get()) {
+			const float ramped_up_tilt = _param_vt_tilt_mc.get() +
+						     fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
+						     time_since_trans_start / _param_vt_f_trans_dur.get() ;
 
 			// only allow increasing tilt (tilt in hover can already be non-zero)
 			_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@@ -351,16 +325,16 @@ void Tiltrotor::update_transition_state()
 		_mc_roll_weight = 1.0f;
 		_mc_yaw_weight = 1.0f;
 
-		if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
-		    _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) {
-			const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
-					     (_params->transition_airspeed - _params->airspeed_blend);
+		if (!_param_fw_arsp_mode.get()  && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
+		    _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) {
+			const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
+					     (_param_vt_arsp_trans.get()  - _param_vt_arsp_blend.get());
 			_mc_roll_weight = weight;
 			_mc_yaw_weight = weight;
 		}
 
 		// without airspeed do timed weight changes
-		if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
+		if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
 		    time_since_trans_start > getMinimumFrontTransitionTime()) {
 			_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
 					  (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
@@ -376,15 +350,15 @@ void Tiltrotor::update_transition_state()
 
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
-		_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
-						fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
-						_params_tiltrotor.front_trans_dur_p2, _params_tiltrotor.tilt_transition, _params_tiltrotor.tilt_fw);
+		_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
+						fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
+						_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
 
 		_mc_roll_weight = 0.0f;
 		_mc_yaw_weight = 0.0f;
 
 		// ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range)
-		int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
+		int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) *
 				      (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
 
 		set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
@@ -412,7 +386,7 @@ void Tiltrotor::update_transition_state()
 
 			float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
 			progress = math::constrain(progress, 0.0f, 1.0f);
-			_tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress);
+			_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
 		}
 
 		_mc_yaw_weight = 1.0f;
@@ -521,9 +495,9 @@ void Tiltrotor::fill_actuator_outputs()
 		_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
-		if (_params->diff_thrust == 1) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
-			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
+		if (_param_vt_fw_difthr_en.get()) {
+			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
+			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
 		}
 
 	} else {
@@ -544,10 +518,10 @@ void Tiltrotor::fill_actuator_outputs()
 	// Fixed wing output
 	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
 
-	if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
-		fw_out[actuator_controls_s::INDEX_ROLL]  	= 0;
-		fw_out[actuator_controls_s::INDEX_PITCH] 	= 0;
-		fw_out[actuator_controls_s::INDEX_YAW]   	= 0;
+	if (_param_vt_elev_mc_lock.get()  && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
+		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
+		fw_out[actuator_controls_s::INDEX_YAW]   = 0;
 
 	} else {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];

src/modules/vtol_att_control/tiltrotor.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -62,23 +62,6 @@ public:
 	void blendThrottleAfterFrontTransition(float scale) override;
 
 private:
-
-	struct {
-		float tilt_mc;				/**< actuator value corresponding to mc tilt */
-		float tilt_transition;			/**< actuator value corresponding to transition tilt (e.g 45 degrees) */
-		float tilt_fw;				/**< actuator value corresponding to fw tilt */
-		float tilt_spinup;			/**< actuator value corresponding to spinup tilt */
-		float front_trans_dur_p2;
-	} _params_tiltrotor;
-
-	struct {
-		param_t tilt_mc;
-		param_t tilt_transition;
-		param_t tilt_fw;
-		param_t tilt_spinup;
-		param_t front_trans_dur_p2;
-	} _params_handles_tiltrotor;
-
 	enum class vtol_mode {
 		MC_MODE = 0,			/**< vtol is in multicopter mode */
 		TRANSITION_FRONT_P1,	/**< vtol is in front transition part 1 mode */
@@ -111,5 +94,13 @@ private:
 	hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
 	bool _tilt_motors_for_startup{false};
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
+					(ParamFloat<px4::params::VT_TILT_MC>) _param_vt_tilt_mc,
+					(ParamFloat<px4::params::VT_TILT_TRANS>) _param_vt_tilt_trans,
+					(ParamFloat<px4::params::VT_TILT_FW>) _param_vt_tilt_fw,
+					(ParamFloat<px4::params::VT_TILT_SPINUP>) _param_vt_tilt_spinup,
+					(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur
+				       )
+
 };
 #endif

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -55,71 +55,22 @@ using namespace matrix;
 using namespace time_literals;
 
 VtolAttitudeControl::VtolAttitudeControl() :
+	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
 	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
 {
-	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
+	// start vtol in rotary wing mode
+	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
-	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
-	_params.vtol_motor_id = 0;
-
-	_params_handles.sys_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
-	_params.ctrl_alloc = 0;
-	param_get(_params_handles.sys_ctrl_alloc, &_params.ctrl_alloc);
-
-	if (_params.ctrl_alloc != 1) {
-		// these are not used with dynamic control allocation
-		_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
-		_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
-		_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
-		_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
-	}
-
-	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
-	_params_handles.vtol_type = param_find("VT_TYPE");
-	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
-	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
-	_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
-	_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
-	_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
-	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
-	_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
-
-	_params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
-	_params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
-	_params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
-	_params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
-	_params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
-	_params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
-	_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
-	_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
-	_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
-	_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
-	_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
-	_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
-	_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
-	_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
-
-	_params_handles.pitch_min_rad = param_find("VT_PTCH_MIN");
-	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
-
-	_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
-	_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
-	_params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2");
-
-	_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
-
-	_params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD");
-	/* fetch initial parameter values */
 	parameters_update();
 
-	if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
+	if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TAILSITTER) {
 		_vtol_type = new Tailsitter(this);
 
-	} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
+	} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TILTROTOR) {
 		_vtol_type = new Tiltrotor(this);
 
-	} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
+	} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::STANDARD) {
 		_vtol_type = new Standard(this);
 
 	} else {
@@ -269,118 +220,22 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
 	}
 }
 
-int
+void
 VtolAttitudeControl::parameters_update()
 {
-	float v;
-	int32_t l;
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
 
-	if (_params.ctrl_alloc != 1) {
-		/* idle pwm for mc mode */
-		param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
-		param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
-		param_get(_params_handles.vt_mc_on_fmu, &l);
-		_params.vt_mc_on_fmu = l;
+		// update parameters from storage
+		updateParams();
 
-		/* vtol motor count */
-		param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
+		if (_vtol_type != nullptr) {
+			_vtol_type->parameters_update();
+		}
 	}
-
-	/* vtol fw permanent stabilization */
-	param_get(_params_handles.vtol_fw_permanent_stab, &l);
-	_vtol_vehicle_status.fw_permanent_stab = (l == 1);
-
-	param_get(_params_handles.vtol_type, &l);
-	_params.vtol_type = l;
-
-	/* vtol lock elevons in multicopter */
-	param_get(_params_handles.elevons_mc_lock, &l);
-	_params.elevons_mc_lock = (l == 1);
-
-	/* minimum relative altitude for FW mode (QuadChute) */
-	param_get(_params_handles.fw_min_alt, &v);
-	_params.fw_min_alt = v;
-
-	/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
-	param_get(_params_handles.fw_alt_err, &v);
-	_params.fw_alt_err = v;
-
-	/* maximum pitch angle (QuadChute) */
-	param_get(_params_handles.fw_qc_max_pitch, &l);
-	_params.fw_qc_max_pitch = l;
-
-	/* maximum roll angle (QuadChute) */
-	param_get(_params_handles.fw_qc_max_roll, &l);
-	_params.fw_qc_max_roll = l;
-
-	param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
-
-	param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);
-
-	/*
-	 * Open loop transition time needs to be larger than minimum transition time,
-	 * anything else makes no sense and can potentially lead to numerical problems.
-	 */
-	if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) {
-		_params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f;
-		param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
-		mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time\t");
-		/* EVENT
-		 * @description <param>VT_F_TR_OL_TM</param> set to {1:.1}.
-		 */
-		events::send<float>(events::ID("vtol_att_ctrl_ol_trans_too_large"), events::Log::Warning,
-				    "Open loop transition time set larger than minimum transition time", _params.front_trans_time_openloop);
-	}
-
-	param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
-	param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
-	param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
-	param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
-	param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
-	param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
-	param_get(_params_handles.airspeed_mode, &l);
-	_params.airspeed_disabled = l != 0;
-	param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
-	param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
-	param_get(_params_handles.diff_thrust, &_params.diff_thrust);
-
-	param_get(_params_handles.diff_thrust_scale, &v);
-	_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
-
-	/* maximum down pitch allowed */
-	param_get(_params_handles.pitch_min_rad, &v);
-	_params.pitch_min_rad = math::radians(v);
-
-	/* maximum down pitch allowed during landing*/
-	param_get(_params_handles.land_pitch_min_rad, &v);
-	_params.land_pitch_min_rad = math::radians(v);
-
-	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
-	param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
-
-	// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
-	_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
-
-	param_get(_params_handles.back_trans_dec_sp, &v);
-	// increase the target deceleration setpoint provided to the controller by 20%
-	// to make overshooting the transition waypoint less likely in the presence of tracking errors
-	_params.back_trans_dec_sp = 1.2f * v;
-
-	param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
-	param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
-
-	param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
-	param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
-	param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
-
-	param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld);
-
-	// update the parameters of the instances of base VtolType
-	if (_vtol_type != nullptr) {
-		_vtol_type->parameters_update();
-	}
-
-	return OK;
 }
 
 void
@@ -408,7 +263,6 @@ VtolAttitudeControl::Run()
 	_last_run_timestamp = now;
 
 	if (!_initialized) {
-		parameters_update();  // initialize parameter cache
 
 		if (_vtol_type->init()) {
 			_initialized = true;
@@ -445,15 +299,7 @@ VtolAttitudeControl::Run()
 	}
 
 	if (should_run) {
-		// check for parameter updates
-		if (_parameter_update_sub.updated()) {
-			// clear update
-			parameter_update_s pupdate;
-			_parameter_update_sub.copy(&pupdate);
-
-			// update parameters from storage
-			parameters_update();
-		}
+		parameters_update();
 
 		_v_control_mode_sub.update(&_v_control_mode);
 		_v_att_sub.update(&_v_att);
@@ -473,8 +319,8 @@ VtolAttitudeControl::Run()
 		}
 
 		// check if mc and fw sp were updated
-		bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
-		bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
+		const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
+		const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
 
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();
@@ -482,13 +328,19 @@ VtolAttitudeControl::Run()
 		// check in which mode we are in and call mode specific functions
 		switch (_vtol_type->get_mode()) {
 		case mode::TRANSITION_TO_FW:
-		case mode::TRANSITION_TO_MC:
-			// vehicle is doing a transition
-			_vtol_vehicle_status.vtol_in_trans_mode = true;
-			_vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition
-			_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW);
+			// vehicle is doing a transition to FW
+			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
 
-			_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
+			if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
+				_vtol_type->update_transition_state();
+				_v_att_sp_pub.publish(_v_att_sp);
+			}
+
+			break;
+
+		case mode::TRANSITION_TO_MC:
+			// vehicle is doing a transition to MC
+			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
 
 			if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
@@ -499,20 +351,18 @@ VtolAttitudeControl::Run()
 
 		case mode::ROTARY_WING:
 			// vehicle is in rotary wing mode
-			_vtol_vehicle_status.vtol_in_rw_mode = true;
-			_vtol_vehicle_status.vtol_in_trans_mode = false;
-			_vtol_vehicle_status.in_transition_to_fw = false;
+			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
-			_vtol_type->update_mc_state();
-			_v_att_sp_pub.publish(_v_att_sp);
+			if (mc_att_sp_updated) {
+				_vtol_type->update_mc_state();
+				_v_att_sp_pub.publish(_v_att_sp);
+			}
 
 			break;
 
 		case mode::FIXED_WING:
 			// vehicle is in fw mode
-			_vtol_vehicle_status.vtol_in_rw_mode = false;
-			_vtol_vehicle_status.vtol_in_trans_mode = false;
-			_vtol_vehicle_status.in_transition_to_fw = false;
+			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 
 			if (fw_att_sp_updated) {
 				_vtol_type->update_fw_state();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -53,12 +53,12 @@
 #include <drivers/drv_pwm_output.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
-#include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
@@ -94,7 +94,7 @@ extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
 
 static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s]
 
-class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
+class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public ModuleParams, public px4::WorkItem
 {
 public:
 
@@ -152,8 +152,6 @@ public:
 	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
 	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
 
-	struct Params 					*get_params() {return &_params;}
-
 private:
 	void Run() override;
 
@@ -214,46 +212,6 @@ private:
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};	// [kg/m^3]
 
-	Params _params{};	// struct holding the parameters
-
-	struct {
-		param_t idle_pwm_mc;
-		param_t vtol_motor_id;
-		param_t vtol_fw_permanent_stab;
-		param_t vtol_type;
-		param_t elevons_mc_lock;
-		param_t fw_min_alt;
-		param_t fw_alt_err;
-		param_t fw_qc_max_pitch;
-		param_t fw_qc_max_roll;
-		param_t front_trans_time_openloop;
-		param_t front_trans_time_min;
-		param_t front_trans_duration;
-		param_t back_trans_duration;
-		param_t transition_airspeed;
-		param_t front_trans_throttle;
-		param_t back_trans_throttle;
-		param_t airspeed_blend;
-		param_t airspeed_mode;
-		param_t front_trans_timeout;
-		param_t mpc_xy_cruise;
-		param_t fw_motors_off;
-		param_t diff_thrust;
-		param_t diff_thrust_scale;
-		param_t pitch_min_rad;
-		param_t land_pitch_min_rad;
-		param_t forward_thrust_scale;
-		param_t dec_to_pitch_ff;
-		param_t dec_to_pitch_i;
-		param_t back_trans_dec_sp;
-		param_t vt_mc_on_fmu;
-		param_t vt_forward_thrust_enable_mode;
-		param_t mpc_land_alt1;
-		param_t mpc_land_alt2;
-		param_t sys_ctrl_alloc;
-		param_t vt_spoiler_mc_ld;
-	} _params_handles{};
-
 	hrt_abstime _last_run_timestamp{0};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines
@@ -271,5 +229,9 @@ private:
 	void		action_request_poll();
 	void		vehicle_cmd_poll();
 
-	int 		parameters_update();			//Update local parameter cache
+	void 		parameters_update();
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::VT_TYPE>) _param_vt_type
+	)
 };

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2022 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
@@ -51,17 +51,6 @@
  */
 PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
 
-/**
- * Permanent stabilization in fw mode
- *
- * If set to one this parameter will cause permanent attitude stabilization in fw mode.
- * This parameter has been introduced for pure convenience sake.
- *
- * @boolean
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
-
 /**
  * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
  *
@@ -92,7 +81,7 @@ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1);
  * Time in seconds used for a transition
  *
  * @unit s
- * @min 0.00
+ * @min 0.1
  * @max 20.00
  * @increment 1
  * @decimal 2
@@ -106,7 +95,7 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f);
  * Time in seconds used for a back transition
  *
  * @unit s
- * @min 0.00
+ * @min 0.1
  * @max 20.00
  * @increment 1
  * @decimal 2
@@ -198,7 +187,7 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
  * Time in seconds after which transition will be cancelled. Disabled if set to 0.
  *
  * @unit s
- * @min 0.00
+ * @min 0.1
  * @max 30.00
  * @increment 1
  * @decimal 2
@@ -370,12 +359,12 @@ PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0);
  * @max 45.0
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f);
+PARAM_DEFINE_FLOAT(VT_PITCH_MIN, -5.0f);
 
 /**
  * Minimum pitch angle during hover landing.
  *
- * Overrides  VT_PTCH_MIN when the vehicle is in LAND mode (hovering).
+ * Overrides  VT_PITCH_MIN when the vehicle is in LAND mode (hovering).
  * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings
  * generating too much lift and preventing the vehicle from sinking at the desired rate.
  *
@@ -383,7 +372,7 @@ PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f);
  * @max 45.0
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f);
+PARAM_DEFINE_FLOAT(VT_LND_PITCH_MIN, -5.0f);
 
 /**
  * Spoiler setting while landing (hover)

src/modules/vtol_att_control/vtol_type.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -52,6 +52,7 @@ using namespace matrix;
 
 
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
+	ModuleParams(nullptr),
 	_attc(att_controller),
 	_vtol_mode(mode::ROTARY_WING)
 {
@@ -74,7 +75,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_airspeed_validated = _attc->get_airspeed();
 	_tecs_status = _attc->get_tecs_status();
 	_land_detected = _attc->get_land_detected();
-	_params = _attc->get_params();
 
 	for (auto &pwm_max : _max_mc_pwm_values.values) {
 		pwm_max = PWM_DEFAULT_MAX;
@@ -87,8 +87,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 
 bool VtolType::init()
 {
-	if (_params->ctrl_alloc != 1) {
-		const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
+	if (!_param_sys_ctrl_alloc.get()) {
+		const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
 
 		int fd = px4_open(dev, 0);
 
@@ -124,8 +124,8 @@ bool VtolType::init()
 
 		px4_close(fd);
 
-		_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id);
-		_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off);
+		_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_mot_id.get());
+		_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_fw_mot_offid.get());
 
 
 		// in order to get the main motors we take all motors and clear the alternate motor bits
@@ -143,6 +143,16 @@ bool VtolType::init()
 
 }
 
+void VtolType::parameters_update()
+{
+	updateParams();
+
+	// make sure that transition speed is above blending speed
+	_param_vt_arsp_trans.set(math::max(_param_vt_arsp_trans.get(), _param_vt_arsp_blend.get()));
+	// make sure that openloop transition time is above minimum time
+	_param_vt_f_tr_ol_tm.set(math::max(_param_vt_f_tr_ol_tm.get(), _param_vt_trans_min_tm.get()));
+}
+
 void VtolType::update_mc_state()
 {
 	if (!_flag_idle_mc) {
@@ -165,7 +175,7 @@ void VtolType::update_mc_state()
 
 	if (_attc->get_pos_sp_triplet()->current.valid
 	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		spoiler_setpoint_hover = _params->vt_spoiler_mc_ld;
+		spoiler_setpoint_hover = _param_vt_spoiler_mc_ld.get();
 	}
 
 	_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_setpoint_hover, 0.f, 1.f), _dt);
@@ -243,12 +253,16 @@ float VtolType::update_and_get_backtransition_pitch_sp()
 	const float track = atan2f(_local_pos->vy, _local_pos->vx);
 	const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
 
+	// increase the target deceleration setpoint provided to the controller by 20%
+	// to make overshooting the transition waypoint less likely in the presence of tracking errors
+	const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f;
+
 	// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
-	const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
+	const float accel_error_forward = dec_sp + accel_body_forward;
 
-	const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
+	const float pitch_sp_new = _param_vt_b_dec_ff.get() * dec_sp + _accel_to_pitch_integ;
 
-	float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
+	float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward;
 
 	if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
 	    (pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) {
@@ -288,15 +302,15 @@ void VtolType::check_quadchute_condition()
 		Eulerf euler = Quatf(_v_att->q);
 
 		// fixed-wing minimum altitude
-		if (_params->fw_min_alt > FLT_EPSILON) {
+		if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
 
-			if (-(_local_pos->z) < _params->fw_min_alt) {
+			if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
 				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
 			}
 		}
 
 		// adaptive quadchute
-		if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
+		if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
 
 			// We use tecs for tracking in FW and local_pos_sp during transitions
 			if (_tecs_running) {
@@ -305,7 +319,7 @@ void VtolType::check_quadchute_condition()
 				_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
 
 				// are we dropping while requesting significant ascend?
-				if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) &&
+				if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
 				    (_ra_hrate < -1.0f) &&
 				    (_ra_hrate_sp > 1.0f)) {
 
@@ -313,7 +327,7 @@ void VtolType::check_quadchute_condition()
 				}
 
 			} else {
-				const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err);
+				const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
 				const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
 
 				if (height_error && height_rate_error) {
@@ -323,17 +337,17 @@ void VtolType::check_quadchute_condition()
 		}
 
 		// fixed-wing maximum pitch angle
-		if (_params->fw_qc_max_pitch > 0) {
+		if (_param_vt_fw_qc_p.get() > 0) {
 
-			if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
+			if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
 				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
 			}
 		}
 
 		// fixed-wing maximum roll angle
-		if (_params->fw_qc_max_roll > 0) {
+		if (_param_vt_fw_qc_r.get() > 0) {
 
-			if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
+			if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
 				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
 			}
 		}
@@ -342,15 +356,15 @@ void VtolType::check_quadchute_condition()
 
 bool VtolType::set_idle_mc()
 {
-	if (_params->ctrl_alloc == 1) {
+	if (_param_sys_ctrl_alloc.get()) {
 		return true;
 	}
 
-	unsigned pwm_value = _params->idle_pwm_mc;
+	unsigned pwm_value = _param_vt_idle_pwm_mc.get();
 	struct pwm_output_values pwm_values {};
 
 	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
+		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
 			pwm_values.values[i] = pwm_value;
 
 		} else {
@@ -365,14 +379,14 @@ bool VtolType::set_idle_mc()
 
 bool VtolType::set_idle_fw()
 {
-	if (_params->ctrl_alloc == 1) {
+	if (_param_sys_ctrl_alloc.get()) {
 		return true;
 	}
 
 	struct pwm_output_values pwm_values {};
 
 	for (int i = 0; i < num_outputs_max; i++) {
-		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
+		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
 			pwm_values.values[i] = PWM_DEFAULT_MIN;
 
 		} else {
@@ -387,7 +401,7 @@ bool VtolType::set_idle_fw()
 
 bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
 {
-	const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
+	const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
 
 	int fd = px4_open(dev, 0);
 
@@ -418,7 +432,7 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
 
 void VtolType::set_all_motor_state(const motor_state target_state, const int value)
 {
-	if (_params->ctrl_alloc == 1) {
+	if (_param_sys_ctrl_alloc.get()) {
 		return;
 	}
 
@@ -428,7 +442,7 @@ void VtolType::set_all_motor_state(const motor_state target_state, const int val
 
 void VtolType::set_main_motor_state(const motor_state target_state, const int value)
 {
-	if (_params->ctrl_alloc == 1) {
+	if (_param_sys_ctrl_alloc.get()) {
 		return;
 	}
 
@@ -442,7 +456,7 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
 
 void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
 {
-	if (_params->ctrl_alloc == 1) {
+	if (_param_sys_ctrl_alloc.get()) {
 		return;
 	}
 
@@ -479,7 +493,7 @@ bool VtolType::set_motor_state(const motor_state target_state, const int32_t cha
 
 		for (int i = 0; i < num_outputs_max; i++) {
 			if (is_channel_set(i, channel_bitmap)) {
-				_current_max_pwm_values.values[i] = _params->idle_pwm_mc;
+				_current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get();
 			}
 		}
 
@@ -537,7 +551,7 @@ float VtolType::pusher_assist()
 	}
 
 	// disable pusher assist depending on setting of forward_thrust_enable_mode:
-	switch (_params->vt_forward_thrust_enable_mode) {
+	switch (_param_vt_fwd_thrust_en.get()) {
 	case DISABLE: // disable in all modes
 		return 0.0f;
 		break;
@@ -552,14 +566,14 @@ float VtolType::pusher_assist()
 		break;
 
 	case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
-		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) {
+		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get())) {
 			return 0.0f;
 		}
 
 		break;
 
 	case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
-		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) {
+		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get())) {
 			return 0.0f;
 		}
 
@@ -569,7 +583,7 @@ float VtolType::pusher_assist()
 		if ((_attc->get_pos_sp_triplet()->current.valid
 		     && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
 		     && _v_control_mode->flag_control_auto_enabled) ||
-		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) {
+		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) {
 			return 0.0f;
 		}
 
@@ -579,7 +593,7 @@ float VtolType::pusher_assist()
 		if ((_attc->get_pos_sp_triplet()->current.valid
 		     && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
 		     && _v_control_mode->flag_control_auto_enabled) ||
-		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) {
+		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) {
 			return 0.0f;
 		}
 
@@ -588,7 +602,7 @@ float VtolType::pusher_assist()
 
 	// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
 	// then the pusher-for-pitch strategy is disabled and we can return
-	if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
+	if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
 			|| _v_control_mode->flag_control_altitude_enabled)) {
 		return 0.0f;
 	}
@@ -619,11 +633,12 @@ float VtolType::pusher_assist()
 	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
 	float forward_thrust = 0.0f;
 
-	float pitch_setpoint_min = _params->pitch_min_rad;
+	float pitch_setpoint_min = math::radians(_param_vt_pitch_min.get());
 
 	if (_attc->get_pos_sp_triplet()->current.valid
 	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift)
+		pitch_setpoint_min = math::radians(
+					     _param_vt_lnd_pitch_min.get()); // set min pitch during LAND (usually lower to generate less lift)
 	}
 
 	// only allow pitching down up to threshold, the rest of the desired
@@ -633,7 +648,7 @@ float VtolType::pusher_assist()
 		// desired roll angle in heading frame stays the same
 		const float roll_new = -asinf(body_z_sp(1));
 
-		forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale;
+		forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _param_vt_fwd_thrust_sc.get();
 		// limit forward actuation to [0, 0.9]
 		forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f);
 
@@ -684,10 +699,10 @@ float VtolType::getFrontTransitionTimeFactor() const
 
 float VtolType::getMinimumFrontTransitionTime() const
 {
-	return getFrontTransitionTimeFactor() * _params->front_trans_time_min;
+	return getFrontTransitionTimeFactor() * _param_vt_trans_min_tm.get();
 }
 
 float VtolType::getOpenLoopFrontTransitionTime() const
 {
-	return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop;
+	return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get();
 }

src/modules/vtol_att_control/vtol_type.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -43,50 +43,16 @@
 #ifndef VTOL_TYPE_H
 #define VTOL_TYPE_H
 
-#include <lib/mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/slew_rate/SlewRate.hpp>
+#include <px4_platform_common/module_params.h>
+
 
 static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s]
 static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s]
 
-struct Params {
-	int32_t ctrl_alloc;
-	int32_t idle_pwm_mc;			// pwm value for idle in mc mode
-	int32_t vtol_motor_id;
-	int32_t vtol_type;
-	bool elevons_mc_lock;		// lock elevons in multicopter mode
-	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
-	float fw_alt_err;			// maximum negative altitude error for FW mode (Adaptive QuadChute)
-	float fw_qc_max_pitch;		// maximum pitch angle FW mode (QuadChute)
-	float fw_qc_max_roll;		// maximum roll angle FW mode (QuadChute)
-	float front_trans_time_openloop;
-	float front_trans_time_min;
-	float front_trans_duration;
-	float back_trans_duration;
-	float transition_airspeed;
-	float front_trans_throttle;
-	float back_trans_throttle;
-	float airspeed_blend;
-	bool airspeed_disabled;
-	float front_trans_timeout;
-	float mpc_xy_cruise;
-	int32_t fw_motors_off;			/**< bitmask of all motors that should be off in fixed wing mode */
-	int32_t diff_thrust;
-	float diff_thrust_scale;
-	float pitch_min_rad;
-	float land_pitch_min_rad;
-	float forward_thrust_scale;
-	float dec_to_pitch_ff;
-	float dec_to_pitch_i;
-	float back_trans_dec_sp;
-	bool vt_mc_on_fmu;
-	int32_t vt_forward_thrust_enable_mode;
-	float mpc_land_alt1;
-	float mpc_land_alt2;
-	float vt_spoiler_mc_ld;
-};
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
 enum class mode {
@@ -133,7 +99,7 @@ enum class pwm_limit_type {
 
 class VtolAttitudeControl;
 
-class VtolType
+class VtolType:  public ModuleParams
 {
 public:
 
@@ -246,11 +212,8 @@ protected:
 	struct vehicle_thrust_setpoint_s 		*_thrust_setpoint_0;
 	struct vehicle_thrust_setpoint_s 		*_thrust_setpoint_1;
 
-	struct Params 					*_params;
-
 	bool _flag_idle_mc = false;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
 
-	bool _pusher_active = false;
 	float _mc_roll_weight = 1.0f;	// weight for multicopter attitude controller roll output
 	float _mc_pitch_weight = 1.0f;	// weight for multicopter attitude controller pitch output
 	float _mc_yaw_weight = 1.0f;	// weight for multicopter attitude controller yaw output
@@ -310,6 +273,46 @@ protected:
 
 	float _dt{0.0025f}; // time step [s]
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
+					(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
+					(ParamFloat<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
+					(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
+					(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
+					(ParamFloat<px4::params::VT_F_TR_OL_TM>) _param_vt_f_tr_ol_tm,
+					(ParamFloat<px4::params::VT_TRANS_MIN_TM>) _param_vt_trans_min_tm,
+
+					(ParamFloat<px4::params::VT_F_TRANS_DUR>) _param_vt_f_trans_dur,
+					(ParamFloat<px4::params::VT_B_TRANS_DUR>) _param_vt_b_trans_dur,
+					(ParamFloat<px4::params::VT_ARSP_TRANS>) _param_vt_arsp_trans,
+					(ParamFloat<px4::params::VT_F_TRANS_THR>) _param_vt_f_trans_thr,
+					(ParamFloat<px4::params::VT_B_TRANS_THR>) _param_vt_b_trans_thr,
+					(ParamFloat<px4::params::VT_ARSP_BLEND>) _param_vt_arsp_blend,
+					(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+					(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
+					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
+					(ParamBool<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_SC>) _param_vt_fw_difthr_sc,
+					(ParamFloat<px4::params::VT_B_DEC_FF>) _param_vt_b_dec_ff,
+					(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
+					(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,
+
+					(ParamFloat<px4::params::VT_PITCH_MIN>) _param_vt_pitch_min,
+					(ParamFloat<px4::params::VT_FWD_THRUST_SC>) _param_vt_fwd_thrust_sc,
+					(ParamInt<px4::params::VT_FWD_THRUST_EN>) _param_vt_fwd_thrust_en,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
+					(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min,
+
+					(ParamBool<px4::params::SYS_CTRL_ALLOC>) _param_sys_ctrl_alloc,
+					(ParamInt<px4::params::VT_IDLE_PWM_MC>) _param_vt_idle_pwm_mc,
+					(ParamInt<px4::params::VT_MOT_ID>) _param_vt_mot_id,
+					(ParamBool<px4::params::VT_MC_ON_FMU>) _param_vt_mc_on_fmu,
+					(ParamInt<px4::params::VT_FW_MOT_OFFID>) _param_vt_fw_mot_offid,
+					(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
+
+				       )
+
 private: