[DO NOT MERGE] v5x debug hacks

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

@@ -131,6 +131,7 @@ 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,30 +181,6 @@ 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,12 +200,13 @@ 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
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		java_version=11
+		gazebo_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 \
@@ -219,30 +220,20 @@ 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_packages \
+		gazebo$gazebo_version \
 		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,15 +9,14 @@ 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=n
+CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/nxp/ucans32k146/src/bringup.c

@@ -159,12 +159,10 @@ 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,7 +33,6 @@ 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

boards/px4/fmu-v5x/nuttx-config/nsh/defconfig

@@ -54,9 +54,16 @@ CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="Auterion"
 CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_ASSERTIONS=y
+CONFIG_DEBUG_ERROR=y
+CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SCHED=y
+CONFIG_DEBUG_SCHED_ERROR=y
+CONFIG_DEBUG_SCHED_WARN=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_WARN=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_GPIO=y

msg/CMakeLists.txt

@@ -164,7 +164,6 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
-	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg

msg/sensor_combined.msg

@@ -2,24 +2,22 @@
 # 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 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.
+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,8 +11,6 @@ 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/tools/urtps_bridge_topics.yaml

@@ -59,7 +59,8 @@ rtps:
     send:    true
   - msg:     vehicle_local_position_setpoint
     receive: true
-  - msg:     trajectory_setpoint
+  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
+    base:    vehicle_local_position_setpoint
     receive: true
   - msg:     vehicle_odometry
     send:    true

msg/trajectory_setpoint.msg

@@ -1,15 +0,0 @@
-# Trajectory setpoint in NED frame
-# Input to PID position controller.
-# Needs to be kinematically consistent and feasible for smooth flight.
-# setting a value to NaN means the state should not be controlled
-
-uint64 timestamp # time since system start (microseconds)
-
-# NED local world frame
-float32[3] position # in meters
-float32[3] velocity # in meters/second
-float32[3] acceleration # in meters/second^2
-float32[3] jerk # in meters/second^3 (for logging only)
-
-float32 yaw # euler angle of desired attitude in radians -PI..+PI
-float32 yawspeed # angular velocity around NED frame z-axis in radians/second

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,15 +8,12 @@ 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,7 +4,6 @@ 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_local_position_setpoint.msg

@@ -1,19 +1,18 @@
 # Local position setpoint in NED frame
-# Telemetry of PID position controller to monitor tracking.
-# NaN means the state was not controlled
+# setting something to NaN means the state should not be controlled
 
 uint64 timestamp	# time since system start (microseconds)
 
 float32 x		# in meters NED
 float32 y		# in meters NED
 float32 z		# in meters NED
-
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
 float32 vx		# in meters/sec
 float32 vy		# in meters/sec
 float32 vz		# in meters/sec
-
 float32[3] acceleration # in meters/sec^2
+float32[3] jerk # in meters/sec^3
 float32[3] thrust	# normalized thrust vector in NED
 
-float32 yaw		# in radians NED -PI..+PI
-float32 yawspeed	# in radians/sec
+# TOPICS vehicle_local_position_setpoint trajectory_setpoint

msg/vehicle_status.msg

@@ -1,3 +1,5 @@
+# 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
@@ -67,12 +69,13 @@ 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 (rotary wing/fixed-wing/rover/airship, see above)
+uint8 vehicle_type          # Type of vehicle (fixed-wing, rotary wing, ground)
                             # 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,6 +7,8 @@ uint8 VEHICLE_VTOL_STATE_FW = 4
 
 uint64 timestamp			# time since system start (microseconds)
 
-uint8 vehicle_vtol_state		# current state of the vtol, see VEHICLE_VTOL_STATE
-
+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
 bool vtol_transition_failsafe		# vtol in transition failsafe mode
+bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit c5c7d2b4f26f52f1dfb425ebde83328606b65d4f
+Subproject commit 22dbf796a3f61e5edbaae0e2716e1794eb10debe

src/drivers/drv_sensor.h

@@ -149,6 +149,9 @@
 #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
@@ -185,6 +188,7 @@
 #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
@@ -204,12 +208,6 @@
 
 #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,44 +455,29 @@ 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);
 			}
@@ -571,13 +556,6 @@ 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);
 
@@ -671,13 +649,6 @@ 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,7 +148,6 @@ 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-2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021 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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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 (config.drdy_gpio != 0) {
+	if (_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:
-		// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
+		// DEVICE_CONFIG: Software reset configuration
 		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::SIGNAL_PATH_RESET) == 0x00)
+		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
 		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
 
 			// Wakeup accel and gyro and schedule remaining configuration
@@ -190,19 +190,15 @@ void ICM42670P::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
+			uint32_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
-				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
-
-				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-					samples = _fifo_gyro_samples;
+				// 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);
 
 				} else {
-					perf_count(_drdy_missed_perf);
+					samples = _fifo_gyro_samples;
 				}
 
 				// push backup schedule back
@@ -213,6 +209,8 @@ 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);
@@ -224,17 +222,12 @@ 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);
 					}
 				}
 			}
@@ -242,9 +235,12 @@ void ICM42670P::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				if (FIFORead(timestamp_sample, samples)) {
+				// PX4_WARN("samples 2  = %d ",samples);
+				if (FIFORead(now, samples)) {
 					success = true;
 
+					// PX4_WARN("success  = %d ",success);
+
 					if (_failure_count > 0) {
 						_failure_count--;
 					}
@@ -264,11 +260,9 @@ 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
@@ -289,8 +283,68 @@ 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);
@@ -320,6 +374,8 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
 	}
 }
 
+
+
 bool ICM42670P::Configure()
 {
 	// first set and clear all configured register bits
@@ -327,9 +383,7 @@ bool ICM42670P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
-	for (const auto &reg_cfg : _register_mreg1_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+	Mreg1Config();
 
 	// now check that all are configured
 	bool success = true;
@@ -340,17 +394,11 @@ bool ICM42670P::Configure()
 		}
 	}
 
-	for (const auto &reg_cfg : _register_mreg1_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+	success = Mreg1Check();
 
-	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 
-	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-	_px4_gyro.set_range(math::radians(2000.f));
+	ConfigureAccel();
+	ConfigureGyro();
 
 	return success;
 }
@@ -363,8 +411,11 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM42670P::DataReady()
 {
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+	uint32_t expected = 0;
+
+	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
+		ScheduleNow();
+	}
 }
 
 bool ICM42670P::DataReadyInterruptConfigure()
@@ -394,19 +445,18 @@ 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;
 }
 
-uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
+template <typename T>
+uint8_t ICM42670P::RegisterRead(T reg)
 {
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
@@ -414,50 +464,14 @@ uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
 	return cmd[1];
 }
 
-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)
+template <typename T>
+void ICM42670P::RegisterWrite(T 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)
 {
@@ -484,10 +498,12 @@ 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) + 6, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -502,6 +518,7 @@ 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();
@@ -535,18 +552,6 @@ 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) {
@@ -558,6 +563,7 @@ 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);
@@ -571,30 +577,18 @@ 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_timestamp_sample.store(0);
+	_drdy_fifo_read_samples.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 = samples;
+	accel.samples = 0;
 	accel.dt = FIFO_SAMPLE_DT;
 
 	for (int i = 0; i < samples; i++) {
@@ -604,15 +598,18 @@ 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[i] = accel_x;
-		accel.y[i] = math::negate(accel_y);
-		accel.z[i] = math::negate(accel_z);
+		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++;
 	}
 
 	_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));
 
-	_px4_accel.updateFIFO(accel);
+	if (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+	}
 }
 
 void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -630,8 +627,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] = math::negate(gyro_y);
-		gyro.z[i] = math::negate(gyro_z);
+		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
 	}
 
 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -661,3 +658,88 @@ 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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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,7 +57,6 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
 public:
 	ICM42670P(const I2CSPIDriverConfig &config);
 	~ICM42670P() override;
-
 	static void print_usage();
 
 	void RunImpl();
