Fix fake GPS velocity frame and COG units

mavros_extras/CMakeLists.txt

@@ -243,6 +243,15 @@ if(BUILD_TESTING)
 
   ament_lint_auto_find_test_dependencies()
 
+  ament_add_gtest(mavros-extras-fake-gps-test test/test_fake_gps_utils.cpp)
+  target_include_directories(mavros-extras-fake-gps-test PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/plugins
+  )
+  target_link_libraries(mavros-extras-fake-gps-test
+    mavros
+    rcpputils::rcpputils
+  )
+
   ament_add_pytest_test(test_srtm test/test_srtm.py)
 endif()
 

mavros_extras/src/plugins/fake_gps.cpp

@@ -29,6 +29,7 @@
 #include "mavros/plugin.hpp"
 #include "mavros/plugin_filter.hpp"
 #include "mavros/setpoint_mixin.hpp"
+#include "fake_gps_utils.hpp"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
@@ -83,7 +84,8 @@ class FakeGPSPlugin : public plugin::Plugin,
     satellites_visible(5),
     fix_type(GPS_FIX_TYPE::NO_GPS),
     tf_rate(10.0),
-    map_origin(0.0, 0.0, 0.0)
+    map_origin(0.0, 0.0, 0.0),
+    has_previous_position(false)
   {
     enable_node_watch_parameters();
 
@@ -302,6 +304,7 @@ class FakeGPSPlugin : public plugin::Plugin,
   Eigen::Vector3d ecef_origin;          //!< geocentric origin [m]
   Eigen::Vector3d old_ecef;             //!< previous geocentric position [m]
   double old_stamp;                     //!< previous stamp [s]
+  bool has_previous_position;           //!< previous sample is available for velocity
 
   /* -*- mid-level helpers and low-level send -*- */
 
@@ -331,11 +334,16 @@ class FakeGPSPlugin : public plugin::Plugin,
       RCLCPP_INFO_STREAM(get_logger(), "FGPS: Caught exception: " << e.what());
     }
 
-    Eigen::Vector3d vel = (old_ecef - current_ecef) / (stamp.seconds() - old_stamp);    // [m/s]
+    Eigen::Vector3d vel_ned = Eigen::Vector3d::Zero();
+    if (has_previous_position) {
+      vel_ned = fake_gps::calculate_velocity_ned(
+        current_ecef, old_ecef, stamp.seconds() - old_stamp, map_origin);
+    }
 
     // store old values
     old_stamp = stamp.seconds();
     old_ecef = current_ecef;
+    has_previous_position = true;
 
     if (use_hil_gps) {
       /**
@@ -346,17 +354,8 @@ class FakeGPSPlugin : public plugin::Plugin,
        */
       mavlink::common::msg::HIL_GPS hil_gps {};
 
-      vel *= 1e2;                   // [cm/s]
-
-      // compute course over ground
-      double cog;
-      if (vel.x() == 0 && vel.y() == 0) {
-        cog = 0;
-      } else if (vel.x() >= 0 && vel.y() < 0) {
-        cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y());
-      } else {
-        cog = M_PI / 2 - atan2(vel.x(), vel.y());
-      }
+      const uint16_t cog = fake_gps::course_over_ground_cdeg(vel_ned);
+      vel_ned *= 1e2;                   // [cm/s]
 
       // Fill in and send message
       hil_gps.time_usec = get_time_usec(stamp);                 // [useconds]
@@ -365,11 +364,11 @@ class FakeGPSPlugin : public plugin::Plugin,
       hil_gps.alt = uas->data.egm96_5->ConvertHeight(
         geodetic.x(), geodetic.y(), geodetic.z(),
         GeographicLib::Geoid::ELLIPSOIDTOGEOID) * 1e3;          // [meters * 1e3]
