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

Delete write registers at start,using another way to repair

This commit is contained in:
ljwang 2017-07-28 14:34:23 +08:00 committed by Andrew Tridgell
parent 648871a388
commit 5a7ec7d429
2 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -91,11 +91,13 @@ bool AP_Compass_QMC5883L::init()
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
//must reset first
_dev->write_register(QMC5883L_REG_CONF2,QMC5883L_RST);
_dev->set_retries(10);
#if 0
_dump_registers();
#endif
if(!_check_whoami()){
goto fail;
}
@ -143,9 +145,10 @@ bool AP_Compass_QMC5883L::init()
bool AP_Compass_QMC5883L::_check_whoami()
{
uint8_t whoami;
//After power on 0x21 == 0x03
if (!_dev->read_registers(0x21, &whoami,1)||
whoami != 0x03){
//Affected by other devices,must read registers 0x00 once or reset,after can read the ID registers reliably
_dev->read_registers(0x00,&whoami,1);
if (!_dev->read_registers(0x0C, &whoami,1)||
whoami != 0x01){
return false;
}
if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)||
@ -240,3 +243,17 @@ void AP_Compass_QMC5883L::read()
_sem->give();
}
void AP_Compass_QMC5883L::_dump_registers()
{
printf("QMC5883L registers dump\n");
for (uint8_t reg = QMC5883L_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
uint8_t v;
_dev->read_registers(reg,&v,1);
printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - ( QMC5883L_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
printf("\n");
}
}
}

1
libraries/AP_Compass/AP_Compass_QMC5883L.h
View File

@ -46,6 +46,7 @@ private:
bool force_external,
enum Rotation rotation);
void _dump_registers();
bool _check_whoami();
void timer();
bool init();