@@ -69,12 +68,12 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f};     // 8000 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 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))};
+	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))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -86,8 +85,10 @@ private:
 		uint8_t FIFO_COUNTL{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
+
+
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -95,17 +96,13 @@ 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);
 
@@ -114,13 +111,9 @@ 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); }
@@ -133,6 +126,12 @@ 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;
@@ -150,7 +149,9 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{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};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -158,32 +159,26 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	} _state{STATE::RESET};
+	};
+
+	STATE _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	uint32_t _fifo_gyro_samples{static_cast<uint32_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::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 },
+		// 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 },
 	};
 };

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 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 = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint32_t SPI_SPEED = 12 * 1000 * 1000; // 24 MHz SPI
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x67;
@@ -66,13 +66,21 @@ namespace Register
 {
 
 enum class BANK_0 : uint8_t {
-	SIGNAL_PATH_RESET = 0x02,
+	DEVICE_CONFIG     = 0x01,
 
 	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,
@@ -82,93 +90,87 @@ 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 MREG1 : uint8_t {
-	FIFO_CONFIG5 = 0x01,
-
-	INT_CONFIG0  = 0x04,
+enum class MREG_1 : uint8_t {
+	FIFO_CONFIG5_MREG1      = 0x01,
+	INT_CONFIG0_MREG1       = 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, // 1: Software reset enabled
-	FIFO_FLUSH                = Bit2, // When set to 1, FIFO will get flushed
+	SOFT_RESET_DEVICE_CONFIG  = Bit4, //
+	FIFO_FLUSH                = Bit2,
 };
 
 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	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
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
 };
-
-// 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_UI_FILT_BW
-	GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
+
+	// 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
 };
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	// 2:0 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
-};
 
+	// 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
+};
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	// FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
-	FIFO_BYPASS            = Bit0, // 0: FIFO is not bypassed
+	// 1 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
 };
 
 // INT_STATUS
@@ -178,63 +180,113 @@ enum INT_STATUS_BIT : uint8_t {
 	FIFO_FULL_INT  = Bit1,
 };
 
-// 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
+// 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
 };
 
+// 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
 
-//---------------- USER BANK MREG1 Register bits
+	// 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
+};
 
 // FIFO_CONFIG5
 enum FIFO_CONFIG5_BIT : uint8_t {
-	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
+	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,
 };
 
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
+	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.
 };
 
 namespace FIFO
 {
-static constexpr size_t SIZE = 1024;
+static constexpr size_t SIZE = 2048;
 
 // 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; // 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]
+	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;
 };
 
 // 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, // 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_ACCEL           = Bit6,
+	HEADER_GYRO            = Bit5,
+	HEADER_20              = Bit4,
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	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
+	HEADER_ODR_ACCEL       = Bit1,
+	HEADER_ODR_GYRO        = Bit0,
 };
 
 }

src/drivers/power_monitor/ina231/CMakeLists.txt

@@ -1,44 +0,0 @@
-############################################################################
-#
-#   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

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

src/drivers/power_monitor/ina231/ina231.cpp

@@ -1,322 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,131 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,76 +0,0 @@
-/****************************************************************************
- *
- *   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-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 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,22 +50,6 @@ 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}
@@ -106,8 +90,6 @@ void PX4Gyroscope::set_scale(float scale)
 		}
 
 		_scale = scale;
-
-		UpdateClipLimit();
 	}
 }
 
@@ -125,9 +107,6 @@ 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();
 
@@ -166,17 +145,8 @@ 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,6 +1,7 @@
+
 /****************************************************************************
  *
- *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 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,7 +53,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; UpdateClipLimit(); }
+	void set_range(float range) { _range = range; }
 	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
@@ -63,8 +64,6 @@ 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)};
 
@@ -77,8 +76,6 @@ 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,25 +146,6 @@ 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,8 +513,7 @@ 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.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
+	if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
 		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,6 +38,28 @@
 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,
@@ -51,12 +73,13 @@ 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 == _arm_state) {
+	if (new_arming_state == current_arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
@@ -90,7 +113,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, !isArmed(),
+						status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
 						time_since_boot);
 
 				_last_preflight_check = hrt_absolute_time();
@@ -98,7 +121,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][_arm_state];
+		bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
 
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
@@ -106,7 +129,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 (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
+				if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
 
 					bool prearm_check_ret = true;
 
@@ -131,8 +154,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			status_flags.system_sensors_initialized = true;
 
 			/* recover from a prearm fail */
-			if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				_arm_state = vehicle_status_s::ARMING_STATE_STANDBY;
+			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+				status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
 			}
 
 			// HIL can always go to standby
@@ -143,7 +166,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 		if (!hil_enabled &&
 		    (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-		    (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
+		    (status.arming_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) {
@@ -154,20 +177,21 @@ 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;
 
-			// Record arm/disarm reason
-			if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition
+			if (was_armed && !armed.armed) { // disarm transition
 				status.latest_disarming_reason = (uint8_t)calling_reason;
 
-			} else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition
+			} else if (!was_armed && armed.armed) { // arm transition
 				status.latest_arming_reason = (uint8_t)calling_reason;
 			}
 
-			// Switch state
-			_arm_state = new_arming_state;
-
-			if (isArmed()) {
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 				status.armed_time = hrt_absolute_time();
 
 			} else {
@@ -181,58 +205,13 @@ 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",
-					     getArmStateName(_arm_state), getArmStateName(new_arming_state));
+					     arming_state_names[status.arming_state], arming_state_names[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}",
-				getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state));
+				eventArmingState(status.arming_state), eventArmingState(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,8 +52,6 @@ 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,
@@ -61,21 +59,19 @@ 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);
 
-	// 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); }
+	// 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",
+	};
 
 private:
