fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 12:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add orientation initialization tests

Browse Source
This commit is contained in:
kamilritz
2019-12-17 18:52:18 +01:00
committed by Mathieu Bresciani
parent 01495ede97
commit 7d3814b1d8
7 changed files with 236 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/sensor_simulator.cpp
+12
View File
@@ -74,3 +74,15 @@ void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,
Vector3f{0.0f,0.0f,0.0f} + gyro_bias);
}
void SensorSimulator::simulateOrientation(Quatf orientation)
{
const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G};
const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f};
const Dcmf R_bodyToWorld(orientation);
const Vector3f sensed_gravity_body = R_bodyToWorld.transpose() * world_sensed_gravity;
const Vector3f body_mag_field = R_bodyToWorld.transpose() * world_mag_field;
_imu.setData(sensed_gravity_body, Vector3f{0.0f,0.0f,0.0f});
_mag.setData(body_mag_field);
}