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

AP_Compass: IST8310: improve initialization 

- Make sure device is reset while initializing.
 - Give better names to register macros
 - Average X, Y and Z by 16: sensor is internally running at 200 sps
   (theoretical, ~160 pratical). The wait time is ~6msec averaging
   by 16. We do 10msec.
This commit is contained in:
Lucas De Marchi 2017-03-01 01:28:39 -08:00 committed by Andrew Tridgell
parent c0691afc1f
commit d84851d8fa
1 changed files with 39 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -29,18 +29,27 @@
#define DEVICE_ID 0x10
#define CNTL1_REG 0xA
#define SINGLE_MEASUREMENT_MODE 0x1
#define ODR_100HZ 0x6
#define CNTL1_BIT_SINGLE_MEASUREMENT_MODE 0x1
#define STAT1_REG 0x2
#define DATA_RDY 0x1
#define CNTL2_REG 0xB
#define CNTL2_BIT_SRST 1
#define AVGCNTL_REG 0x41
#define AVERAGING_Y_BY_2 0x20
#define AVERAGING_XZ_BY_4 0x04
#define AVGCNTL_BIT_XZ_0 (0)
#define AVGCNTL_BIT_XZ_2 (1)
#define AVGCNTL_BIT_XZ_4 (2)
#define AVGCNTL_BIT_XZ_8 (3)
#define AVGCNTL_BIT_XZ_16 (4)
#define AVGCNTL_BIT_Y_0 (0 << 3)
#define AVGCNTL_BIT_Y_2 (1 << 3)
#define AVGCNTL_BIT_Y_4 (2 << 3)
#define AVGCNTL_BIT_Y_8 (3 << 3)
#define AVGCNTL_BIT_Y_16 (4 << 3)
#define PDCNTL_REG 0x42
#define NORMAL_PULSE_DURATION 0xC0
#define PDCNTL_BIT_PULSE_DURATION_NORMAL 0xC0
#define SAMPLING_PERIOD_USEC (10 * USEC_PER_MSEC)
@ -74,6 +83,8 @@ AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass,
bool AP_Compass_IST8310::init()
{
uint8_t reset_count = 0;
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
@ -88,8 +99,28 @@ bool AP_Compass_IST8310::init()
goto fail;
}
if (!_dev->write_register(AVGCNTL_REG, AVERAGING_Y_BY_2 | AVERAGING_XZ_BY_4) ||
!_dev->write_register(PDCNTL_REG, NORMAL_PULSE_DURATION)) {
for (; reset_count < 5; reset_count++) {
if (!_dev->write_register(CNTL2_REG, CNTL2_BIT_SRST)) {
hal.scheduler->delay(10);
continue;
}
hal.scheduler->delay(10);
uint8_t cntl2 = 0xFF;
if (_dev->read_registers(CNTL2_REG, &cntl2, 1) &&
(cntl2 & 0x01) == 0) {
break;
}
}
if (reset_count == 5) {
fprintf(stderr, "IST8310: failed to reset device\n");
goto fail;
}
if (!_dev->write_register(AVGCNTL_REG, AVGCNTL_BIT_Y_16 | AVGCNTL_BIT_XZ_16) ||
!_dev->write_register(PDCNTL_REG, PDCNTL_BIT_PULSE_DURATION_NORMAL)) {
fprintf(stderr, "IST8310: found device but could not set it up\n");
goto fail;
}
@ -128,7 +159,7 @@ fail:
void AP_Compass_IST8310::start_conversion()
{
if (!_dev->write_register(CNTL1_REG, SINGLE_MEASUREMENT_MODE)) {
if (!_dev->write_register(CNTL1_REG, CNTL1_BIT_SINGLE_MEASUREMENT_MODE)) {
hal.util->perf_count(_perf_xfer_err);
_need_start = true;
return;