aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: allow for runtime changes to SIM_MAG_DIA_?

Browse Source 
this makes for easier testing
This commit is contained in:
Andrew Tridgell 2018-07-17 12:56:44 +10:00
parent 3c2e8baee2
commit b4c7d1925e
2 changed files with 35 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -25,25 +25,38 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
// make first compass external // make first compass external
set_external(_compass_instance[0], true); set_external(_compass_instance[0], true);
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
// create correction matrix for diagnonals and off-diagonals hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
Vector3f diag = _sitl->mag_diag.get();
if (diag.is_zero()) {
diag(1,1,1);
}
const Vector3f &diagonals = diag;
const Vector3f &offdiagonals = _sitl->mag_offdiag;
_eliptical_corr = Matrix3f(diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z);
if (!_eliptical_corr.invert()) {
_eliptical_corr.identity();
}
} }
} }
/*
create correction matrix for diagnonals and off-diagonals
*/
void AP_Compass_SITL::_setup_eliptical_correcion(void)
{
Vector3f diag = _sitl->mag_diag.get();
if (diag.is_zero()) {
diag(1,1,1);
}
const Vector3f &diagonals = diag;
const Vector3f &offdiagonals = _sitl->mag_offdiag;
if (diagonals == _last_dia && offdiagonals == _last_odi) {
return;
}
_eliptical_corr = Matrix3f(diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z);
if (!_eliptical_corr.invert()) {
_eliptical_corr.identity();
}
_last_dia = diag;
_last_odi = offdiagonals;
}
void AP_Compass_SITL::_timer() void AP_Compass_SITL::_timer()
{ {
// TODO: Refactor delay buffer with AP_Baro_SITL. // TODO: Refactor delay buffer with AP_Baro_SITL.
@ -91,6 +104,8 @@ void AP_Compass_SITL::_timer()
new_mag_data = buffer[best_index].data; new_mag_data = buffer[best_index].data;
} }
_setup_eliptical_correcion();
new_mag_data = _eliptical_corr * new_mag_data; new_mag_data = _eliptical_corr * new_mag_data;
new_mag_data -= _sitl->mag_ofs.get(); new_mag_data -= _sitl->mag_ofs.get();

7
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -37,8 +37,11 @@ private:
Vector3f _mag_accum[SITL_NUM_COMPASSES]; Vector3f _mag_accum[SITL_NUM_COMPASSES];
uint32_t _accum_count; uint32_t _accum_count;
void _setup_eliptical_correcion();
Matrix3f _eliptical_corr; Matrix3f _eliptical_corr;
Vector3f _last_dia;
Vector3f _last_odi;
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD