AHRS: fixed initial attitude for non-zero AHRS_ORIENTATION

libraries/AP_AHRS/AP_AHRS.cpp

@@ -229,6 +229,8 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
 // init sets up INS board orientation
 void AP_AHRS::init()
 {
+    update_orientation();
+
     // EKF1 is no longer supported - handle case where it is selected
     if (_ekf_type.get() == 1) {
         AP_BoardConfig::config_error("EKF1 not available");

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -71,7 +71,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
 void AP_AHRS::update_orientation()
 {
     const uint32_t now_ms = AP_HAL::millis();
-    if (now_ms - last_orientation_update_ms < 1000) {
+    if (last_orientation_update_ms != 0 &&
+        now_ms - last_orientation_update_ms < 1000) {
         // only update once/second
         return;
     }

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

@@ -195,6 +195,9 @@ void AP_InertialSensor_SITL::generate_accel()
     }
 
     accel_accum /= nsamples;
+
+    accel_accum.rotate(sitl->imu_orientation);
+
     _rotate_and_correct_accel(accel_instance, accel_accum);
     _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64());
 
@@ -298,6 +301,9 @@ void AP_InertialSensor_SITL::generate_gyro()
 #endif
     }
     gyro_accum /= nsamples;
+
+    gyro_accum.rotate(sitl->imu_orientation);
+
     _rotate_and_correct_gyro(gyro_instance, gyro_accum);
     _notify_new_gyro_raw_sample(gyro_instance, gyro_accum, AP_HAL::micros64());
 }

libraries/SITL/SIM_Aircraft.cpp

@@ -411,6 +411,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
     fdm.bodyMagField = mag_bf;
 
+    fdm.bodyMagField.rotate(sitl->imu_orientation);
+
     // copy laser scanner results
     fdm.scanner.points = scanner.points;
     fdm.scanner.ranges = scanner.ranges;

libraries/SITL/SITL.cpp

@@ -400,6 +400,12 @@ const AP_Param::GroupInfo SIM::var_info2[] = {
     // @Description: Ground behavior of aircraft (tailsitter, no movement, forward only)
     AP_GROUPINFO("GND_BEHAV",   41, SIM,  gnd_behav, -1),
 
+    // @Param: IMU_ORIENT
+    // @CopyFieldsFrom: AHRS_ORIENTATION
+    // @DisplayName: IMU orientation
+    // @Description: Simulated orientation of the IMUs
+    AP_GROUPINFO("IMU_ORIENT",   42, SIM,  imu_orientation, 0),
+
     // sailboat wave and tide simulation parameters
 
     // @Param: WAVE_ENABLE

libraries/SITL/SITL.h

@@ -449,6 +449,8 @@ class SIM {
 
     AP_Int8 gnd_behav;
 
+    AP_Enum<Rotation> imu_orientation;
+
     struct {
         AP_Int8 enable;     // 0: disabled, 1: roll and pitch, 2: roll, pitch and heave
         AP_Float length;    // m