-	static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state);
+	static inline events::px4::enums::arming_state_t eventArmingState(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,6 +44,7 @@ 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
@@ -65,6 +66,8 @@ 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
@@ -78,9 +81,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: identical states",
-			{ 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, 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,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED
 		},
 
 		// TRANSITION_CHANGED tests
@@ -89,97 +92,97 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"transition: init to standby",
-			{ 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, 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,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: init to standby error",
-			{ 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, 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,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: init to reboot",
-			{ 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, 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,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to init",
-			{ 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, 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_INIT,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to standby error",
-			{ 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, 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_ERROR,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to reboot",
-			{ 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, 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_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: armed to standby",
-			{ 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_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_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby error to reboot",
-			{ 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_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_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: in air restore to armed",
-			{ 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_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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: in air restore to reboot",
-			{ 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_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_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, 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}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_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,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		// Safety switch arming tests
 
 		{
 			"transition: standby to armed, no safety switch",
-			{ 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_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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		{
 			"transition: standby to armed, safety switch 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_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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
 		},
 
 		// TRANSITION_DENIED tests
@@ -188,51 +191,51 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: init to armed",
-			{ 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, 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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: armed to init",
-			{ 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_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_INIT,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: armed to reboot",
-			{ 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_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_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: standby error to armed",
-			{ 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_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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: standby error to standby",
-			{ 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_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,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: reboot to armed",
-			{ 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_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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		{
 			"no transition: in air restore to standby",
-			{ 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_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_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 
 		// Sensor tests
@@ -246,9 +249,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		{
 			"no transition: init to armed, safety switch 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, 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_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
+			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
 		},
 	};
 
@@ -265,7 +268,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		PreFlightCheck::arm_requirements_t arm_req{};
 
 		// Setup initial machine state
-		arm_state_machine.forceArmState(test->current_state.arming_state);
+		status.arming_state = 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
@@ -273,6 +276,9 @@ 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
@@ -291,7 +297,8 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		// Validate result of transition
 		EXPECT_EQ(result, test->expected_transition_result) << 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;
+		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;
 	}
 }

src/modules/commander/Commander.cpp

@@ -91,20 +91,6 @@ 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()
@@ -479,7 +465,7 @@ int Commander::custom_command(int argc, char *argv[])
 
 int Commander::print_status()
 {
-	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
+	PX4_INFO("arming: %s", _arm_state_machine.arming_state_names[_status.arming_state]);
 	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
 	perf_print_counter(_loop_perf);
 	perf_print_counter(_preflight_check_perf);
@@ -702,7 +688,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		run_preflight_checks = false;
 	}
 
-	if (run_preflight_checks && !_arm_state_machine.isArmed()) {
+	if (run_preflight_checks && !_armed.armed) {
 		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) {
@@ -829,6 +815,7 @@ 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;
@@ -840,7 +827,7 @@ Commander::Commander() :
 	_status_flags.rc_calibration_valid = true;
 
 	// default for vtol is rotary wing
-	_vtol_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+	_vtol_status.vtol_in_rw_mode = true;
 
 	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
 	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
@@ -1026,13 +1013,12 @@ 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);
 
-				// 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);
+				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;
+					}
 				}
 
 				transition_result_t arming_res = TRANSITION_DENIED;
@@ -1386,7 +1372,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
+			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1505,7 +1492,9 @@ 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 (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
+			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			    || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
+			    || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1540,7 +1529,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
+			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN
+			    || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1609,7 +1600,6 @@ 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;
 
@@ -1631,7 +1621,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 unsigned
 Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 {
-	if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
+	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
 		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
@@ -1679,7 +1669,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 unsigned
 Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
 {
-	if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
+	if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
 		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
@@ -1759,7 +1749,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 (_arm_state_machine.isArmed()) {
+		if (_armed.armed) {
 			disarm(arm_disarm_reason);
 
 		} else {
@@ -1804,7 +1794,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
-		    && !_arm_state_machine.isArmed() && (_internal_state.main_state_changes == 0)
+		    && !_armed.armed && (_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;
@@ -2093,10 +2083,8 @@ 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.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_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_ground = is_ground_rover(_status);
 
 	/* disable manual override for all systems that rely on electronic stabilization */
@@ -2136,6 +2124,8 @@ void Commander::updateParameters()
 void
 Commander::run()
 {
+	bool sensor_fail_tune_played = false;
+
 	/* initialize */
 	led_init();
 	buzzer_init();
@@ -2179,9 +2169,6 @@ 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();
 
@@ -2191,7 +2178,7 @@ Commander::run()
 			_parameter_update_sub.copy(&update);
 
 			/* update parameters */
-			if (!_arm_state_machine.isArmed()) {
+			if (!_armed.armed) {
 				updateParameters();
 
 				_status_changed = true;
@@ -2245,7 +2232,7 @@ Commander::run()
 			_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
 
 			// Only take actions if armed
-			if (_arm_state_machine.isArmed()) {
+			if (_armed.armed) {
 				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");
@@ -2289,7 +2276,7 @@ Commander::run()
 						 _safety.safety_switch_available, _status);
 
 				// disarm if safety is now on and still armed
-				if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off
+				if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off
 				    && (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
 					disarm(arm_disarm_reason_t::safety_button);
 				}
@@ -2313,33 +2300,30 @@ 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 (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;
+				// 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;
 
 				if (new_vehicle_type != _status.vehicle_type) {
-					_status.vehicle_type = new_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_changed = true;
 				}
 
-				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;
+				if (_status.in_transition_mode != _vtol_status.vtol_in_trans_mode) {
+					_status.in_transition_mode = _vtol_status.vtol_in_trans_mode;
 					_status_changed = true;
 				}
 
-				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);
+				if (_status.in_transition_to_fw != _vtol_status.in_transition_to_fw) {
+					_status.in_transition_to_fw = _vtol_status.in_transition_to_fw;
 					_status_changed = true;
 				}
 
@@ -2388,7 +2372,7 @@ Commander::run()
 		estimator_check();
 
 		// Auto disarm when landed or kill switch engaged
-		if (_arm_state_machine.isArmed()) {
+		if (_armed.armed) {
 
 			// Check for auto-disarm on landing or pre-flight
 			if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
@@ -2451,7 +2435,7 @@ Commander::run()
 		}
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
+		if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 
 			_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 					vehicle_status_s::ARMING_STATE_STANDBY, _armed,
@@ -2506,7 +2490,7 @@ Commander::run()
 			}
 
 			// Transition main state to loiter or auto-mission after takeoff is completed.
-			if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
+			if (_armed.armed && !_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)
@@ -2530,7 +2514,7 @@ Commander::run()
 		const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
 
 		// Geofence actions
-		if (_arm_state_machine.isArmed()
+		if (_armed.armed
 		    && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
 		    && !in_low_battery_failsafe_delay) {
 
@@ -2630,7 +2614,7 @@ Commander::run()
 		}
 
 		/* Check for mission flight termination */
-		if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination &&
+		if (_armed.armed && _mission_result_sub.get().flight_termination &&
 		    !_status_flags.circuit_breaker_flight_termination_disabled) {
 
 
@@ -2680,8 +2664,7 @@ Commander::run()
 
 				const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
 
-				if (!_arm_state_machine.isArmed() && (is_mavlink || !_mode_switch_mapped)
-				    && (_internal_state.main_state_changes == 0)) {
+				if (!_armed.armed && (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++;
@@ -2715,7 +2698,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
-			    && _arm_state_machine.isArmed()
+			    && _armed.armed
 			    && !_status_flags.rc_calibration_in_progress
 			    && manual_control_setpoint.valid
 			    && manual_control_setpoint.sticks_moving
@@ -2752,7 +2735,7 @@ Commander::run()
 		avoidance_check();
 
 		/* check if we are disarmed and there is a better mode to wait in */
-		if (!_arm_state_machine.isArmed()) {
+		if (!_armed.armed) {
 			/* 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.
@@ -2802,7 +2785,7 @@ Commander::run()
 			auto fd_status_flags = _failure_detector.getStatusFlags();
 			_status_changed = true;
 
-			if (_arm_state_machine.isArmed()) {
+			if (_armed.armed) {
 				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) {
@@ -2929,7 +2912,7 @@ Commander::run()
 			checkWindSpeedThresholds();
 		}
 
-		_status_flags.flight_terminated = _armed.force_failsafe || _armed.manual_lockdown;
+		_status_flags.flight_terminated = _armed.force_failsafe || _armed.lockdown || _armed.manual_lockdown;
 
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
@@ -2954,7 +2937,7 @@ Commander::run()
 
 		// automatically set or update home position
 		if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
-			if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) {
+			if (!_armed.armed && _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));
@@ -2970,10 +2953,10 @@ Commander::run()
 		}
 
 		// check for arming state change
-		if (_was_armed != _arm_state_machine.isArmed()) {
+		if (_was_armed != _armed.armed) {
 			_status_changed = true;
 
-			if (_arm_state_machine.isArmed()) {
+			if (_armed.armed) {
 				if (!_vehicle_land_detected.landed) { // check if takeoff already detected upon arming
 					_have_taken_off_since_arming = true;
 				}
@@ -2987,7 +2970,7 @@ Commander::run()
 			}
 		}
 
-		if (!_arm_state_machine.isArmed()) {
+		if (!_armed.armed) {
 			/* Reset the flag if disarmed. */
 			_have_taken_off_since_arming = false;
 			_imbalanced_propeller_check_triggered = false;
@@ -3030,49 +3013,54 @@ 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) {
 
-		// prearm mode
-		switch ((PrearmedMode)_param_com_prearm_mode.get()) {
-		case PrearmedMode::DISABLED:
-			/* skip prearmed state  */
-			_armed.prearmed = false;
-			break;
+			update_control_mode();
 
-		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;
+			_status.timestamp = hrt_absolute_time();
+			_status_pub.publish(_status);
 
-		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 */
+			switch ((PrearmedMode)_param_com_prearm_mode.get()) {
+			case PrearmedMode::DISABLED:
+				/* skip prearmed state  */
 				_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;
 			}
 
-			break;
+			_armed.timestamp = hrt_absolute_time();
+			_armed_pub.publish(_armed);
 
-		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);
+			/* publish internal state for logging purposes */
+			_internal_state.timestamp = hrt_absolute_time();
+			_commander_state_pub.publish(_internal_state);
 
+			// 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));
@@ -3084,34 +3072,17 @@ Commander::run()
 				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req,
 							_status, false);
 
-				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);
+				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
+						 && prearm_check_res), _status);
 			}
 
-			// 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)
+			/* publish vehicle_status_flags */
 			_status_flags.timestamp = hrt_absolute_time();
 			_vehicle_status_flags_pub.publish(_status_flags);
 
-			// commander_state publish internal state for logging purposes
-			_internal_state.timestamp = hrt_absolute_time();
-			_commander_state_pub.publish(_internal_state);
-
-			// failure_detector_status publish
+			/* publish failure_detector data */
 			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;
@@ -3122,12 +3093,11 @@ 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 && _arm_state_machine.isArmed() &&
+		if (!_arm_tune_played && _armed.armed &&
 		    (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
 
 			/* play tune when armed */
@@ -3145,7 +3115,7 @@ Commander::run()
 			/* play tune on battery warning */
 			set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
 
-		} else if (_status.failsafe && _arm_state_machine.isArmed()) {
+		} else if (_status.failsafe && _armed.armed) {
 			tune_failsafe(true);
 
 		} else {
@@ -3153,7 +3123,7 @@ Commander::run()
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (!_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) {
+		if (!_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
 
 			// Notify the user that it is safe to approach the vehicle
 			if (_arm_tune_played) {
@@ -3164,10 +3134,14 @@ Commander::run()
 		}
 
 		/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
-		if (!_status_flags.system_sensors_initialized &&
-		    !vehicle_status_flags_prev.system_hotplug_timeout && _status_flags.system_hotplug_timeout) {
+		_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)) {
 
 			set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING);
+			sensor_fail_tune_played = true;
+			_status_changed = true;
 		}
 
 		// check if the worker has finished
@@ -3196,11 +3170,11 @@ Commander::run()
 		_last_local_position_valid = _status_flags.local_position_valid;
 		_last_global_position_valid = _status_flags.global_position_valid;
 
-		_was_armed = _arm_state_machine.isArmed();
+		_was_armed = _armed.armed;
 
 		arm_auth_update(now, params_updated);
 
-		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
+		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
 
 		perf_end(_loop_perf);
 
@@ -3276,14 +3250,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 = _arm_state_machine.isArmed() ? 1_ms : 250_ms;
+		uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 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 (_arm_state_machine.isArmed()) {
+		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			led_mode = led_control_s::MODE_ON;
 			set_normal_color = true;
 
@@ -3291,7 +3265,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 (_arm_state_machine.isStandby()) {
+		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			led_mode = led_control_s::MODE_BREATHE;
 			set_normal_color = true;
 
@@ -3299,7 +3273,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 (_arm_state_machine.isInit()) {
+		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			// if in init status it should not be in the error state
 			led_mode = led_control_s::MODE_OFF;
 
@@ -3339,7 +3313,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 (_arm_state_machine.isArmed()) {
+	if (_armed.armed) {
 		if (_status.failsafe) {
 			BOARD_ARMED_LED_OFF();
 
@@ -3355,7 +3329,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			BOARD_ARMED_LED_ON();
 		}
 
-	} else if (_arm_state_machine.isStandby()) {
+	} else if (_armed.ready_to_arm) {
 		BOARD_ARMED_LED_OFF();
 
 		// ready to arm, blink at 1Hz
@@ -3430,7 +3404,7 @@ Commander::update_control_mode()
 	_vehicle_control_mode = {};
 
 	/* set vehicle_control_mode according to set_navigation_state */
-	_vehicle_control_mode.flag_armed = _arm_state_machine.isArmed();
+	_vehicle_control_mode.flag_armed = _armed.armed;
 
 	switch (_status.nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
@@ -3562,7 +3536,11 @@ Commander::update_control_mode()
 bool
 Commander::stabilization_required()
 {
-	return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	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
 }
 
 void
@@ -3579,7 +3557,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(_arm_state_machine.isArmed());
+		tune_negative(_armed.armed);
 
 		_last_print_mode_reject_time = hrt_absolute_time();
 	}
@@ -3711,7 +3689,7 @@ void Commander::data_link_check()
 						events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
 					}
 
-					if (!_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) {
+					if (!_armed.armed && !_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));
@@ -3885,7 +3863,7 @@ void Commander::battery_status_check()
 			battery_required_count++;
 		}
 
-		if (_arm_state_machine.isArmed()) {
+		if (_armed.armed) {
 
 			if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
 				mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
@@ -3931,11 +3909,9 @@ 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)),
-									      _arm_state_machine.isArmed() ? "Land now!" : "");
+									      battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)),  _armed.armed ? "Land now!" : "");
 
-							events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ?
-									events::px4::enums::suggested_action_t::land :
+							events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
 									events::px4::enums::suggested_action_t::none;
 
 							/* EVENT
@@ -3969,7 +3945,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
-	    && _arm_state_machine.isArmed()
+	    && _armed.armed
 	    && !_vehicle_land_detected.ground_contact // not in any landing stage
 	    && !_rtl_time_actions_done
 	    && PX4_ISFINITE(worst_battery_time_s)
@@ -3995,7 +3971,7 @@ void Commander::battery_status_check()
 	bool battery_warning_level_increased_while_armed = false;
 	bool update_internal_battery_state = false;
 
-	if (_arm_state_machine.isArmed()) {
+	if (_armed.armed) {
 		if (worst_warning > _battery_warning) {
 			battery_warning_level_increased_while_armed = true;
 			update_internal_battery_state = true;
@@ -4168,7 +4144,7 @@ void Commander::estimator_check()
 
 			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-				if (!_arm_state_machine.isArmed()) {
+				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 					_nav_test_failed = false;
 					_nav_test_passed = false;
 
@@ -4232,7 +4208,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 (!_arm_state_machine.isArmed()) {
+		if (!_armed.armed) {
 			if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
 				xy_valid = false;
 			}
@@ -4423,8 +4399,7 @@ 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 = _arm_state_machine.isArmed() ?
-							events::px4::enums::suggested_action_t::land :
+					events::px4::enums::suggested_action_t action = _armed.armed ? 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"),
@@ -4432,8 +4407,7 @@ void Commander::esc_status_check()
 				}
 			}
 
-			mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg,
-					     _arm_state_machine.isArmed() ? "Land now!" : "");
+			mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
 
 			_last_esc_online_flags = esc_status.esc_online_flags;
 			_status_flags.escs_error = true;
@@ -4467,7 +4441,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), _arm_state_machine.isArmed() ? user_action : "");
+								      esc_fault_reason_str(fault_reason_index),  _armed.armed ? 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, 5.f);
+PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 10.0f);
 
 /**
  * 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,6 +100,7 @@ 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,81 +56,26 @@ using matrix::Vector2f;
 using matrix::Vector3f;
 using matrix::wrap_pi;
 
-// 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
+enum class velocity_frame_t : uint8_t {
+	LOCAL_FRAME_FRD,
+	BODY_FRAME_FRD
 };
 
-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 {
+struct gps_message {
 	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
 };
 
@@ -206,7 +151,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)
-	VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
+	velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
 	uint8_t     reset_counter{};
 };
 
@@ -232,13 +177,61 @@ 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{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 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 terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
@@ -508,7 +501,7 @@ union filter_control_status_u {
 };
 
 // Mavlink bitmask containing state of estimator solution
-union ekf_solution_status_u {
+union ekf_solution_status {
 	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 & 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) {
+		if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS)
+				|| (_params.fusion_mode & MASK_USE_EVVEL)) && !_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 & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) {
+				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
 					startEvPosFusion();
 				}
 
 				// turn on use of external vision measurements for velocity
-				if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) {
+				if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
 					startEvVelFusion();
 				}
 			}
 		}
 
 		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw
+		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_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 & SensorFusionMask::USE_GPS) {
+			if (_params.fusion_mode & MASK_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 & SensorFusionMask::ROTATE_EXT_VIS) {
+				if (_params.fusion_mode & MASK_ROTATE_EV) {
 					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 & SensorFusionMask::USE_OPT_FLOW)
+			if (!(_params.fusion_mode & MASK_USE_OF)
 			    || _inhibit_flow_use) {
 
 				stopFlowFusion();
@@ -480,11 +480,11 @@ void Ekf::controlOpticalFlowFusion()
 		}
 
 		// optical flow fusion mode selection logic
-		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
+		if ((_params.fusion_mode & MASK_USE_OF) // 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 == MagFuseType::NONE) {
+			if (_control_status.flags.yaw_align || _params.mag_fusion_type == MAG_FUSE_TYPE_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 & SensorFusionMask::USE_GPS_YAW)
+	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
 	    || _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 VerticalHeightSensor::BARO:
+	case VDIST_SENSOR_BARO:
 		if (do_range_aid) {
 			if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
 				startRngAidHgtFusion();
@@ -862,7 +862,7 @@ void Ekf::controlHeightFusion()
 
 		break;
 
-	case VerticalHeightSensor::RANGE:
+	case VDIST_SENSOR_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 VerticalHeightSensor::GPS:
+	case VDIST_SENSOR_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 VerticalHeightSensor::EV:
+	case VDIST_SENSOR_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 & SensorFusionMask::USE_DRAG))) {
+	if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
 		_control_status.flags.wind = false;
 	}
 
@@ -1053,7 +1053,7 @@ void Ekf::controlBetaFusion()
 
 void Ekf::controlDragFusion()
 {
-	if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
+	if ((_params.fusion_mode & MASK_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 & SensorFusionMask::INHIBIT_ACC_BIAS)
+	const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_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 <= MagFuseType::MAG_3D) {
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_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 <= MagFuseType::MAG_3D) {
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_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 gpsMessage &gps) override;
+	bool collect_gps(const gps_message &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 gpsMessage &gps);
+	bool gps_is_good(const gps_message &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 & SensorFusionMask::ROTATE_EXT_VIS) {
+	if (_params.fusion_mode & MASK_ROTATE_EV) {
 		_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 <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
+	if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_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 == MagFuseType::INDOOR) {
+	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_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 & GeoDeclinationMask::USE_GEO_DECL) {
+	} else if (_params.mag_declination_source & MASK_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_u soln_status;
+	ekf_solution_status 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 VelocityFrame::BODY_FRAME_FRD:
+	case velocity_frame_t::BODY_FRAME_FRD:
 		vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
 		break;
 
-	case VelocityFrame::LOCAL_FRAME_FRD:
+	case velocity_frame_t::LOCAL_FRAME_FRD:
 		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
+		if (_params.fusion_mode & MASK_ROTATE_EV) {
 			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 VelocityFrame::BODY_FRAME_FRD:
+	case velocity_frame_t::BODY_FRAME_FRD:
 		ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
 		break;
 
-	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) {
+	case velocity_frame_t::LOCAL_FRAME_FRD:
+		if (_params.fusion_mode & MASK_ROTATE_EV) {
 			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 gpsMessage &gps)
+void EstimatorInterface::setGpsData(const gps_message &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 & SensorFusionMask::USE_DRAG)) {
+	if ((_params.fusion_mode & MASK_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 != MagFuseType::NONE) {
+	if (_params.mag_fusion_type != MAG_FUSE_TYPE_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 == VerticalHeightSensor::RANGE)) {
+	if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
+	if (_params.fusion_mode & MASK_USE_GPS) {
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
+	if (_params.fusion_mode & MASK_USE_OF) {
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
 
-	if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) {
+	if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
 		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 gpsMessage &gps) = 0;
+	virtual bool collect_gps(const gps_message &gps) = 0;
 
 	void setIMUData(const imuSample &imu_sample);
 
 	void setMagData(const magSample &mag_sample);
 
-	void setGpsData(const gpsMessage &gps);
+	void setGpsData(const gps_message &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 & GeoDeclinationMask::SAVE_GEO_DECL)) {
+		if (_NED_origin_initialised && (_params.mag_declination_source & MASK_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 gpsMessage &gps)
+bool Ekf::collect_gps(const gps_message &gps)
 {
 	// Run GPS checks always
 	_gps_checks_passed = gps_is_good(gps);
@@ -92,7 +92,7 @@ bool Ekf::collect_gps(const gpsMessage &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 != MagFuseType::NONE)
+		if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE)
 		     && !declination_was_valid) {
 			_mag_yaw_reset_req = true;
 		}
@@ -120,7 +120,7 @@ bool Ekf::collect_gps(const gpsMessage &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 != MagFuseType::NONE) {
+			if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
 				if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
 					_mag_yaw_reset_req = true;
 				}
@@ -141,7 +141,7 @@ bool Ekf::collect_gps(const gpsMessage &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 gpsMessage &gps)
+bool Ekf::gps_is_good(const gps_message &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 & SensorFusionMask::USE_GPS)) {
+	if (!(_params.fusion_mode & MASK_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 & SensorFusionMask::ROTATE_EXT_VIS)) {
+					if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
 						resetHorizontalPositionToVision();
 					}
 				}
@@ -136,7 +136,7 @@ void Ekf::controlGpsFusion()
 					startGpsFusion();
 				}
 
-			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
+			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_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 & GeoDeclinationMask::USE_GEO_DECL)
+			if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_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 >= MagFuseType::NONE
+	if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
 	    || _control_status.flags.mag_fault
 	    || !_control_status.flags.tilt_align) {
 
@@ -111,18 +111,18 @@ void Ekf::controlMagFusion()
 		default:
 
 		/* fallthrough */
-		case MagFuseType::AUTO:
+		case MAG_FUSE_TYPE_AUTO:
 			selectMagAuto();
 			break;
 
-		case MagFuseType::INDOOR:
+		case MAG_FUSE_TYPE_INDOOR:
 
 		/* fallthrough */
-		case MagFuseType::HEADING:
+		case MAG_FUSE_TYPE_HEADING:
 			startMagHdgFusion();
 			break;
 
-		case MagFuseType::MAG_3D:
+		case MAG_FUSE_TYPE_3D:
 			startMag3DFusion();
 			break;
 		}
@@ -188,7 +188,7 @@ void Ekf::runOnGroundYawReset()
 
 bool Ekf::canResetMagHeading() const
 {
-	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MagFuseType::NONE);
+	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_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 & GeoDeclinationMask::FUSE_DECL);
+	const bool user_selected = (_params.mag_declination_source & MASK_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 == MagFuseType::INDOOR);
+	const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_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 <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) {
+	    && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_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 & GeoDeclinationMask::USE_GEO_DECL) {
+	if (_params.mag_declination_source & MASK_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 != MagFuseType::NONE) {
+		if (_params->mag_fusion_type != MAG_FUSE_TYPE_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() & SensorFusionMask::INHIBIT_ACC_BIAS)) {
+		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_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 = VelocityFrame::BODY_FRAME_FRD;
+				ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
 
 			} else {
-				ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+				ev_data.vel_frame = velocity_frame_t::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);
 		}
 
-		gpsMessage gps_msg{
+		gps_message gps_msg{
 			.time_usec = vehicle_gps_position.timestamp,
 			.lat = vehicle_gps_position.lat,
 			.lon = vehicle_gps_position.lon,
@@ -1878,7 +1878,10 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 
 void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 {
-	if (_param_ekf2_aid_mask.get() & SensorFusionMask::INHIBIT_ACC_BIAS) {
+	_accel_cal.cal_available = true;
+	return;
+
+	if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
 		_accel_cal.cal_available = false;
 		return;
 	}
@@ -1918,6 +1921,9 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 {
+	_gyro_cal.cal_available = true;
+	return;
+
 	static constexpr float max_var_allowed = 1e-3f;
 	static constexpr float max_var_ratio = 1e2f;
 
@@ -1948,6 +1954,9 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
+	_mag_cal.cal_available = true;
+	return;
+
 	// Check if conditions are OK for learning of magnetometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 

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

@@ -12,7 +12,7 @@ EkfWrapper::~EkfWrapper()
 
 void EkfWrapper::setBaroHeight()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO;
 }
 
 bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -22,7 +22,7 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
 
 void EkfWrapper::setGpsHeight()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS;
 }
 
 bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -32,7 +32,7 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
 
 void EkfWrapper::setRangeHeight()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE;
 }
 
 bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -42,7 +42,7 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
 
 void EkfWrapper::setVisionHeight()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_EV;
 }
 
 bool EkfWrapper::isIntendingVisionHeightFusion() const
@@ -52,12 +52,12 @@ bool EkfWrapper::isIntendingVisionHeightFusion() const
 
 void EkfWrapper::enableGpsFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS;
+	_ekf_params->fusion_mode |= MASK_USE_GPS;
 }
 
 void EkfWrapper::disableGpsFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS;
+	_ekf_params->fusion_mode &= ~MASK_USE_GPS;
 }
 
 bool EkfWrapper::isIntendingGpsFusion() const
@@ -67,12 +67,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
 
 void EkfWrapper::enableGpsHeadingFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_YAW;
+	_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
 }
 
 void EkfWrapper::disableGpsHeadingFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_YAW;
+	_ekf_params->fusion_mode &= ~MASK_USE_GPSYAW;
 }
 
 bool EkfWrapper::isIntendingGpsHeadingFusion() const
@@ -82,12 +82,12 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const
 
 void EkfWrapper::enableFlowFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_OPT_FLOW;
+	_ekf_params->fusion_mode |= MASK_USE_OF;
 }
 
 void EkfWrapper::disableFlowFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_OPT_FLOW;
+	_ekf_params->fusion_mode &= ~MASK_USE_OF;
 }
 
 bool EkfWrapper::isIntendingFlowFusion() const
@@ -102,12 +102,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
 
 void EkfWrapper::enableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
+	_ekf_params->fusion_mode |= MASK_USE_EVPOS;
 }
 
 void EkfWrapper::disableExternalVisionPositionFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
+	_ekf_params->fusion_mode &= ~MASK_USE_EVPOS;
 }
 
 bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@@ -117,12 +117,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
 
 void EkfWrapper::enableExternalVisionVelocityFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_VEL;
+	_ekf_params->fusion_mode |= MASK_USE_EVVEL;
 }
 
 void EkfWrapper::disableExternalVisionVelocityFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_VEL;
+	_ekf_params->fusion_mode &= ~MASK_USE_EVVEL;
 }
 
 bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
@@ -132,12 +132,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
 
 void EkfWrapper::enableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
+	_ekf_params->fusion_mode |= MASK_USE_EVYAW;
 }
 
 void EkfWrapper::disableExternalVisionHeadingFusion()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
+	_ekf_params->fusion_mode &= ~MASK_USE_EVYAW;
 }
 
 bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@@ -147,12 +147,12 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
 
 void EkfWrapper::enableExternalVisionAlignment()
 {
-	_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
+	_ekf_params->fusion_mode |= MASK_ROTATE_EV;
 }
 
 void EkfWrapper::disableExternalVisionAlignment()
 {
-	_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
+	_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
 }
 
 bool EkfWrapper::isIntendingMagHeadingFusion() const
@@ -167,7 +167,7 @@ bool EkfWrapper::isIntendingMag3DFusion() const
 
 void EkfWrapper::setMagFuseTypeNone()
 {
-	_ekf_params->mag_fusion_type = MagFuseType::NONE;
+	_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_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 gpsMessage &gps)
+void Gps::setData(const gps_message &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);
 }
 
-gpsMessage Gps::getDefaultGpsData()
+gps_message Gps::getDefaultGpsData()
 {
-	gpsMessage gps_data{};
+	gps_message 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 gpsMessage &gps);
+	void setData(const gps_message &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);
 
-	gpsMessage getDefaultGpsData();
+	gps_message getDefaultGpsData();
 
 private:
 	void send(uint64_t time) override;
 
-	gpsMessage _gps_data{};
+	gps_message _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 = VelocityFrame::BODY_FRAME_FRD;
+	_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
 }
 
 void Vio::setVelocityFrameToLocal()
 {
-	_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
+	_vio_data.vel_frame = velocity_frame_t::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 = VelocityFrame::LOCAL_FRAME_FRD;
+	vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
 	return vio_data;
 }
 

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -459,12 +459,12 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_current_task.task->setYawHandler(_wv_controller);
 
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
-	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
+	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;
 
 	if (_current_task.task->updateInitialize() && _current_task.task->update()) {
 		// setpoints and constraints for the position controller from flighttask
-		setpoint = _current_task.task->getTrajectorySetpoint();
+		setpoint = _current_task.task->getPositionSetpoint();
 		constraints = _current_task.task->getConstraints();
 	}
 
@@ -504,7 +504,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_old_landing_gear_position = landing_gear.landing_gear;
 }
 
-void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
+void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
 				      const vehicle_local_position_s &vehicle_local_position)
 {
 	if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
@@ -518,8 +518,8 @@ void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
 
 	if (vehicle_local_position.z < min_z) {
 		// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
-		setpoint.position[2] = min_z;
-		setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f);
+		setpoint.z = min_z;
+		setpoint.vz = math::max(setpoint.vz, 0.f);
 	}
 }
 
@@ -531,11 +531,11 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	// Save current setpoints for the next FlightTask
-	trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
+	vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint;
 	ekf_reset_counters_s last_reset_counters{};
 
 	if (isAnyTaskActive()) {
-		last_setpoint = _current_task.task->getTrajectorySetpoint();
+		last_setpoint = _current_task.task->getPositionSetpoint();
 		last_reset_counters = _current_task.task->getResetCounters();
 	}
 

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -47,7 +47,6 @@
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/takeoff_status.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
@@ -93,7 +92,7 @@ private:
 	void send_vehicle_cmd_do(uint8_t nav_state);
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
-	void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
+	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
 
 	/**
 	 * Switch to a specific task (for normal usage)
@@ -153,7 +152,7 @@ private:
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
-	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() :
 
 }
 
-bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 
@@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 	_velocity_setpoint = _velocity;
 	_position_setpoint = _position;
 
-	Vector3f vel_prev{last_setpoint.velocity};
-	Vector3f pos_prev{last_setpoint.position};
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
 	Vector3f accel_prev{last_setpoint.acceleration};
 
 	for (int i = 0; i < 3; i++) {

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -87,7 +87,7 @@ public:
 	FlightTaskAuto();
 
 	virtual ~FlightTaskAuto() = default;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskDescend.hpp"
 
-bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp

@@ -46,7 +46,7 @@ public:
 	virtual ~FlightTaskDescend() = default;
 
 	bool update() override;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskFailsafe.hpp"
 
-bool FlightTaskFailsafe::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;

src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -47,7 +47,7 @@ public:
 
 	virtual ~FlightTaskFailsafe() = default;
 	bool update() override;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -4,12 +4,12 @@
 
 constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
-const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
+const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
 
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
-bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
@@ -21,8 +21,8 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 void FlightTask::reActivate()
 {
 	// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
-	trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
-	setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
+	vehicle_local_position_setpoint_s setpoint_preserve_vertical{empty_setpoint};
+	setpoint_preserve_vertical.vz = _velocity_setpoint(2);
 	activate(setpoint_preserve_vertical);
 }
 
@@ -76,20 +76,30 @@ void FlightTask::_checkEkfResetCounters()
 	}
 }
 
-const trajectory_setpoint_s FlightTask::getTrajectorySetpoint()
+const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 {
-	trajectory_setpoint_s trajectory_setpoint{};
-	trajectory_setpoint.timestamp = hrt_absolute_time();
+	/* fill position setpoint message */
+	vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
+	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
 
-	_position_setpoint.copyTo(trajectory_setpoint.position);
-	_velocity_setpoint.copyTo(trajectory_setpoint.velocity);
-	_acceleration_setpoint.copyTo(trajectory_setpoint.acceleration);
-	_jerk_setpoint.copyTo(trajectory_setpoint.jerk);
+	vehicle_local_position_setpoint.x = _position_setpoint(0);
+	vehicle_local_position_setpoint.y = _position_setpoint(1);
+	vehicle_local_position_setpoint.z = _position_setpoint(2);
 
-	trajectory_setpoint.yaw = _yaw_setpoint;
-	trajectory_setpoint.yawspeed = _yawspeed_setpoint;
+	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
+	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
+	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
 
-	return trajectory_setpoint;
+	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+
+	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
+	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
+
+	// deprecated, only kept for output logging
+	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
+	return vehicle_local_position_setpoint;
 }
 
 void FlightTask::_resetSetpoints()

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -46,7 +46,6 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/landing_gear.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -82,7 +81,7 @@ public:
 	 * @param last_setpoint last output of the previous task
 	 * @return true on success, false on error
 	 */
-	virtual bool activate(const trajectory_setpoint_s &last_setpoint);
+	virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
 
 	/**
 	 * Call this to reset an active Flight Task
@@ -113,7 +112,7 @@ public:
 	 * Get the output data
 	 * @return task output setpoints that get executed by the positon controller
 	 */
-	const trajectory_setpoint_s getTrajectorySetpoint();
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
 	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
 	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
@@ -142,7 +141,7 @@ public:
 	 * Empty setpoint.
 	 * All setpoints are set to NAN.
 	 */
-	static const trajectory_setpoint_s empty_setpoint;
+	static const vehicle_local_position_setpoint_s empty_setpoint;
 
 	/**
 	 * Empty constraints.

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -43,21 +43,21 @@ FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
 	_stick_acceleration_xy(this)
 {};
 
-bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
 
 	_stick_acceleration_xy.resetPosition();
 
-	if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) {
-		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity));
+	if (PX4_ISFINITE(last_setpoint.vx) && PX4_ISFINITE(last_setpoint.vy)) {
+		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.vx, last_setpoint.vy));
 
 	} else {
 		_stick_acceleration_xy.resetVelocity(_velocity.xy());
 	}
 
 	if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) {
-		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
+		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
 	}
 
 	return ret;

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -49,7 +49,7 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 public:
 	FlightTaskManualAcceleration();
 	virtual ~FlightTaskManualAcceleration() = default;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
 	/**

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -60,7 +60,7 @@ bool FlightTaskManualAltitude::updateInitialize()
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
-bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_yaw_setpoint = NAN;

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -49,7 +49,7 @@ class FlightTaskManualAltitude : public FlightTask
 public:
 	FlightTaskManualAltitude();
 	virtual ~FlightTaskManualAltitude() = default;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
 

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -41,17 +41,17 @@
 
 using namespace matrix;
 
-bool FlightTaskManualAltitudeSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 
 	// Check if the previous FlightTask provided setpoints
 
 	// If the position setpoint is unknown, set to the current postion
-	float z_sp_last = PX4_ISFINITE(last_setpoint.position[2]) ? last_setpoint.position[2] : _position(2);
+	float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
 
 	// If the velocity setpoint is unknown, set to the current velocity
-	float vz_sp_last = PX4_ISFINITE(last_setpoint.velocity[2]) ? last_setpoint.velocity[2] : _velocity(2);
+	float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
 
 	// No acceleration estimate available, set to zero if the setpoint is NAN
 	float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -48,7 +48,7 @@ public:
 	FlightTaskManualAltitudeSmoothVel() = default;
 	virtual ~FlightTaskManualAltitudeSmoothVel() = default;
 
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 
 protected:
 	virtual void _updateSetpoints() override;

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -49,7 +49,7 @@ public:
 	FlightTaskManualPosition();
 
 	virtual ~FlightTaskManualPosition() = default;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
 	/**

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -38,14 +38,14 @@
 
 using namespace matrix;
 
-bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualPosition::activate(last_setpoint);
 
 	// Check if the previous FlightTask provided setpoints
 	Vector3f accel_prev{last_setpoint.acceleration};
-	Vector3f vel_prev{last_setpoint.velocity};
-	Vector3f pos_prev{last_setpoint.position};
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
 
 	for (int i = 0; i < 3; i++) {
 		// If the position setpoint is unknown, set to the current postion

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -53,7 +53,7 @@ public:
 
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 
 protected:

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -158,7 +158,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 	}
 }
 
-bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 	_orbit_radius = _radius_min;
@@ -176,8 +176,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 	      && PX4_ISFINITE(_velocity(1))
 	      && PX4_ISFINITE(_velocity(2));
 
-	Vector3f pos_prev{last_setpoint.position};
-	Vector3f vel_prev{last_setpoint.velocity};
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
 	Vector3f accel_prev{last_setpoint.acceleration};
 
 	for (int i = 0; i < 3; i++) {

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -57,7 +57,7 @@ public:
 	virtual ~FlightTaskOrbit() = default;
 
 	bool applyCommandParameters(const vehicle_command_s &command) override;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
 protected:

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp

@@ -70,14 +70,14 @@ void FlightTaskTransition::updateParameters()
 	}
 }
 
-bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
+bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 
 	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
 
-	if (PX4_ISFINITE(last_setpoint.velocity[2])) {
-		_vel_z_filter.reset(last_setpoint.velocity[2]);
+	if (PX4_ISFINITE(last_setpoint.vz)) {
+		_vel_z_filter.reset(last_setpoint.vz);
 
 	} else {
 		_vel_z_filter.reset(_velocity(2));

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp

@@ -54,7 +54,7 @@ public:
 	FlightTaskTransition();
 
 	virtual ~FlightTaskTransition() = default;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
 

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

@@ -67,10 +67,6 @@ 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,6 +33,8 @@
 
 #include "FixedwingAttitudeControl.hpp"
 
+#include <vtol_att_control/vtol_type.h>
+
 using namespace time_literals;
 using math::constrain;
 using math::gradual;
@@ -46,6 +48,15 @@ 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();
 
@@ -117,7 +128,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 && _vehicle_status.is_vtol_tailsitter;
+		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
 
 		if (is_hovering || is_tailsitter_transition) {
 			_vcontrol_mode.flag_control_attitude_enabled = false;
@@ -129,7 +140,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 void
 FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 {
-	const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode;
+	const bool is_tailsitter_transition = _is_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)) {
@@ -201,7 +212,7 @@ void
 FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
 {
 	if (_rates_sp_sub.update(&_rates_sp)) {
-		if (_vehicle_status.is_vtol_tailsitter) {
+		if (_is_tailsitter) {
 			float tmp = _rates_sp.roll;
 			_rates_sp.roll = -_rates_sp.yaw;
 			_rates_sp.yaw = tmp;
@@ -300,7 +311,7 @@ void FixedwingAttitudeControl::Run()
 		float pitchspeed = angular_velocity.xyz[1];
 		float yawspeed = angular_velocity.xyz[2];
 
-		if (_vehicle_status.is_vtol_tailsitter) {
+		if (_is_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
@@ -368,7 +379,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 && !_vehicle_status.is_vtol_tailsitter)
+					   !_vehicle_status.in_transition_mode && !_is_tailsitter)
 				       || (dt > 0.02f);
 
 		/* if we are in rotary wing mode, do nothing */
@@ -406,7 +417,7 @@ void FixedwingAttitudeControl::Run()
 			 */
 			if (_landed
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
+				&& !_vehicle_status.in_transition_mode && !_is_tailsitter)) {
 
 				_roll_ctrl.reset_integrator();
 				_pitch_ctrl.reset_integrator();

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -151,6 +151,8 @@ 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,6 +33,7 @@
 
 #include "FixedwingPositionControl.hpp"
 
+#include <vtol_att_control/vtol_type.h>
 #include <px4_platform_common/events.h>
 
 using math::constrain;
@@ -58,6 +59,12 @@ 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
@@ -363,7 +370,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 (_vehicle_status.is_vtol_tailsitter) {
+		if (_vtol_tailsitter) {
 			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
 			R = R * R_offset;
 
@@ -2735,6 +2742,9 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
 		local_position_setpoint.acceleration[0] = NAN;
 		local_position_setpoint.acceleration[1] = NAN;
 		local_position_setpoint.acceleration[2] = NAN;
+		local_position_setpoint.jerk[0] = NAN;
+		local_position_setpoint.jerk[1] = NAN;
+		local_position_setpoint.jerk[2] = NAN;
 		local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
 		local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
 		local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -80,7 +80,6 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/tecs_status.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -95,6 +94,7 @@
 #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;
@@ -526,6 +526,7 @@ 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/land_detector/LandDetector.cpp

@@ -93,6 +93,15 @@ void LandDetector::Run()
 	actuator_armed_s actuator_armed;
 
 	if (_actuator_armed_sub.update(&actuator_armed)) {
+		if (!_armed && actuator_armed.armed) {
+			// force in air briefly
+			_freefall_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_ground_contact_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_maybe_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_ground_effect_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+		}
+
 		_armed = actuator_armed.armed;
 	}
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -44,7 +44,6 @@
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/hover_thrust_estimate.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/takeoff_status.h>

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit 99e82cad70494903a23a67de08ff9cbb5918d8f3
+Subproject commit a1cb2c0e71f191f04da3d92d92db8702a7e91ff6

src/modules/mavlink/mavlink_mission.cpp

@@ -298,21 +298,19 @@ void
 MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 	mission_item_s mission_item{};
-	int16_t bytes_read = 0;
-	bool read_success = false;
+	int read_result = 0;
 
 	switch (_mission_type) {
 
 	case MAV_MISSION_TYPE_MISSION: {
-			bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s));
-			read_success = (bytes_read == sizeof(mission_item_s));
+			read_result = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s);
 		}
 		break;
 
 	case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
 			mission_fence_point_s mission_fence_point;
-			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));
+			read_result = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) ==
+				      sizeof(mission_fence_point_s);
 
 			mission_item.nav_cmd = mission_fence_point.nav_cmd;
 			mission_item.frame = mission_fence_point.frame;
@@ -332,8 +330,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;
-			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));
+			read_result = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) ==
+				      sizeof(mission_safe_point_s);
 
 			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
 			mission_item.frame = mission_safe_point.frame;
@@ -350,7 +348,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		break;
 	}
 
-	if (read_success) {
+	if (read_result > 0) {
 		_time_last_sent = hrt_absolute_time();
 
 		if (_int_mode) {
@@ -384,14 +382,9 @@ 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_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);
+			_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");
 		}
 
 		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

@@ -1011,18 +1011,18 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 	    (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) &&
 	    (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) {
 
-		trajectory_setpoint_s setpoint{};
+		vehicle_local_position_setpoint_s setpoint{};
 
 		const uint16_t type_mask = target_local_ned.type_mask;
 
 		if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
-			setpoint.position[0] = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
-			setpoint.position[1] = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
-			setpoint.position[2] = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
+			setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
+			setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
+			setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
 
-			setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
-			setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
-			setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
+			setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
+			setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
+			setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
 
 			setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_local_ned.afx;
 			setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_local_ned.afy;
@@ -1047,12 +1047,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 
 				const float yaw = matrix::Eulerf{R}(2);
 
-				setpoint.velocity[0] = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
-				setpoint.velocity[1] = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
-				setpoint.velocity[2] = velocity_body_sp(2);
+				setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
+				setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
+				setpoint.vz = velocity_body_sp(2);
 
 			} else {
-				matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.velocity);
+				setpoint.vx = NAN;
+				setpoint.vy = NAN;
+				setpoint.vz = NAN;
 			}
 
 			const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE |
@@ -1069,10 +1071,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				acceleration_setpoint.copyTo(setpoint.acceleration);
 
 			} else {
-				matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.acceleration);
+				setpoint.acceleration[0] = NAN;
+				setpoint.acceleration[1] = NAN;
+				setpoint.acceleration[2] = NAN;
 			}
 
