feat(ekf2): publish acceleration in VehicleOdometry

.github/workflows/clang-tidy.yml

@@ -60,7 +60,10 @@ jobs:
       if: always() && github.event_name == 'pull_request'
       run: |
         mkdir -p pr-review
-        git diff -U0 origin/${{ github.base_ref }}...HEAD \
+        # Exclude msg/translation_node: it is a standalone ROS 2 package that
+        # depends on px4_msgs_old, which is not installed in the SITL clang-tidy
+        # environment.  It has its own CI via ros_translation_node.yml.
+        git diff -U0 origin/${{ github.base_ref }}...HEAD -- ':!msg/translation_node' \
           | clang-tidy-diff-18.py -p1 \
               -path build/px4_sitl_default-clang \
               -export-fixes pr-review/fixes.yml \

Makefile

@@ -525,7 +525,7 @@ px4_sitl_default-clang:
 # To add manual exclusions, append to CLANG_TIDY_EXCLUDE_EXTRA below.
 # Submodules are automatically excluded - no action needed when adding new ones.
 CLANG_TIDY_SUBMODULES := $(shell git config --file .gitmodules --get-regexp path | awk '{print $$2}' | tr '\n' '|' | sed 's/|$$//')
-CLANG_TIDY_EXCLUDE_EXTRA := src/systemcmds/tests|src/examples|src/modules/gyro_fft/CMSIS_5|src/lib/drivers/smbus|src/drivers/gpio|src/modules/commander/failsafe/emscripten|failsafe_test\.dir|\.pb\.cc
+CLANG_TIDY_EXCLUDE_EXTRA := src/systemcmds/tests|src/examples|src/modules/gyro_fft/CMSIS_5|src/lib/drivers/smbus|src/drivers/gpio|src/modules/commander/failsafe/emscripten|failsafe_test\.dir|\.pb\.cc|msg/translation_node
 CLANG_TIDY_EXCLUDE := $(CLANG_TIDY_SUBMODULES)|$(CLANG_TIDY_EXCLUDE_EXTRA)
 
 clang-tidy: px4_sitl_default-clang

Tools/ci/run-clang-tidy-pr.py

@@ -27,6 +27,7 @@
     'src/drivers/gpio',
     r'src/modules/commander/failsafe/emscripten',
     r'failsafe_test\.dir',
+    'msg/translation_node',  # separate ROS 2 package; depends on px4_msgs_old not available in SITL build
 ])
 
 

msg/px4_msgs_old/msg/VehicleOdometryV0.msg

@@ -0,0 +1,35 @@
+# Vehicle odometry data
+#
+# Fits ROS REP 147 for aerial vehicles
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp         # [us] Time since system start
+uint64 timestamp_sample  # [us] Timestamp sample
+
+uint8 pose_frame              # [@enum POSE_FRAME] Position and orientation frame of reference
+uint8 POSE_FRAME_UNKNOWN = 0  # Unknown frame
+uint8 POSE_FRAME_NED     = 1  # North-East-Down (NED) navigation frame. Aligned with True North.
+uint8 POSE_FRAME_FRD     = 2  # Forward-Right-Down (FRD) frame. Constant arbitrary heading offset from True North. Z is down.
+
+float32[3] position  # [m] [@frame local frame] [@invalid NaN If invalid/unknown] Position. Origin is position of GC at startup.
+float32[4] q         # [-] [@invalid NaN First value if invalid/unknown] Attitude (expressed as a quaternion) relative to pose reference frame at current location. Follows the Hamiltonian convention (w, x, y, z, right-handed, passive rotations from body to world)
+
+uint8 velocity_frame               # [@enum VELOCITY_FRAME] Reference frame of the velocity data
+uint8 VELOCITY_FRAME_UNKNOWN  = 0  # Unknown frame
+uint8 VELOCITY_FRAME_NED      = 1  # NED navigation frame at current position.
+uint8 VELOCITY_FRAME_FRD      = 2  # FRD navigation frame at current position. Constant arbitrary heading offset from True North. Z is down.
+uint8 VELOCITY_FRAME_BODY_FRD = 3  # FRD body-fixed frame
+
+float32[3] velocity          # [m/s] [@frame @velocity_frame] [@invalid NaN If invalid/unknown] Velocity.
+float32[3] angular_velocity  # [rad/s] [@frame @VELOCITY_FRAME_BODY_FRD] [@invalid NaN If invalid/unknown] Angular velocity in body-fixed frame
+
+float32[3] position_variance     # [m^2] Variance of position error
+float32[3] orientation_variance  # [rad^2] Variance of orientation/attitude error (expressed in body frame)
+float32[3] velocity_variance     # [m^2/s^2] Variance of velocity error
+
+uint8 reset_counter  # [-] Reset counter. Counts reset events on attitude, velocity and position.
+int8 quality         # [-] [@invalid 0] Quality. Unused.
+
+# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
+# TOPICS estimator_odometry

msg/translation_node/translations/all_translations.h

