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

AP_Compass: use thread per bus for lsm303d mag

This commit is contained in:
Andrew Tridgell 2016-11-03 21:03:45 +11:00
parent b92c48548a
commit 36190ba545
2 changed files with 34 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -208,7 +208,7 @@ void AP_Compass_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_
*/
bool AP_Compass_LSM303D::_data_ready()
{
return (_drdy_pin_m->read()) != 0;
return _drdy_pin_m == nullptr || (_drdy_pin_m->read() != 0);
}
@ -253,17 +253,12 @@ bool AP_Compass_LSM303D::_read_sample()
bool AP_Compass_LSM303D::init()
{
// TODO: support users without data ready pin
if (LSM303D_DRDY_M_PIN < 0) {
return false;
if (LSM303D_DRDY_M_PIN >= 0) {
_drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
_drdy_pin_m->mode(HAL_GPIO_INPUT);
}
_drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
_drdy_pin_m->mode(HAL_GPIO_INPUT);
hal.scheduler->suspend_timer_procs();
bool success = _hardware_init();
hal.scheduler->resume_timer_procs();
if (!success) {
return false;
@ -280,7 +275,10 @@ bool AP_Compass_LSM303D::init()
set_external(_compass_instance, false);
#endif
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
_accum_sem = hal.util->new_semaphore();
// read at 100Hz
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, bool));
return true;
}
@ -339,49 +337,37 @@ fail_whoami:
return false;
}
void AP_Compass_LSM303D::_update()
bool AP_Compass_LSM303D::_update()
{
uint32_t time_us = AP_HAL::micros();
Vector3f raw_field;
if (AP_HAL::micros() - _last_update_timestamp < 10000) {
return;
}
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
if (!_read_sample()) {
goto fail;
return true;
}
raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, time_us, _compass_instance);
publish_raw_field(raw_field, AP_HAL::micros(), _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
if (_accum_sem->take(0)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_accum_sem->give();
}
_last_update_timestamp = AP_HAL::micros();
fail:
_dev->get_semaphore()->give();
return true;
}
// Read Sensor data
@ -391,18 +377,23 @@ void AP_Compass_LSM303D::read()
return;
}
if (!_accum_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_accum_sem->give();
return;
}
hal.scheduler->suspend_timer_procs();
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
hal.scheduler->resume_timer_procs();
_accum_sem->give();
publish_filtered_field(field, _compass_instance);
}

4
libraries/AP_Compass/AP_Compass_LSM303D.h
View File

@ -33,7 +33,7 @@ private:
bool _data_ready();
bool _hardware_init();
void _update();
bool _update();
void _disable_i2c();
bool _mag_set_range(uint8_t max_ga);
bool _mag_set_samplerate(uint16_t frequency);
@ -45,7 +45,6 @@ private:
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
uint32_t _last_update_timestamp;
int16_t _mag_x;
int16_t _mag_y;
int16_t _mag_z;
@ -57,4 +56,5 @@ private:
uint8_t _mag_range_ga;
uint8_t _mag_samplerate;
uint8_t _reg7_expected;
AP_HAL::Semaphore *_accum_sem;
};