mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
uncrustify libraries/AP_Compass/AP_Compass_HMC5843.cpp
This commit is contained in:
parent
662d285f44
commit
c8ede643dc
@ -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()
|
||||
|
||||
|
||||
/*
|
||||
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()
|
||||
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()
|
||||
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
|
||||
|
Loading…
Reference in New Issue
Block a user