@@ -19,6 +19,7 @@
 #include "translation_vehicle_attitude_setpoint_v1.h"
 #include "translation_vehicle_command_ack_v1.h"
 #include "translation_vehicle_local_position_v1.h"
+#include "translation_vehicle_odometry_v1.h"
 #include "translation_vehicle_status_v1.h"
 #include "translation_vehicle_status_v2.h"
 #include "translation_vehicle_status_v3.h"

msg/translation_node/translations/translation_vehicle_odometry_v1.h

@@ -0,0 +1,54 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate VehicleOdometry v0 <--> v1
+#include <limits>
+#include <px4_msgs_old/msg/vehicle_odometry_v0.hpp>
+#include <px4_msgs/msg/vehicle_odometry.hpp>
+
+class VehicleOdometryV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::VehicleOdometryV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::VehicleOdometry;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/vehicle_odometry";
+
+#define COMMON_VEHICLE_ODOMETRY_FIELDS(FUNCTION) \
+	FUNCTION(timestamp) \
+	FUNCTION(timestamp_sample) \
+	FUNCTION(pose_frame) \
+	FUNCTION(position) \
+	FUNCTION(q) \
+	FUNCTION(velocity_frame) \
+	FUNCTION(velocity) \
+	FUNCTION(angular_velocity) \
+	FUNCTION(position_variance) \
+	FUNCTION(orientation_variance) \
+	FUNCTION(velocity_variance) \
+	FUNCTION(reset_counter) \
+	FUNCTION(quality)
+
+#define COPY_FIELD_FROM_OLDER(field) msg_newer.field = msg_older.field;
+#define COPY_FIELD_TO_OLDER(field) msg_older.field = msg_newer.field;
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		COMMON_VEHICLE_ODOMETRY_FIELDS(COPY_FIELD_FROM_OLDER)
+		msg_newer.acceleration.fill(std::numeric_limits<float>::quiet_NaN());
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		COMMON_VEHICLE_ODOMETRY_FIELDS(COPY_FIELD_TO_OLDER)
+	}
+
+#undef COPY_FIELD_FROM_OLDER
+#undef COPY_FIELD_TO_OLDER
+#undef COMMON_VEHICLE_ODOMETRY_FIELDS
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleOdometryV1Translation);

msg/versioned/VehicleOdometry.msg

@@ -2,7 +2,7 @@
 #
 # Fits ROS REP 147 for aerial vehicles
 
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp         # [us] Time since system start
 uint64 timestamp_sample  # [us] Timestamp sample
@@ -22,6 +22,7 @@ uint8 VELOCITY_FRAME_FRD      = 2  # FRD navigation frame at current position. C
 uint8 VELOCITY_FRAME_BODY_FRD = 3  # FRD body-fixed frame
 
 float32[3] velocity          # [m/s] [@frame @velocity_frame] [@invalid NaN If invalid/unknown] Velocity.
+float32[3] acceleration      # [m/s^2] [@frame local frame] [@invalid NaN If invalid/unknown] Acceleration of body origin in local NED frame.
 float32[3] angular_velocity  # [rad/s] [@frame @VELOCITY_FRAME_BODY_FRD] [@invalid NaN If invalid/unknown] Angular velocity in body-fixed frame
 
 float32[3] position_variance     # [m^2] Variance of position error

src/modules/ekf2/EKF2.cpp

@@ -825,8 +825,10 @@ void EKF2::Run()
 		if (_ekf.update()) {
 			perf_set_elapsed(_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
 
+			_vel_deriv = _ekf.getVelocityDerivative();
 			PublishLocalPosition(now);
 			PublishOdometry(now, imu_sample_new);
+			_ekf.resetVelocityDerivativeAccumulation();
 			PublishGlobalPosition(now);
 			PublishSensorBias(now);
 
@@ -1711,11 +1713,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.z_deriv = _ekf.getVerticalPositionDerivative();
 
 	// Acceleration of body origin in local frame
-	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
-	_ekf.resetVelocityDerivativeAccumulation();
-	lpos.ax = vel_deriv(0);
-	lpos.ay = vel_deriv(1);
-	lpos.az = vel_deriv(2);
+	lpos.ax = _vel_deriv(0);
+	lpos.ay = _vel_deriv(1);
+	lpos.az = _vel_deriv(2);
 
 	lpos.xy_valid = _ekf.isLocalHorizontalPositionValid();
 	lpos.v_xy_valid = _ekf.isLocalHorizontalPositionValid();
@@ -1829,6 +1829,9 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
 	odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
 	_ekf.getVelocity().copyTo(odom.velocity);
 
+	// acceleration
+	_vel_deriv.copyTo(odom.acceleration);
+
 	// angular_velocity
 	_ekf.getAngularVelocityAndResetAccumulator().copyTo(odom.angular_velocity);
 

src/modules/ekf2/EKF2.hpp

@@ -302,6 +302,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
 
+	Vector3f _vel_deriv{}; ///< NED velocity derivative (acceleration), shared between PublishLocalPosition and PublishOdometry
+
 	hrt_abstime _last_sensor_bias_published{0};
 
 	hrt_abstime _status_fake_hgt_pub_last{0};