zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

uncrustify libraries/AP_Compass/AP_Compass_HMC5843.cpp

mission-4.1.18
uncrustify 13 years ago committed by Pat Hickey
parent
7840eebaef
commit
4f9c6bbb19
1 changed files with 205 additions and 205 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 410
      libraries/AP_Compass/AP_Compass_HMC5843.cpp

410
libraries/AP_Compass/AP_Compass_HMC5843.cpp
Unescape View File

@ -1,25 +1,25 @@ @@ -1,25 +1,25 @@
// -*- 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>
#include <FastSerial.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#include "Arduino.h"
#else
#include "WConstants.h"
#include "WConstants.h"
#endif
#include <I2C.h>
@ -54,233 +54,233 @@ @@ -54,233 +54,233 @@
// read_register - read a register value
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
{
if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
healthy = false;
return false;
}
return true;
if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
healthy = false;
return false;
}
return true;
}
// write_register - update a register value
bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
{
if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
healthy = false;
return false;
}
return true;
if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
healthy = false;
return false;
}
return true;
}
// Read Sensor data
bool AP_Compass_HMC5843::read_raw()
{
uint8_t buff[6];
if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
healthy = false;
return false;
}
int16_t rx, ry, rz;
rx = (int16_t)(buff[0] << 8) | buff[1];
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rz = (int16_t)(buff[2] << 8) | buff[3];
ry = (int16_t)(buff[4] << 8) | buff[5];
} else {
ry = (int16_t)(buff[2] << 8) | buff[3];
rz = (int16_t)(buff[4] << 8) | buff[5];
}
if (rx == -4096 || ry == -4096 || rz == -4096) {
// no valid data available
return false;
}
mag_x = -rx;
mag_y = ry;
mag_z = -rz;
return true;
uint8_t buff[6];
if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
healthy = false;
return false;
}
int16_t rx, ry, rz;
rx = (int16_t)(buff[0] << 8) | buff[1];
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rz = (int16_t)(buff[2] << 8) | buff[3];
ry = (int16_t)(buff[4] << 8) | buff[5];
} else {
ry = (int16_t)(buff[2] << 8) | buff[3];
rz = (int16_t)(buff[4] << 8) | buff[5];
}
if (rx == -4096 || ry == -4096 || rz == -4096) {
// no valid data available
return false;
}
mag_x = -rx;
mag_y = ry;
mag_z = -rz;
return true;
}
/*
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))
return false;
return true;
if (!write_register(ConfigRegA, _base_config) ||
!write_register(ConfigRegB, magGain) ||
!write_register(ModeRegister, ContinuousConversion))
return false;
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
byte calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
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)) {
healthy = false;
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
return false;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 20 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
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))
continue;
// read values from the compass
delay(50);
if (!read_raw())
continue; // we didn't read valid values
delay(10);
float cal[3];
cal[0] = fabs(expected_x / (float)mag_x);
cal[1] = fabs(expected_yz / (float)mag_y);
cal[2] = fabs(expected_yz / (float)mag_z);
if (cal[0] > 0.7 && cal[0] < 1.3 &&
cal[1] > 0.7 && cal[1] < 1.3 &&
cal[2] > 0.7 && cal[2] < 1.3) {
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
int numAttempts = 0, good_count = 0;
bool success = false;
byte calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
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)) {
healthy = false;
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
return false;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 20 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
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))
continue;
// read values from the compass
delay(50);
if (!read_raw())
continue; // we didn't read valid values
delay(10);
float cal[3];
cal[0] = fabs(expected_x / (float)mag_x);
cal[1] = fabs(expected_yz / (float)mag_y);
cal[2] = fabs(expected_yz / (float)mag_z);
if (cal[0] > 0.7 && cal[0] < 1.3 &&
cal[1] > 0.7 && cal[1] < 1.3 &&
cal[2] > 0.7 && cal[2] < 1.3) {
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0
/* useful for debugging */
Serial.print("mag_x: ");
Serial.print(mag_x);
Serial.print(" mag_y: ");
Serial.print(mag_y);
Serial.print(" mag_z: ");
Serial.println(mag_z);
Serial.print("CalX: ");
Serial.print(calibration[0]/good_count);
Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count);
/* useful for debugging */
Serial.print("mag_x: ");
Serial.print(mag_x);
Serial.print(" mag_y: ");
Serial.print(mag_y);
Serial.print(" mag_z: ");
Serial.println(mag_z);
Serial.print("CalX: ");
Serial.print(calibration[0]/good_count);
Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count);
#endif
}
if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
return false;
}
_initialised = true;
return success;
}
if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
return false;
}
_initialised = true;
return success;
}
// Read Sensor data
bool AP_Compass_HMC5843::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
}
if (!healthy) {
if (millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = millis() + 1000;
return false;
}
}
if (!read_raw()) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = millis() + 1000;
I2c.setSpeed(false);
return false;
}
mag_x *= calibration[0];
mag_y *= calibration[1];
mag_z *= calibration[2];
last_update = micros(); // record time of update
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
rot_mag.rotate(_orientation);
rot_mag += _offset.get();
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
}
if (!healthy) {
if (millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = millis() + 1000;
return false;
}
}
if (!read_raw()) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = millis() + 1000;
I2c.setSpeed(false);
return false;
}
mag_x *= calibration[0];
mag_y *= calibration[1];
mag_z *= calibration[2];
last_update = micros(); // record time of update
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
rot_mag.rotate(_orientation);
rot_mag += _offset.get();
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
}
// set orientation
void
AP_Compass_HMC5843::set_orientation(enum Rotation rotation)
{
_orientation = rotation;
_orientation = rotation;
}

Write Preview
Loading…
Cancel
Save