From 2804ee6d1e1acce637950c46cfa14ce1d4038825 Mon Sep 17 00:00:00 2001 From: NDev Date: Sat, 28 Mar 2026 02:07:52 -0400 Subject: [PATCH 1/3] SITL: make set_pose home reset flag-controlled --- libraries/SITL/SIM_Aircraft.cpp | 29 ++++++++++++++++++++++++----- libraries/SITL/SIM_Aircraft.h | 3 ++- libraries/SITL/SITL.h | 4 ++++ 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 16d0305939c8b..afba2c6899d82 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1384,7 +1384,9 @@ void Aircraft::update_eas_airspeed() /* set pose on the aircraft, called from scripting */ -bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, const Vector3f &velocity_ef, const Vector3f &gyro_rads) +bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, + const Vector3f &velocity_ef, const Vector3f &gyro_rads, + uint8_t flags) { if (instance >= MAX_SIM_INSTANCES || instances[instance] == nullptr) { return false; @@ -1393,11 +1395,29 @@ bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion WITH_SEMAPHORE(aircraft.pose_sem); quat.rotation_matrix(aircraft.dcm); - aircraft.home = loc; - aircraft.origin = loc; aircraft.velocity_ef = velocity_ef; aircraft.location = loc; - aircraft.position = Vector3d(0, 0, 0); + + if ((flags & SITL::SIM::SET_POSE_RESET_HOME_ORIGIN) != 0) { + // Reset the local reference frame so the requested pose becomes the new absolute origin. + aircraft.home = loc; + aircraft.origin = loc; + aircraft.home_is_set = true; + aircraft.position = Vector3d(0, 0, 0); + aircraft.ground_level = aircraft.home.alt * 0.01f; + + float roll, pitch, yaw; + aircraft.dcm.to_euler(&roll, &pitch, &yaw); + aircraft.home_yaw = degrees(yaw); + + if (aircraft.precland != nullptr) { + aircraft.precland->set_default_location(aircraft.home.lat * 1.0e-7f, + aircraft.home.lng * 1.0e-7f, + static_cast(aircraft.get_home_yaw())); + } + } else { + aircraft.position = aircraft.home.get_distance_NED_double(loc); + } aircraft.smoothing.position = aircraft.position; aircraft.smoothing.rotation_b2e = aircraft.dcm; aircraft.smoothing.velocity_ef = velocity_ef; @@ -1414,4 +1434,3 @@ bool SITL::SIM::set_pose(uint8_t instance, const Location &loc, const Quaternion { return Aircraft::set_pose(instance, loc, quat, velocity_ef, gyro_rads); } - diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 4aab342ed7165..13fb02602fe33 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -194,7 +194,8 @@ class Aircraft { used by scripting to control simulated aircraft position */ static bool set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, - const Vector3f &velocity_ef, const Vector3f &gyro_rads); + const Vector3f &velocity_ef, const Vector3f &gyro_rads, + uint8_t flags = 0); protected: SIM *sitl; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 4f39f6c19d508..9d9fc7b1296d3 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -629,6 +629,10 @@ class SIM { /* used by scripting to control simulated aircraft position */ + enum SetPoseFlags : uint8_t { + SET_POSE_RESET_HOME_ORIGIN = 1U << 0, + }; + bool set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, const Vector3f &velocity_ef, const Vector3f &gyro_rads); }; From abc087257855144f150dbccd0e58ffbfa7da31f7 Mon Sep 17 00:00:00 2001 From: NDev Date: Sat, 28 Mar 2026 02:08:23 -0400 Subject: [PATCH 2/3] SITL: expose flagged set_pose to Lua --- .../AP_Scripting/generator/description/bindings.desc | 2 +- libraries/SITL/SIM_Aircraft.cpp | 10 ++++++++++ libraries/SITL/SITL.h | 3 +++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c2c9d52ca178d..3c2c4f8630366 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -1132,6 +1132,7 @@ include SITL/SITL.h depends AP_SIM_ENABLED singleton SITL::SIM depends AP_SIM_ENABLED singleton SITL::SIM rename sim singleton SITL::SIM method set_pose boolean uint8_t'skip_check Location Quaternion Vector3f Vector3f +singleton SITL::SIM method set_pose_flags boolean uint8_t'skip_check Location Quaternion Vector3f Vector3f uint8_t'skip_check userdata DroneCAN_Handle depends HAL_ENABLE_DRONECAN_DRIVERS userdata DroneCAN_Handle creation DroneCAN_Handle::new_handle 4 @@ -1140,4 +1141,3 @@ userdata DroneCAN_Handle manual request DroneCAN_Handle::request 2 1 userdata DroneCAN_Handle method subscribe boolean userdata DroneCAN_Handle manual check_message DroneCAN_Handle::check_message 0 4 userdata DroneCAN_Handle manual_operator __gc DroneCAN_Handle::__gc - diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index afba2c6899d82..8f4677b51f0a3 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1434,3 +1434,13 @@ bool SITL::SIM::set_pose(uint8_t instance, const Location &loc, const Quaternion { return Aircraft::set_pose(instance, loc, quat, velocity_ef, gyro_rads); } + +/* + wrapper for scripting access with explicit flags + */ +bool SITL::SIM::set_pose_flags(uint8_t instance, const Location &loc, const Quaternion &quat, + const Vector3f &velocity_ef, const Vector3f &gyro_rads, + uint8_t flags) +{ + return Aircraft::set_pose(instance, loc, quat, velocity_ef, gyro_rads, flags); +} diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 9d9fc7b1296d3..38e87ebeaab8d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -635,6 +635,9 @@ class SIM { bool set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, const Vector3f &velocity_ef, const Vector3f &gyro_rads); + bool set_pose_flags(uint8_t instance, const Location &loc, const Quaternion &quat, + const Vector3f &velocity_ef, const Vector3f &gyro_rads, + uint8_t flags); }; } // namespace SITL From 6c4eed0fc4dce0c7129a9411b64390ea250d1eea Mon Sep 17 00:00:00 2001 From: NDev Date: Sat, 28 Mar 2026 02:08:43 -0400 Subject: [PATCH 3/3] AP_Scripting: document flagged set_pose binding --- libraries/AP_Scripting/docs/docs.lua | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index a492a1b48d760..20feec14921df 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -4226,6 +4226,16 @@ sim = {} ---@return boolean function sim:set_pose(instance, loc, orient, velocity_bf, gyro_rads) end +-- set pose of simulated vehicle with flags. Bit 0 resets home and origin. Requires AHRS_EKF_TYPE=10 +---@param instance integer -- 0 for first vehicle +---@param loc Location_ud +---@param orient Quaternion_ud +---@param velocity_bf Vector3f_ud -- body frame velocity +---@param gyro_rads Vector3f_ud -- gyro body rate in rad/s +---@param flags integer -- bitmask, bit 0 resets home and origin +---@return boolean +function sim:set_pose_flags(instance, loc, orient, velocity_bf, gyro_rads, flags) end + -- CRSF menu parameter userdata object ---@class (exact) CRSFParameter_ud local CRSFParameter_ud = {} @@ -4330,4 +4340,3 @@ function DroneCAN_Handle_ud:request(target_node, payload) end ---@param payload string -- payload for message ---@return boolean -- true if send succeeded function DroneCAN_Handle_ud:broadcast(payload) end -