SITL: only recalculate ahrs rotation matrix if necessary

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_Terrain *>(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);

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,

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);