-			matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);
+			setpoint.x = NAN;
+			setpoint.y = NAN;
+			setpoint.z = NAN;
 
 		} else {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %" PRIu8 " unsupported\t",
@@ -1082,14 +1088,19 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			return;
 		}
 
+		setpoint.thrust[0] = NAN;
+		setpoint.thrust[1] = NAN;
+		setpoint.thrust[2] = NAN;
+
 		setpoint.yaw      = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE)      ? (float)NAN : target_local_ned.yaw;
 		setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_local_ned.yaw_rate;
 
 
 		offboard_control_mode_s ocm{};
-		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
-		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
-		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
+		ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
+		ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
+				   || PX4_ISFINITE(setpoint.acceleration[2]);
 
 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@@ -1131,7 +1142,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 	    (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) &&
 	    (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) {
 
-		trajectory_setpoint_s setpoint{};
+		vehicle_local_position_setpoint_s setpoint{};
 
 		const uint16_t type_mask = target_global_int.type_mask;
 
@@ -1151,10 +1162,10 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 			// global -> local
 			const double lat = target_global_int.lat_int / 1e7;
 			const double lon = target_global_int.lon_int / 1e7;
-			global_local_proj_ref.project(lat, lon, setpoint.position[0], setpoint.position[1]);
+			global_local_proj_ref.project(lat, lon, setpoint.x, setpoint.y);
 
 			if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
-				setpoint.position[2] = local_pos.ref_alt - target_global_int.alt;
+				setpoint.z = local_pos.ref_alt - target_global_int.alt;
 
 			} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
 				home_position_s home_position{};