-      hil_gps.vel = vel.block<2, 1>(0, 0).norm();               // [cm/s]
-      hil_gps.vn = vel.x();                                     // [cm/s]
-      hil_gps.ve = vel.y();                                     // [cm/s]
-      hil_gps.vd = vel.z();                                     // [cm/s]
-      hil_gps.cog = cog * 1e2;                                  // [degrees * 1e2]
+      hil_gps.vel = vel_ned.block<2, 1>(0, 0).norm();           // [cm/s]
+      hil_gps.vn = vel_ned.x();                                 // [cm/s]
+      hil_gps.ve = vel_ned.y();                                 // [cm/s]
+      hil_gps.vd = vel_ned.z();                                 // [cm/s]
+      hil_gps.cog = cog;                                        // [degrees * 1e2]
       hil_gps.eph = eph * 1e2;                                  // [cm]
       hil_gps.epv = epv * 1e2;                                  // [cm]
       hil_gps.fix_type = utils::enum_value(fix_type);
@@ -396,10 +395,10 @@ class FakeGPSPlugin : public plugin::Plugin,
       if (epv == 0.0f) {
         gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VDOP);
       }
-      if (fabs(vel.x()) <= 0.01f && fabs(vel.y()) <= 0.01f) {
+      if (fabs(vel_ned.x()) <= 0.01f && fabs(vel_ned.y()) <= 0.01f) {
         gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_HORIZ);
       }
-      if (fabs(vel.z()) <= 0.01f) {
+      if (fabs(vel_ned.z()) <= 0.01f) {
         gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_VERT);
       }
       int64_t tdiff = (gps_input.time_usec / 1000) - UNIX_OFFSET_MSEC;
@@ -413,9 +412,9 @@ class FakeGPSPlugin : public plugin::Plugin,
       gps_input.alt = uas->data.egm96_5->ConvertHeight(
         geodetic.x(), geodetic.y(), geodetic.z(),
         GeographicLib::Geoid::ELLIPSOIDTOGEOID);            // [meters]
-      gps_input.vn = vel.x();                               // [m/s]
-      gps_input.ve = vel.y();                               // [m/s]
-      gps_input.vd = vel.z();                               // [m/s]
+      gps_input.vn = vel_ned.x();                           // [m/s]
+      gps_input.ve = vel_ned.y();                           // [m/s]
+      gps_input.vd = vel_ned.z();                           // [m/s]
       gps_input.hdop = eph;                                 // [m]
       gps_input.vdop = epv;                                 // [m]
       gps_input.fix_type = utils::enum_value(fix_type);

mavros_extras/src/plugins/fake_gps_utils.hpp

@@ -0,0 +1,74 @@
+/*
+ * Copyright 2026 Daniil Mordanov.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+
+#include <Eigen/Core>
+
+#include "mavros/frame_tf.hpp"
+
+namespace mavros
+{
+namespace extra_plugins
+{
+namespace fake_gps
+{
+
+/**
+ * @brief Derive local NED velocity from two ECEF positions.
+ *
+ * @param current_ecef Current ECEF position [m].
+ * @param previous_ecef Previous ECEF position [m].
+ * @param dt Elapsed time [s].
+ * @param map_origin Geodetic map origin [latitude, longitude, altitude].
+ * @return Velocity in the local North-East-Down frame [m/s].
+ */
+inline Eigen::Vector3d calculate_velocity_ned(
+  const Eigen::Vector3d & current_ecef,
+  const Eigen::Vector3d & previous_ecef,
+  const double dt,
+  const Eigen::Vector3d & map_origin)
+{
+  if (dt <= 0.0) {
+    return Eigen::Vector3d::Zero();
+  }
+
+  const Eigen::Vector3d velocity_ecef = (current_ecef - previous_ecef) / dt;
+  const Eigen::Vector3d velocity_enu = ftf::transform_frame_ecef_enu(
+    velocity_ecef, map_origin);
+  return ftf::transform_frame_enu_ned(velocity_enu);
+}
+
+/**
+ * @brief Compute course over ground from a NED velocity vector.
+ *
+ * @return Course clockwise from north in centidegrees in the range [0, 35999].
+ */
+inline uint16_t course_over_ground_cdeg(const Eigen::Vector3d & velocity_ned)
+{
+  const double north = velocity_ned.x();
+  const double east = velocity_ned.y();
+
+  if (north == 0.0 && east == 0.0) {
+    return 0;
+  }
+
+  double course_deg = std::atan2(east, north) * 180.0 / M_PI;
+  if (course_deg < 0.0) {
+    course_deg += 360.0;
+  }
+
+  return static_cast<uint16_t>(std::lround(course_deg * 100.0)) % 36000;
+}
+
+}  // namespace fake_gps
+}  // namespace extra_plugins
+}  // namespace mavros

