Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fix milligauss code in LSM303D driver

Browse Source
This commit is contained in:
raspilot 2015-09-14 21:22:23 +08:00 committed by Randy Mackay
parent 2c62a3a1c5
commit 1b5e6849d9
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -271,9 +271,9 @@ bool AP_Compass_LSM303D::_read_raw()
raw_mag_report_tx.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
_spi->transaction((uint8_t *)&raw_mag_report_tx, (uint8_t *)&raw_mag_report_rx, sizeof(raw_mag_report_tx));
_mag_x = raw_mag_report_rx.x / 16;
_mag_y = raw_mag_report_rx.y / 16;
_mag_z = raw_mag_report_rx.z / 16;
_mag_x = raw_mag_report_rx.x;
_mag_y = raw_mag_report_rx.y;
_mag_z = raw_mag_report_rx.z;
if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) {
return false;
@ -332,21 +332,29 @@ AP_Compass_LSM303D::init()
_scaling[1] = 1.0;
_scaling[2] = 1.0;
_spi_sem->give();
hal.scheduler->resume_timer_procs();
_initialised = true;
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_dev_id(_compass_instance, get_dev_id());
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
set_external(_compass_instance, false);
#endif
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
set_milligauss_ratio(_compass_instance, 1.0f);
_spi_sem->give();
hal.scheduler->resume_timer_procs();
_initialised = true;
return _initialised;
}
uint32_t AP_Compass_LSM303D::get_dev_id()
{
return AP_COMPASS_TYPE_LSM303D;
}
bool AP_Compass_LSM303D::_hardware_init(void)
{
if (!_spi_sem->take(100)) {
@ -402,7 +410,7 @@ void AP_Compass_LSM303D::_collect_samples()
if (!_read_raw()) {
error("_read_raw() failed\n");
} else {
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
uint32_t time_us = hal.scheduler->micros();
// rotate raw_field from sensor frame to body frame
@ -445,6 +453,7 @@ void AP_Compass_LSM303D::read()
return;
}
hal.scheduler->suspend_timer_procs();
Vector3f field(_mag_x_accum * _scaling[0],
_mag_y_accum * _scaling[1],
_mag_z_accum * _scaling[2]);
@ -452,6 +461,7 @@ void AP_Compass_LSM303D::read()
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
hal.scheduler->resume_timer_procs();
publish_filtered_field(field, _compass_instance);
}
@ -481,22 +491,22 @@ uint8_t AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga)
if (max_ga <= 2) {
_mag_range_ga = 2;
setbits |= REG6_FULL_SCALE_2GA_M;
new_scale_ga_digit = 0.080e-3f;
new_scale_ga_digit = 0.080f;
} else if (max_ga <= 4) {
_mag_range_ga = 4;
setbits |= REG6_FULL_SCALE_4GA_M;
new_scale_ga_digit = 0.160e-3f;
new_scale_ga_digit = 0.160f;
} else if (max_ga <= 8) {
_mag_range_ga = 8;
setbits |= REG6_FULL_SCALE_8GA_M;
new_scale_ga_digit = 0.320e-3f;
new_scale_ga_digit = 0.320f;
} else if (max_ga <= 12) {
_mag_range_ga = 12;
setbits |= REG6_FULL_SCALE_12GA_M;
new_scale_ga_digit = 0.479e-3f;
new_scale_ga_digit = 0.479f;
} else {
return -1;

7
libraries/AP_Compass/AP_Compass_LSM303D.h
View File

@ -32,9 +32,9 @@ private:
int16_t _mag_x = 0;
int16_t _mag_y = 0;
int16_t _mag_z = 0;
int16_t _mag_x_accum = 0;
int16_t _mag_y_accum = 0;
int16_t _mag_z_accum = 0;
float _mag_x_accum = 0.0f;
float _mag_y_accum = 0.0f;
float _mag_z_accum = 0.0f;
uint32_t _last_accum_time = 0;
uint32_t _last_update_timestamp = 0;
uint8_t _accum_count = 0;
@ -51,6 +51,7 @@ public:
AP_Compass_LSM303D(Compass &compass);
bool init(void);
void read(void);
uint32_t get_dev_id();
// detect the sensor
static AP_Compass_Backend *detect_spi(Compass &compass);