Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: support -roll180 option to frames

Browse Source
This commit is contained in:
Andrew Tridgell 2017-06-05 15:36:55 +10:00
parent 6a6f9681ab
commit 74265f523a
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_Aircraft.cpp
View File

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

2
libraries/SITL/SIM_Aircraft.h
View File

@ -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,