From 74265f523a14a821e853ff6f8cf657aca39a2ed5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Jun 2017 15:36:55 +1000 Subject: [PATCH] SITL: support -roll180 option to frames --- libraries/SITL/SIM_Aircraft.cpp | 19 +++++++++++++++++++ libraries/SITL/SIM_Aircraft.h | 2 ++ 2 files changed, 21 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index ea09791dc9..22b09af2e7 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -73,6 +73,11 @@ 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; + } + terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_"); } @@ -379,6 +384,20 @@ 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; + + 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 (last_speedup != sitl->speedup && sitl->speedup > 0) { set_speedup(sitl->speedup); last_speedup = sitl->speedup; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 97c1556f5c..805c6980f9 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -164,6 +164,8 @@ protected: bool use_time_sync = true; float last_speedup = -1.0f; + enum Rotation imu_rotation = ROTATION_NONE; + enum { GROUND_BEHAVIOR_NONE = 0, GROUND_BEHAVIOR_NO_MOVEMENT,