@@ -1162,7 +1173,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 
 				if (home_position.valid_alt) {
 					const float alt = home_position.alt - target_global_int.alt;
-					setpoint.position[2] = alt - local_pos.ref_alt;
+					setpoint.z = alt - local_pos.ref_alt;
 
 				} else {
 					// home altitude required
@@ -1175,7 +1186,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 
 				if (vehicle_global_position.terrain_alt_valid) {
 					const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
-					setpoint.position[2] = local_pos.ref_alt - alt;
+					setpoint.z = local_pos.ref_alt - alt;
 
 				} else {
 					// valid terrain alt required
@@ -1191,27 +1202,34 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 			}
 
 		} else {
-			matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);
+			setpoint.x = NAN;
+			setpoint.y = NAN;
+			setpoint.z = NAN;
 		}
 
 		// velocity
-		setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
-		setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
-		setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
+		setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
+		setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
+		setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
 
 		// acceleration
 		setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_global_int.afx;
 		setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_global_int.afy;
 		setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_global_int.afz;
 
+		setpoint.thrust[0] = NAN;
+		setpoint.thrust[1] = NAN;
+		setpoint.thrust[2] = NAN;
+
 		setpoint.yaw      = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE)      ? (float)NAN : target_global_int.yaw;
 		setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_global_int.yaw_rate;
 
 
 		offboard_control_mode_s ocm{};
