|
|
|
@ -1,17 +1,17 @@
@@ -1,17 +1,17 @@
|
|
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
|
|
|
|
/*
|
|
|
|
|
AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer |
|
|
|
|
Code by Jordi Muñoz and Jose Julio. DIYDrones.com |
|
|
|
|
|
|
|
|
|
This library is free software; you can redistribute it and/or |
|
|
|
|
modify it under the terms of the GNU Lesser General Public |
|
|
|
|
License as published by the Free Software Foundation; either |
|
|
|
|
version 2.1 of the License, or (at your option) any later version. |
|
|
|
|
|
|
|
|
|
Sensor is conected to I2C port |
|
|
|
|
Sensor is initialized in Continuos mode (10Hz) |
|
|
|
|
|
|
|
|
|
*/ |
|
|
|
|
* AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer |
|
|
|
|
* Code by Jordi Muñoz and Jose Julio. DIYDrones.com |
|
|
|
|
* |
|
|
|
|
* This library is free software; you can redistribute it and/or |
|
|
|
|
* modify it under the terms of the GNU Lesser General Public |
|
|
|
|
* License as published by the Free Software Foundation; either |
|
|
|
|
* version 2.1 of the License, or (at your option) any later version. |
|
|
|
|
* |
|
|
|
|
* Sensor is conected to I2C port |
|
|
|
|
* Sensor is initialized in Continuos mode (10Hz) |
|
|
|
|
* |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
// AVR LibC Includes
|
|
|
|
|
#include <math.h> |
|
|
|
@ -105,13 +105,13 @@ bool AP_Compass_HMC5843::read_raw()
@@ -105,13 +105,13 @@ bool AP_Compass_HMC5843::read_raw()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
re-initialise after a IO error |
|
|
|
|
* re-initialise after a IO error |
|
|
|
|
*/ |
|
|
|
|
bool AP_Compass_HMC5843::re_initialise() |
|
|
|
|
{ |
|
|
|
|
if (! write_register(ConfigRegA, _base_config) || |
|
|
|
|
! write_register(ConfigRegB, magGain) || |
|
|
|
|
! write_register(ModeRegister, ContinuousConversion)) |
|
|
|
|
if (!write_register(ConfigRegA, _base_config) || |
|
|
|
|
!write_register(ConfigRegB, magGain) || |
|
|
|
|
!write_register(ModeRegister, ContinuousConversion)) |
|
|
|
|
return false; |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
@ -131,8 +131,8 @@ AP_Compass_HMC5843::init()
@@ -131,8 +131,8 @@ AP_Compass_HMC5843::init()
|
|
|
|
|
delay(10); |
|
|
|
|
|
|
|
|
|
// determine if we are using 5843 or 5883L
|
|
|
|
|
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || |
|
|
|
|
! read_register(ConfigRegA, &_base_config)) { |
|
|
|
|
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || |
|
|
|
|
!read_register(ConfigRegA, &_base_config)) { |
|
|
|
|
healthy = false; |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
@ -160,13 +160,13 @@ AP_Compass_HMC5843::init()
@@ -160,13 +160,13 @@ AP_Compass_HMC5843::init()
|
|
|
|
|
numAttempts++; |
|
|
|
|
|
|
|
|
|
// force positiveBias (compass should return 715 for all channels)
|
|
|
|
|
if (! write_register(ConfigRegA, PositiveBiasConfig)) |
|
|
|
|
if (!write_register(ConfigRegA, PositiveBiasConfig)) |
|
|
|
|
continue; // compass not responding on the bus
|
|
|
|
|
delay(50); |
|
|
|
|
|
|
|
|
|
// set gains
|
|
|
|
|
if (! write_register(ConfigRegB, calibration_gain) || |
|
|
|
|
! write_register(ModeRegister, SingleConversion)) |
|
|
|
|
if (!write_register(ConfigRegB, calibration_gain) || |
|
|
|
|
!write_register(ModeRegister, SingleConversion)) |
|
|
|
|
continue; |
|
|
|
|
|
|
|
|
|
// read values from the compass
|
|
|
|
|