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

AP_Compass: make a transition to milligauss in Compass library

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-08-24 17:53:17 +03:00 committed by Andrew Tridgell
parent 693613aa0f
commit e93ff44a97
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Compass/Compass.cpp
View File

@ -517,6 +517,9 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offset.set(offsets);
save_offsets(i);
/* Ugly hack to update offsets in milligauss that are going to be used across all the codebase in the future */
_state[i].offset_milligauss = offsets * _backends[i]->get_conversion_ratio();
}
}
@ -624,7 +627,7 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
const Vector3f &field = get_field();
const Vector3f &field = get_field_milligauss();
float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
@ -664,6 +667,11 @@ bool Compass::configured(uint8_t i)
return false;
}
// exit immediately if all offsets (mG) are zero
if (is_zero(get_offsets_milligauss(i).length())) {
return false;
}
#if COMPASS_MAX_INSTANCES > 1
// backup detected dev_id
int32_t dev_id_orig = _state[i].dev_id;