mavros_extras/test/test_fake_gps_utils.cpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright 2026 Daniil Mordanov.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#include <gtest/gtest.h>
+
+#include <Eigen/Core>
+
+#include "fake_gps_utils.hpp"
+
+namespace fake_gps = mavros::extra_plugins::fake_gps;
+
+static constexpr double epsilon = 1e-9;
+
+TEST(FakeGpsUtils, ConvertsEcefVelocityToNed)
+{
+  const Eigen::Vector3d map_origin(0.0, 0.0, 0.0);
+  const Eigen::Vector3d previous_ecef = Eigen::Vector3d::Zero();
+
+  const auto north = fake_gps::calculate_velocity_ned(
+    Eigen::Vector3d(0.0, 0.0, 2.0), previous_ecef, 2.0, map_origin);
+  EXPECT_NEAR(north.x(), 1.0, epsilon);
+  EXPECT_NEAR(north.y(), 0.0, epsilon);
+  EXPECT_NEAR(north.z(), 0.0, epsilon);
+
+  const auto east = fake_gps::calculate_velocity_ned(
+    Eigen::Vector3d(0.0, 2.0, 0.0), previous_ecef, 2.0, map_origin);
+  EXPECT_NEAR(east.x(), 0.0, epsilon);
+  EXPECT_NEAR(east.y(), 1.0, epsilon);
+  EXPECT_NEAR(east.z(), 0.0, epsilon);
+
+  const auto up = fake_gps::calculate_velocity_ned(
+    Eigen::Vector3d(2.0, 0.0, 0.0), previous_ecef, 2.0, map_origin);
+  EXPECT_NEAR(up.x(), 0.0, epsilon);
+  EXPECT_NEAR(up.y(), 0.0, epsilon);
+  EXPECT_NEAR(up.z(), -1.0, epsilon);
+}
+
+TEST(FakeGpsUtils, RejectsNonIncreasingTimestamps)
+{
+  const Eigen::Vector3d current_ecef(1.0, 2.0, 3.0);
+  const Eigen::Vector3d previous_ecef = Eigen::Vector3d::Zero();
+  const Eigen::Vector3d map_origin(0.0, 0.0, 0.0);
+
+  EXPECT_TRUE(
+    fake_gps::calculate_velocity_ned(
+      current_ecef, previous_ecef, 0.0, map_origin).isZero());
+  EXPECT_TRUE(
+    fake_gps::calculate_velocity_ned(
+      current_ecef, previous_ecef, -1.0, map_origin).isZero());
+}
+
+TEST(FakeGpsUtils, ComputesCourseClockwiseFromNorth)
+{
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(0.0, 0.0, 0.0)), 0);
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(1.0, 0.0, 0.0)), 0);
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(1.0, 1.0, 0.0)), 4500);
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(0.0, 1.0, 0.0)), 9000);
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(-1.0, 0.0, 0.0)), 18000);
+  EXPECT_EQ(
+    fake_gps::course_over_ground_cdeg(Eigen::Vector3d(0.0, -1.0, 0.0)), 27000);
+}