diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 883229268..a6f32bfcc 100644 --- a/mavros_extras/CMakeLists.txt +++ b/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() diff --git a/mavros_extras/src/plugins/fake_gps.cpp b/mavros_extras/src/plugins/fake_gps.cpp index 83bc916a4..3edf7d8e7 100644 --- a/mavros_extras/src/plugins/fake_gps.cpp +++ b/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); diff --git a/mavros_extras/src/plugins/fake_gps_utils.hpp b/mavros_extras/src/plugins/fake_gps_utils.hpp new file mode 100644 index 000000000..f8f30d52b --- /dev/null +++ b/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 +#include + +#include + +#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(std::lround(course_deg * 100.0)) % 36000; +} + +} // namespace fake_gps +} // namespace extra_plugins +} // namespace mavros diff --git a/mavros_extras/test/test_fake_gps_utils.cpp b/mavros_extras/test/test_fake_gps_utils.cpp new file mode 100644 index 000000000..dcf2a6e37 --- /dev/null +++ b/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 + +#include + +#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); +}