diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 08ecbd385d..8ffdf96a44 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -73,11 +73,10 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) : last_wall_time_us = get_wall_time_us(); frame_counter = 0; - // support rotated IMUs for testing - if (strstr(frame_str, "-roll180")) { - imu_rotation = ROTATION_ROLL_180; - } - + // 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_"); } @@ -392,18 +391,21 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.altitude = smoothing.location.alt * 1.0e-2; } - if (imu_rotation != ROTATION_NONE) { - Vector3f accel(fdm.xAccel, fdm.yAccel, fdm.zAccel); - accel.rotate(imu_rotation); - fdm.xAccel = accel.x; - fdm.yAccel = accel.y; - fdm.zAccel = accel.z; + if (ahrs_orientation != nullptr) { + enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); - Vector3f rgyro(fdm.rollRate, fdm.pitchRate, fdm.yawRate); - rgyro.rotate(imu_rotation); - fdm.rollRate = degrees(rgyro.x); - fdm.pitchRate = degrees(rgyro.y); - fdm.yawRate = degrees(rgyro.z); + 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) { diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index b3dc8d761e..6a64231494 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -164,8 +164,9 @@ protected: bool use_time_sync = true; float last_speedup = -1.0f; - enum Rotation imu_rotation = ROTATION_NONE; - + // allow for AHRS_ORIENTATION + AP_Int8 *ahrs_orientation; + enum { GROUND_BEHAVIOR_NONE = 0, GROUND_BEHAVIOR_NO_MOVEMENT, diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index b33b2dfdef..31e0c7fd9e 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/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(); @@ -428,14 +406,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; diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index 0a9defbbf2..c34fc9d722 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/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;