SITL: allow for tailsitters in RealFlight

libraries/SITL/SIM_Aircraft.cpp

@@ -73,6 +73,10 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     last_wall_time_us = get_wall_time_us();
     frame_counter = 0;
 
+    // allow for orientation settings, such as with tailsitters
+    enum ap_var_type ptype;
+    ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
+
     terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
 }
 
@@ -387,6 +391,23 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
         fdm.altitude  = smoothing.location.alt * 1.0e-2;
     }
 
+    if (ahrs_orientation != nullptr) {
+        enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
+
+        if (imu_rotation != ROTATION_NONE) {
+            Matrix3f m = dcm;
+            Matrix3f rot;
+            rot.from_rotation(imu_rotation);
+            m = m * rot.transposed();
+
+            m.to_euler(&r, &p, &y);
+            fdm.rollDeg  = degrees(r);
+            fdm.pitchDeg = degrees(p);
+            fdm.yawDeg   = degrees(y);
+            fdm.quaternion.from_rotation_matrix(m);
+        }
+    }
+    
     if (last_speedup != sitl->speedup && sitl->speedup > 0) {
         set_speedup(sitl->speedup);
         last_speedup = sitl->speedup;
@@ -786,3 +807,4 @@ void Aircraft::extrapolate_sensors(float delta_time)
 }
 
 
+

libraries/SITL/SIM_Aircraft.h

@@ -164,6 +164,9 @@ protected:
     bool use_time_sync = true;
     float last_speedup = -1.0f;
 
+    // allow for AHRS_ORIENTATION
+    AP_Int8 *ahrs_orientation;
+    
     enum {
         GROUND_BEHAVIOR_NONE = 0,
         GROUND_BEHAVIOR_NO_MOVEMENT,
@@ -316,3 +319,4 @@ private:
 };
 
 } // namespace SITL
+

libraries/SITL/SIM_FlightAxis.cpp

@@ -87,28 +87,6 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
         AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
     }
 
-    if (strstr(frame_str, "pitch270")) {
-        // rotate tailsitter airframes for fixed wing view
-        rotation = ROTATION_PITCH_270;
-    }
-    if (strstr(frame_str, "pitch90")) {
-        // rotate tailsitter airframes for fixed wing view
-        rotation = ROTATION_PITCH_90;
-    }
-
-    switch (rotation) {
-    case ROTATION_NONE:
-        break;
-    case ROTATION_PITCH_90:
-        att_rotation.from_euler(0, radians(90), 0);
-        break;
-    case ROTATION_PITCH_270:
-        att_rotation.from_euler(0, radians(270), 0);
-        break;
-    default:
-        AP_HAL::panic("Unsupported flightaxis rotation %u\n", (unsigned)rotation);
-    }
-
     /* Create the thread that will be waiting for data from FlightAxis */
     mutex = hal.util->new_semaphore();
 
@@ -432,14 +410,6 @@ void FlightAxis::update(const struct sitl_input &input)
                state.m_accelerationBodyAY_MPS2,
                state.m_accelerationBodyAZ_MPS2);
 
-    if (rotation != ROTATION_NONE) {
-        dcm.transpose();
-        dcm = att_rotation * dcm;
-        dcm.transpose();
-        gyro.rotate(rotation);
-        accel_body.rotate(rotation);
-    }
-
     // accel on the ground is nasty in realflight, and prevents helicopter disarm
     if (state.m_isTouchingGround) {
         Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
@@ -517,3 +487,4 @@ void FlightAxis::report_FPS(void)
         last_frame_count_s = state.m_currentPhysicsTime_SEC;
     }
 }
+

libraries/SITL/SIM_FlightAxis.h

@@ -176,8 +176,6 @@ private:
     double last_frame_count_s;
     Vector3f position_offset;
     Vector3f last_velocity_ef;
-    Matrix3f att_rotation;
-    enum Rotation rotation = ROTATION_NONE;
 
     const char *controller_ip = "127.0.0.1";
     uint16_t controller_port = 18083;
@@ -188,3 +186,4 @@ private:
 
 
 } // namespace SITL
+