Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: revisited AK8963 code 

Reduced duplication a bit and revisited the update method which is now
split into several logical hunks.
This commit is contained in:
Staroselskii Georgii 2015-07-22 14:50:32 +03:00 committed by Andrew Tridgell
parent fffedae3e0
commit 8ab33c46ce
2 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -102,8 +102,7 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus)
_last_accum_time(0),
_bus(bus)
{
_mag_x_accum =_mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_reset_filter();
}
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass)
@ -213,14 +212,10 @@ void AP_Compass_AK8963::read()
return;
}
/* Update */
Vector3f field(_mag_x_accum * _magnetometer_ASA[0],
_mag_y_accum * _magnetometer_ASA[1],
_mag_z_accum * _magnetometer_ASA[2]);
auto field = _get_filtered_field();
_make_factory_sensitivity_adjustment(field);
field /= _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_reset_filter();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
field.rotate(ROTATION_YAW_90);
@ -229,6 +224,27 @@ void AP_Compass_AK8963::read()
publish_field(field, _compass_instance);
}
Vector3f AP_Compass_AK8963::_get_filtered_field() const
{
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
return field;
}
void AP_Compass_AK8963::_reset_filter()
{
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
}
void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) const
{
field.x *= _magnetometer_ASA[0];
field.y *= _magnetometer_ASA[1];
field.z *= _magnetometer_ASA[2];
}
void AP_Compass_AK8963::_update()
{
if (hal.scheduler->micros() - _last_update_timestamp < 10000) {

4
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -46,6 +46,10 @@ public:
void accumulate(void);
private:
void _make_factory_sensitivity_adjustment(Vector3f& field) const;
Vector3f _get_filtered_field() const;
void _reset_filter();
bool _reset();
bool _setup_mode();
bool _check_id();