-		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
-		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
-		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
+		ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
+		ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
+				   || PX4_ISFINITE(setpoint.acceleration[2]);
 
 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");
@@ -2323,7 +2341,6 @@ 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);
 	}
@@ -2753,7 +2770,6 @@ 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/mavlink/mavlink_receiver.h

@@ -95,7 +95,6 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/tune_control.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -315,7 +314,7 @@ private:
 	uORB::Publication<vehicle_attitude_setpoint_s>		_fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Publication<vehicle_global_position_s>		_global_pos_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_local_position_s>		_local_pos_pub{ORB_ID(vehicle_local_position)};
-	uORB::Publication<trajectory_setpoint_s>		_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<vehicle_local_position_setpoint_s>	_trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};

src/modules/mc_att_control/mc_att_control.hpp

@@ -54,6 +54,7 @@
 #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,6 +58,13 @@ 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();
 }
@@ -294,8 +301,6 @@ 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/mc_pos_control/MulticopterPositionControl.cpp

@@ -615,6 +615,7 @@ void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_se
 	setpoint.yaw = setpoint.yawspeed = NAN;
 	setpoint.vx = setpoint.vy = setpoint.vz = NAN;
 	setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
+	setpoint.jerk[0] = setpoint.jerk[1] = setpoint.jerk[2] = NAN;
 	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
 }
 

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -57,7 +57,6 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_control_mode.h>

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -246,6 +246,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	local_position_setpoint.vy = _vel_sp(1);
 	local_position_setpoint.vz = _vel_sp(2);
 	_acc_sp.copyTo(local_position_setpoint.acceleration);
+	nans<3, 1>().copyTo(local_position_setpoint.jerk);
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }