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: 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; 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)); _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_x = raw_mag_report_rx.x;
_mag_y = raw_mag_report_rx.y / 16; _mag_y = raw_mag_report_rx.y;
_mag_z = raw_mag_report_rx.z / 16; _mag_z = raw_mag_report_rx.z;
if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) { if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) {
return false; return false;
@ -332,21 +332,29 @@ AP_Compass_LSM303D::init()
_scaling[1] = 1.0; _scaling[1] = 1.0;
_scaling[2] = 1.0; _scaling[2] = 1.0;
_spi_sem->give(); /* register the compass instance in the frontend */
hal.scheduler->resume_timer_procs();
_initialised = true;
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
_compass_instance = register_compass(); _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 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
set_external(_compass_instance, false); set_external(_compass_instance, false);
#endif #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; return _initialised;
} }
uint32_t AP_Compass_LSM303D::get_dev_id()
{
return AP_COMPASS_TYPE_LSM303D;
}
bool AP_Compass_LSM303D::_hardware_init(void) bool AP_Compass_LSM303D::_hardware_init(void)
{ {
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
@ -402,7 +410,7 @@ void AP_Compass_LSM303D::_collect_samples()
if (!_read_raw()) { if (!_read_raw()) {
error("_read_raw() failed\n"); error("_read_raw() failed\n");
} else { } 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(); uint32_t time_us = hal.scheduler->micros();
// rotate raw_field from sensor frame to body frame // rotate raw_field from sensor frame to body frame
@ -445,6 +453,7 @@ void AP_Compass_LSM303D::read()
return; return;
} }
hal.scheduler->suspend_timer_procs();
Vector3f field(_mag_x_accum * _scaling[0], Vector3f field(_mag_x_accum * _scaling[0],
_mag_y_accum * _scaling[1], _mag_y_accum * _scaling[1],
_mag_z_accum * _scaling[2]); _mag_z_accum * _scaling[2]);
@ -452,6 +461,7 @@ void AP_Compass_LSM303D::read()
_accum_count = 0; _accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0; _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
hal.scheduler->resume_timer_procs();
publish_filtered_field(field, _compass_instance); 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) { if (max_ga <= 2) {
_mag_range_ga = 2; _mag_range_ga = 2;
setbits |= REG6_FULL_SCALE_2GA_M; setbits |= REG6_FULL_SCALE_2GA_M;
new_scale_ga_digit = 0.080e-3f; new_scale_ga_digit = 0.080f;
} else if (max_ga <= 4) { } else if (max_ga <= 4) {
_mag_range_ga = 4; _mag_range_ga = 4;
setbits |= REG6_FULL_SCALE_4GA_M; setbits |= REG6_FULL_SCALE_4GA_M;
new_scale_ga_digit = 0.160e-3f; new_scale_ga_digit = 0.160f;
} else if (max_ga <= 8) { } else if (max_ga <= 8) {
_mag_range_ga = 8; _mag_range_ga = 8;
setbits |= REG6_FULL_SCALE_8GA_M; setbits |= REG6_FULL_SCALE_8GA_M;
new_scale_ga_digit = 0.320e-3f; new_scale_ga_digit = 0.320f;
} else if (max_ga <= 12) { } else if (max_ga <= 12) {
_mag_range_ga = 12; _mag_range_ga = 12;
setbits |= REG6_FULL_SCALE_12GA_M; setbits |= REG6_FULL_SCALE_12GA_M;
new_scale_ga_digit = 0.479e-3f; new_scale_ga_digit = 0.479f;
} else { } else {
return -1; 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_x = 0;
int16_t _mag_y = 0; int16_t _mag_y = 0;
int16_t _mag_z = 0; int16_t _mag_z = 0;
int16_t _mag_x_accum = 0; float _mag_x_accum = 0.0f;
int16_t _mag_y_accum = 0; float _mag_y_accum = 0.0f;
int16_t _mag_z_accum = 0; float _mag_z_accum = 0.0f;
uint32_t _last_accum_time = 0; uint32_t _last_accum_time = 0;
uint32_t _last_update_timestamp = 0; uint32_t _last_update_timestamp = 0;
uint8_t _accum_count = 0; uint8_t _accum_count = 0;
@ -51,6 +51,7 @@ public:
AP_Compass_LSM303D(Compass &compass); AP_Compass_LSM303D(Compass &compass);
bool init(void); bool init(void);
void read(void); void read(void);
uint32_t get_dev_id();
// detect the sensor // detect the sensor
static AP_Compass_Backend *detect_spi(Compass &compass); static AP_Compass_Backend *detect_spi(Compass &compass);