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

ekf2 sensor_sim: set correct world mag field

Browse Source
This commit is contained in:
bresch
2024-02-27 12:33:43 -05:00
committed by Daniel Agar
parent b80f15f7b5
commit b46fc9a67d
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
+6 -3
View File
@@ -132,7 +132,7 @@ void SensorSimulator::setSensorDataToDefault()
_flow.setData(_flow.dataAtRest()); _flow.setData(_flow.dataAtRest());
_gps.setData(_gps.getDefaultGpsData()); _gps.setData(_gps.getDefaultGpsData());
_imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f}); _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); _rng.setData(0.2f, 100);
_vio.setData(_vio.dataAtRest()); _vio.setData(_vio.dataAtRest());
} }
@@ -358,7 +358,7 @@ void SensorSimulator::setSensorDataFromTrajectory()
// Magnetometer // Magnetometer
if (_mag.isRunning()) { 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; const Vector3f mag_field_body = R_world_to_body * world_mag_field;
_mag.setData(mag_field_body); _mag.setData(mag_field_body);
} }
@@ -419,7 +419,10 @@ void SensorSimulator::simulateOrientation(Quatf orientation)
_R_body_to_world = Dcmf(orientation); _R_body_to_world = Dcmf(orientation);
const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G}; 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 sensed_gravity_body = _R_body_to_world.transpose() * world_sensed_gravity;
const Vector3f body_mag_field = _R_body_to_world.transpose() * world_mag_field; const Vector3f body_mag_field = _R_body_to_world.transpose() * world_mag_field;