Merge pull request #2738 from mavlink/pr-set-gps-origin

Add method set to GPS origin

Author: julianoes

docs/en/cpp/api_reference/classmavsdk_1_1_action.md

@@ -69,6 +69,7 @@ void | [set_return_to_launch_altitude_async](#classmavsdk_1_1_action_1acdc4360c2
 [Result](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1adc2e13257ef13de0e7610cf879a0ec51) | [set_return_to_launch_altitude](#classmavsdk_1_1_action_1a5b05e84d35fad5b0ba2837aae1b3686e) (float relative_altitude_m)const | Set the return to launch minimum return altitude (in meters).
 void | [set_current_speed_async](#classmavsdk_1_1_action_1afd210be0eba436c81da79107562a0b6c) (float speed_m_s, const [ResultCallback](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1a70a7b6e742d0c86728dc2e1827dacccd) callback) | Set current speed.
 [Result](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1adc2e13257ef13de0e7610cf879a0ec51) | [set_current_speed](#classmavsdk_1_1_action_1af3b74cf3912411d9476b6eeac0984afb) (float speed_m_s)const | Set current speed.
+[Result](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1adc2e13257ef13de0e7610cf879a0ec51) | [set_gps_global_origin](#classmavsdk_1_1_action_1abf97620153fca87f2dd89a6eb96a8479) (double latitude_deg, double longitude_deg, float absolute_altitude_m)const | Set GPS Global Origin.
 const [Action](classmavsdk_1_1_action.md) & | [operator=](#classmavsdk_1_1_action_1a89482740f533e194fade200103b5adef) (const [Action](classmavsdk_1_1_action.md) &)=delete | Equality operator (object is not copyable).
 
 
@@ -972,6 +973,29 @@ This function is blocking. See 'set_current_speed_async' for the non-blocking co
 
  [Result](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1adc2e13257ef13de0e7610cf879a0ec51) - Result of request.
 
+### set_gps_global_origin() {#classmavsdk_1_1_action_1abf97620153fca87f2dd89a6eb96a8479}
+```cpp
+Result mavsdk::Action::set_gps_global_origin(double latitude_deg, double longitude_deg, float absolute_altitude_m) const
+```
+
+
+Set GPS Global Origin.
+
+Sets the GPS coordinates of the vehicle local origin (0,0,0) position.
+
+
+This function is blocking.
+
+**Parameters**
+
+* double **latitude_deg** - 
+* double **longitude_deg** - 
+* float **absolute_altitude_m** - 
+
+**Returns**
+
+ [Result](classmavsdk_1_1_action.md#classmavsdk_1_1_action_1adc2e13257ef13de0e7610cf879a0ec51) - Result of request.
+
 ### operator=() {#classmavsdk_1_1_action_1a89482740f533e194fade200103b5adef}
 ```cpp
 const Action & mavsdk::Action::operator=(const Action &)=delete

examples/CMakeLists.txt

@@ -42,6 +42,7 @@ add_subdirectory(offboard)
 add_subdirectory(parachute)
 add_subdirectory(params)
 add_subdirectory(set_actuator)
+add_subdirectory(set_gps_origin)
 add_subdirectory(sniffer)
 add_subdirectory(system_info)
 add_subdirectory(takeoff_and_land)

examples/set_gps_origin/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(set_gps_origin)
+
+add_executable(set_gps_origin
+    set_gps_origin.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(set_gps_origin
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(set_gps_origin PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(set_gps_origin PRIVATE -W2)
+endif()

examples/set_gps_origin/set_gps_origin.cpp

@@ -0,0 +1,204 @@
+//
+// Example demonstrating how to set GPS global origin after flying to a new location.
+// This takes off, flies 50m north using offboard velocity control, lands,
+// and then sets the GPS global origin at the new location.
+//
+
+#include <chrono>
+#include <cmath>
+#include <future>
+#include <iostream>
+#include <thread>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/offboard/offboard.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udpin://0.0.0.0:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = mavsdk.first_autopilot(3.0);
+    if (!system) {
+        std::cerr << "Timed out waiting for system\n";
+        return 1;
+    }
+
+    // Instantiate plugins
+    auto action = Action{system.value()};
+    auto offboard = Offboard{system.value()};
+    auto telemetry = Telemetry{system.value()};
+
+    // Wait for system to be ready
+    while (!telemetry.health_all_ok()) {
+        std::cout << "Waiting for system to be ready\n";
+        sleep_for(seconds(1));
+    }
+    std::cout << "System is ready\n";
+
+    // Get initial GPS origin
+    auto origin_result = telemetry.get_gps_global_origin();
+    if (origin_result.first == Telemetry::Result::Success) {
+        std::cout << "Initial GPS origin: " << origin_result.second << '\n';
+    }
+
+    // Get initial position
+    auto initial_position = telemetry.position();
+    std::cout << "Initial position: lat=" << initial_position.latitude_deg
+              << ", lon=" << initial_position.longitude_deg
+              << ", alt=" << initial_position.absolute_altitude_m << "m AMSL\n";
+
+    // Arm
+    std::cout << "Arming...\n";
+    const auto arm_result = action.arm();
+    if (arm_result != Action::Result::Success) {
+        std::cerr << "Arming failed: " << arm_result << '\n';
+        return 1;
+    }
+    std::cout << "Armed\n";
+
+    // Take off
+    std::cout << "Taking off...\n";
+    const auto takeoff_result = action.takeoff();
+    if (takeoff_result != Action::Result::Success) {
+        std::cerr << "Takeoff failed: " << takeoff_result << '\n';
+        return 1;
+    }
+
+    // Wait until in air
+    auto in_air_promise = std::promise<void>{};
+    auto in_air_future = in_air_promise.get_future();
+    Telemetry::LandedStateHandle landed_handle = telemetry.subscribe_landed_state(
+        [&telemetry, &in_air_promise, &landed_handle](Telemetry::LandedState state) {
+            if (state == Telemetry::LandedState::InAir) {
+                std::cout << "Takeoff finished\n";
+                telemetry.unsubscribe_landed_state(landed_handle);
+                in_air_promise.set_value();
+            }
+        });
+
+    if (in_air_future.wait_for(seconds(10)) == std::future_status::timeout) {
+        std::cerr << "Takeoff timed out\n";
+        telemetry.unsubscribe_landed_state(landed_handle);
+        return 1;
+    }
+
+    // Let it stabilize at takeoff altitude
+    sleep_for(seconds(2));
+
+    // Start offboard mode and fly 50m north
+    std::cout << "Starting offboard mode to fly 50m north...\n";
+
+    // Send setpoint once before starting offboard
+    Offboard::VelocityNedYaw stay{};
+    offboard.set_velocity_ned(stay);
+
+    Offboard::Result offboard_result = offboard.start();
+    if (offboard_result != Offboard::Result::Success) {
+        std::cerr << "Offboard start failed: " << offboard_result << '\n';
+        return 1;
+    }
+
+    // Fly north at 5 m/s for 10 seconds = 50m
+    std::cout << "Flying north at 5 m/s for 10 seconds...\n";
+    Offboard::VelocityNedYaw fly_north{};
+    fly_north.north_m_s = 5.0f;
+    fly_north.yaw_deg = 0.0f;
+    offboard.set_velocity_ned(fly_north);
+    sleep_for(seconds(10));
+
+    // Stop and hover
+    std::cout << "Stopping...\n";
+    offboard.set_velocity_ned(stay);
+    sleep_for(seconds(2));
+
+    // Stop offboard mode
+    offboard_result = offboard.stop();
+    if (offboard_result != Offboard::Result::Success) {
+        std::cerr << "Offboard stop failed: " << offboard_result << '\n';
+        return 1;
+    }
+
+    // Get position before landing
+    auto position_before_landing = telemetry.position();
+    std::cout << "Position before landing: lat=" << position_before_landing.latitude_deg
+              << ", lon=" << position_before_landing.longitude_deg
+              << ", alt=" << position_before_landing.absolute_altitude_m << "m AMSL\n";
+
+    // Land
+    std::cout << "Landing...\n";
+    const auto land_result = action.land();
+    if (land_result != Action::Result::Success) {
+        std::cerr << "Landing failed: " << land_result << '\n';
+        return 1;
+    }
+
+    // Wait until landed
+    while (telemetry.in_air()) {
+        std::cout << "Waiting for landing...\n";
+        sleep_for(seconds(1));
+    }
+    std::cout << "Landed!\n";
+
+    // Wait a bit for things to settle
+    sleep_for(seconds(2));
+
+    // Get final position
+    auto final_position = telemetry.position();
+    std::cout << "Final position: lat=" << final_position.latitude_deg
+              << ", lon=" << final_position.longitude_deg
+              << ", alt=" << final_position.absolute_altitude_m << "m AMSL\n";
+
+    // Set the GPS global origin at the current position
+    std::cout << "Setting GPS global origin at current position...\n";
+    const auto set_origin_result = action.set_gps_global_origin(
+        final_position.latitude_deg,
+        final_position.longitude_deg,
+        final_position.absolute_altitude_m);
+
+    if (set_origin_result != Action::Result::Success) {
+        std::cerr << "Setting GPS global origin failed: " << set_origin_result << '\n';
+        return 1;
+    }
+    std::cout << "GPS global origin set successfully!\n";
+
+    // Verify by getting the new origin
+    auto new_origin_result = telemetry.get_gps_global_origin();
+    if (new_origin_result.first == Telemetry::Result::Success) {
+        std::cout << "New GPS origin: " << new_origin_result.second << '\n';
+    } else {
+        std::cerr << "Failed to get new GPS origin: " << new_origin_result.first << '\n';
+    }
+
+    // Wait a bit before exiting
+    sleep_for(seconds(3));
+    std::cout << "Finished!\n";
+
+    return 0;
+}

proto

@@ -256,6 +256,12 @@ Action::Result Action::set_current_speed(float speed_m_s) const
     return _impl->set_current_speed(speed_m_s);
 }
 
+Action::Result Action::set_gps_global_origin(
+    double latitude_deg, double longitude_deg, float absolute_altitude_m) const
+{
+    return _impl->set_gps_global_origin(latitude_deg, longitude_deg, absolute_altitude_m);
+}
+
 std::ostream& operator<<(std::ostream& str, Action::Result const& result)
 {
     switch (result) {

src/mavsdk/plugins/action/action.cpp

@@ -767,6 +767,75 @@ Action::Result ActionImpl::set_current_speed(float speed_m_s)
     return fut.get();
 }
 
+Action::Result ActionImpl::set_gps_global_origin(
+    double latitude_deg, double longitude_deg, float absolute_altitude_m) const
+{
+    const int32_t latitude_e7 = static_cast<int32_t>(std::round(latitude_deg * 1e7));
+    const int32_t longitude_e7 = static_cast<int32_t>(std::round(longitude_deg * 1e7));
+    const int32_t altitude_mm = static_cast<int32_t>(std::round(absolute_altitude_m * 1000.0f));
+
+    auto prom = std::promise<Action::Result>();
+    auto fut = prom.get_future();
+    std::atomic<bool> prom_already_set{false};
+
+    // Use a unique cookie for this handler
+    const void* cookie = this;
+
+    // Register handler for GPS_GLOBAL_ORIGIN response.
+    // Note: Older PX4 versions (pre-v1.17) had a race condition where they would
+    // broadcast GPS_GLOBAL_ORIGIN with stale values immediately after receiving
+    // SET_GPS_GLOBAL_ORIGIN, before EKF2 had processed the command. To handle
+    // this, we wait for a response with matching values rather than accepting
+    // the first response.
+    _system_impl->register_mavlink_message_handler(
+        MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN,
+        [&prom, &prom_already_set, latitude_e7, longitude_e7, altitude_mm](
+            const mavlink_message_t& message) {
+            mavlink_gps_global_origin_t origin;
+            mavlink_msg_gps_global_origin_decode(&message, &origin);
+
+            // Only signal success when we receive the values we set
+            if (origin.latitude == latitude_e7 && origin.longitude == longitude_e7 &&
+                origin.altitude == altitude_mm) {
+                if (!prom_already_set.exchange(true)) {
+                    prom.set_value(Action::Result::Success);
+                }
+            }
+            // Otherwise, keep waiting for the correct values (or timeout)
+        },
+        cookie);
+
+    // Send the SET_GPS_GLOBAL_ORIGIN message
+    if (!_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
+            mavlink_message_t message;
+            mavlink_msg_set_gps_global_origin_pack_chan(
+                mavlink_address.system_id,
+                mavlink_address.component_id,
+                channel,
+                &message,
+                _system_impl->get_system_id(),
+                latitude_e7,
+                longitude_e7,
+                altitude_mm,
+                0);
+            return message;
+        })) {
+        _system_impl->unregister_mavlink_message_handler(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, cookie);
+        return Action::Result::ConnectionError;
+    }
+
+    // Wait for response with timeout
+    auto status = fut.wait_for(std::chrono::duration<double>(_system_impl->timeout_s()));
+
+    _system_impl->unregister_mavlink_message_handler(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, cookie);
+
+    if (status == std::future_status::timeout) {
+        return Action::Result::Timeout;
+    }
+
+    return fut.get();
+}
+
 Action::Result ActionImpl::action_result_from_command_result(MavlinkCommandSender::Result result)
 {
     switch (result) {

src/mavsdk/plugins/action/action_impl.cpp

@@ -92,6 +92,9 @@ class ActionImpl : public PluginImplBase {
     void set_current_speed_async(float speed_m_s, const Action::ResultCallback& callback);
     Action::Result set_current_speed(float speed_m_s);
 
+    Action::Result set_gps_global_origin(
+        double latitude_deg, double longitude_deg, float absolute_altitude_m) const;
+
     Action::Result set_return_to_launch_altitude(const float relative_altitude_m) const;
     std::pair<Action::Result, float> get_return_to_launch_altitude() const;
 

src/mavsdk/plugins/action/action_impl.h

@@ -644,6 +644,20 @@ class Action : public PluginBase {
      */
     Result set_current_speed(float speed_m_s) const;
 
+    /**
+     * @brief Set GPS Global Origin.
+     *
+     * Sets the GPS coordinates of the vehicle local origin (0,0,0) position.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_gps_global_origin(
+        double latitude_deg, double longitude_deg, float absolute_altitude_m) const;
+
     /**
      * @brief Copy constructor.
      */