diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index a492a1b48d7606..20feec14921df2 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 - diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c2c9d52ca178db..3c2c4f86303662 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 16d0305939c8b9..8f4677b51f0a3c 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; @@ -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); +} diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 4aab342ed7165b..13fb02602fe331 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 4f39f6c19d5083..38e87ebeaab8d9 100644 --- a/libraries/SITL/SITL.h +++ b/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