From 76b985fc9c48d4255d0a1e279589ed32ac6243a6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 11 Dec 2019 17:11:03 -0700 Subject: [PATCH] SITL: only recalculate ahrs rotation matrix if necessary --- libraries/SITL/SIM_Aircraft.cpp | 15 ++++++++++++--- libraries/SITL/SIM_Aircraft.h | 2 ++ libraries/SITL/SIM_FlightAxis.cpp | 10 ++-------- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 669f19c17f..3436c4ec8a 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -73,6 +73,12 @@ Aircraft::Aircraft(const char *frame_str) : // allow for orientation settings, such as with tailsitters enum ap_var_type ptype; ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype); + + enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); + ahrs_rotation_inv.from_rotation(imu_rotation); + ahrs_rotation_inv.transpose(); + last_imu_rotation = imu_rotation; + terrain = reinterpret_cast(AP_Param::find_object("TERRAIN_")); } @@ -371,11 +377,14 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) if (ahrs_orientation != nullptr) { enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); + if (imu_rotation != last_imu_rotation) { + ahrs_rotation_inv.from_rotation(imu_rotation); + ahrs_rotation_inv.transpose(); + last_imu_rotation = imu_rotation; + } if (imu_rotation != ROTATION_NONE) { Matrix3f m = dcm; - Matrix3f rot; - rot.from_rotation(imu_rotation); - m = m * rot.transposed(); + m = m * ahrs_rotation_inv; m.to_euler(&r, &p, &y); fdm.rollDeg = degrees(r); diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 768b3aaa79..225b3aaaff 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -193,6 +193,8 @@ protected: // allow for AHRS_ORIENTATION AP_Int8 *ahrs_orientation; + enum Rotation last_imu_rotation; + Matrix3f ahrs_rotation_inv; enum GroundBehaviour { GROUND_BEHAVIOR_NONE = 0, diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index b0b6afddea..5f1c8aa915 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -460,14 +460,8 @@ void FlightAxis::update(const struct sitl_input &input) Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef; Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef); - if (ahrs_orientation != nullptr) { - enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); - - if (imu_rotation != ROTATION_NONE) { - Matrix3f rot; - rot.from_rotation(imu_rotation); - airspeed3d = airspeed3d * rot.transposed(); - } + if (last_imu_rotation != ROTATION_NONE) { + airspeed3d = airspeed3d * ahrs_rotation_inv; } airspeed_pitot = MAX(airspeed3d.x,0);