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

Fixed the QMC5883 identification,add check ID

Browse Source
This commit is contained in:
ljwang 2017-07-25 18:33:26 +08:00 committed by Andrew Tridgell
parent 4865aef9e4
commit 648871a388
2 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -96,11 +96,8 @@ bool AP_Compass_QMC5883L::init()
_dev->set_retries(10); _dev->set_retries(10);
uint8_t whoami; if(!_check_whoami()){
if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)|| goto fail;
whoami != QMC5883_ID_VAL){
// not an QMC5883L
goto fail;
} }
if (!_dev->write_register(0x0B, 0x01)|| if (!_dev->write_register(0x0B, 0x01)||
@ -143,6 +140,20 @@ bool AP_Compass_QMC5883L::init()
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
return false; return false;
} }
bool AP_Compass_QMC5883L::_check_whoami()
{
uint8_t whoami;
//After power on 0x21 == 0x03
if (!_dev->read_registers(0x21, &whoami,1)||
whoami != 0x03){
return false;
}
if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)||
whoami != QMC5883_ID_VAL){
return false;
}
return true;
}
void AP_Compass_QMC5883L::timer() void AP_Compass_QMC5883L::timer()
{ {

1
libraries/AP_Compass/AP_Compass_QMC5883L.h
View File

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