dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2 sensor_sim: set correct world mag field

This commit is contained in:
bresch 2024-02-20 13:39:55 +01:00 committed by Daniel Agar
parent b80f15f7b5
commit b46fc9a67d
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
View File

@ -132,7 +132,7 @@ void SensorSimulator::setSensorDataToDefault()
_flow.setData(_flow.dataAtRest());
_gps.setData(_gps.getDefaultGpsData());
_imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f});
_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
_mag.setData(Vector3f{0.218f, 0.f, 0.43f});
_rng.setData(0.2f, 100);
_vio.setData(_vio.dataAtRest());
}
@ -358,7 +358,7 @@ void SensorSimulator::setSensorDataFromTrajectory()
// Magnetometer
if (_mag.isRunning()) {
const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f};
const Vector3f world_mag_field = Vector3f{0.218f, 0.f, 0.43f};
const Vector3f mag_field_body = R_world_to_body * world_mag_field;
_mag.setData(mag_field_body);
}
@ -419,7 +419,10 @@ void SensorSimulator::simulateOrientation(Quatf orientation)
_R_body_to_world = Dcmf(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};
// The world mag field Y component is 0 as most unit tests assume no magnetic dectination
const Vector3f world_mag_field = Vector3f{0.218f, 0.f, 0.43f};
const Vector3f sensed_gravity_body = _R_body_to_world.transpose() * world_sensed_gravity;
const Vector3f body_mag_field = _R_body_to_world.transpose() * world_mag_field;