SITL: add Lua-compatible flagged set_pose follow-up

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
-

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
-

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<int16_t>(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;
@@ -1415,3 +1435,12 @@ 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);
+}

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;

libraries/SITL/SITL.h

@@ -629,8 +629,15 @@ 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);
+    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