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

uncrustify libraries/AP_Compass/AP_Compass_HMC5843.cpp

This commit is contained in:
uncrustify 2012-08-16 23:19:22 -07:00 committed by Pat Hickey
parent 662d285f44
commit c8ede643dc
1 changed files with 182 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

364
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -1,25 +1,25 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/* /*
AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer * AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
Code by Jordi Muñoz and Jose Julio. DIYDrones.com * Code by Jordi Muñoz and Jose Julio. DIYDrones.com
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*
Sensor is conected to I2C port * Sensor is conected to I2C port
Sensor is initialized in Continuos mode (10Hz) * Sensor is initialized in Continuos mode (10Hz)
*
*/ */
// AVR LibC Includes // AVR LibC Includes
#include <math.h> #include <math.h>
#include <FastSerial.h> #include <FastSerial.h>
#if defined(ARDUINO) && ARDUINO >= 100 #if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h" #include "Arduino.h"
#else #else
#include "WConstants.h" #include "WConstants.h"
#endif #endif
#include <I2C.h> #include <I2C.h>
@ -54,233 +54,233 @@
// read_register - read a register value // read_register - read a register value
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value) bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
{ {
if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) { if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
healthy = false; healthy = false;
return false; return false;
} }
return true; return true;
} }
// write_register - update a register value // write_register - update a register value
bool AP_Compass_HMC5843::write_register(uint8_t address, byte value) bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
{ {
if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) { if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
healthy = false; healthy = false;
return false; return false;
} }
return true; return true;
} }
// Read Sensor data // Read Sensor data
bool AP_Compass_HMC5843::read_raw() bool AP_Compass_HMC5843::read_raw()
{ {
uint8_t buff[6]; uint8_t buff[6];
if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) { if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
healthy = false; healthy = false;
return false; return false;
} }
int16_t rx, ry, rz; int16_t rx, ry, rz;
rx = (int16_t)(buff[0] << 8) | buff[1]; rx = (int16_t)(buff[0] << 8) | buff[1];
if (product_id == AP_COMPASS_TYPE_HMC5883L) { if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rz = (int16_t)(buff[2] << 8) | buff[3]; rz = (int16_t)(buff[2] << 8) | buff[3];
ry = (int16_t)(buff[4] << 8) | buff[5]; ry = (int16_t)(buff[4] << 8) | buff[5];
} else { } else {
ry = (int16_t)(buff[2] << 8) | buff[3]; ry = (int16_t)(buff[2] << 8) | buff[3];
rz = (int16_t)(buff[4] << 8) | buff[5]; rz = (int16_t)(buff[4] << 8) | buff[5];
} }
if (rx == -4096 || ry == -4096 || rz == -4096) { if (rx == -4096 || ry == -4096 || rz == -4096) {
// no valid data available // no valid data available
return false; return false;
} }
mag_x = -rx; mag_x = -rx;
mag_y = ry; mag_y = ry;
mag_z = -rz; mag_z = -rz;
return true; return true;
} }
/* /*
re-initialise after a IO error * re-initialise after a IO error
*/ */
bool AP_Compass_HMC5843::re_initialise() bool AP_Compass_HMC5843::re_initialise()
{ {
if (! write_register(ConfigRegA, _base_config) || if (!write_register(ConfigRegA, _base_config) ||
! write_register(ConfigRegB, magGain) || !write_register(ConfigRegB, magGain) ||
! write_register(ModeRegister, ContinuousConversion)) !write_register(ModeRegister, ContinuousConversion))
return false; return false;
return true; return true;
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool bool
AP_Compass_HMC5843::init() AP_Compass_HMC5843::init()
{ {
int numAttempts = 0, good_count = 0; int numAttempts = 0, good_count = 0;
bool success = false; bool success = false;
byte calibration_gain = 0x20; byte calibration_gain = 0x20;
uint16_t expected_x = 715; uint16_t expected_x = 715;
uint16_t expected_yz = 715; uint16_t expected_yz = 715;
float gain_multiple = 1.0; float gain_multiple = 1.0;
delay(10); delay(10);
// determine if we are using 5843 or 5883L // determine if we are using 5843 or 5883L
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
! read_register(ConfigRegA, &_base_config)) { !read_register(ConfigRegA, &_base_config)) {
healthy = false; healthy = false;
return false; return false;
} }
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config // a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L; product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60; calibration_gain = 0x60;
expected_x = 766; expected_x = 766;
expected_yz = 713; expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843; product_id = AP_COMPASS_TYPE_HMC5843;
} else { } else {
// not behaving like either supported compass type // not behaving like either supported compass type
return false; return false;
} }
calibration[0] = 0; calibration[0] = 0;
calibration[1] = 0; calibration[1] = 0;
calibration[2] = 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) while ( success == 0 && numAttempts < 20 && good_count < 5)
if (! write_register(ConfigRegA, PositiveBiasConfig)) {
continue; // compass not responding on the bus // record number of attempts at initialisation
delay(50); numAttempts++;
// set gains // force positiveBias (compass should return 715 for all channels)
if (! write_register(ConfigRegB, calibration_gain) || if (!write_register(ConfigRegA, PositiveBiasConfig))
! write_register(ModeRegister, SingleConversion)) continue; // compass not responding on the bus
continue; delay(50);
// read values from the compass // set gains
delay(50); if (!write_register(ConfigRegB, calibration_gain) ||
if (!read_raw()) !write_register(ModeRegister, SingleConversion))
continue; // we didn't read valid values continue;
delay(10); // read values from the compass
delay(50);
if (!read_raw())
continue; // we didn't read valid values
float cal[3]; delay(10);
cal[0] = fabs(expected_x / (float)mag_x); float cal[3];
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[0] = fabs(expected_x / (float)mag_x);
cal[1] > 0.7 && cal[1] < 1.3 && cal[1] = fabs(expected_yz / (float)mag_y);
cal[2] > 0.7 && cal[2] < 1.3) { cal[2] = fabs(expected_yz / (float)mag_z);
good_count++;
calibration[0] += cal[0]; if (cal[0] > 0.7 && cal[0] < 1.3 &&
calibration[1] += cal[1]; cal[1] > 0.7 && cal[1] < 1.3 &&
calibration[2] += cal[2]; cal[2] > 0.7 && cal[2] < 1.3) {
} good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0 #if 0
/* useful for debugging */ /* useful for debugging */
Serial.print("mag_x: "); Serial.print("mag_x: ");
Serial.print(mag_x); Serial.print(mag_x);
Serial.print(" mag_y: "); Serial.print(" mag_y: ");
Serial.print(mag_y); Serial.print(mag_y);
Serial.print(" mag_z: "); Serial.print(" mag_z: ");
Serial.println(mag_z); Serial.println(mag_z);
Serial.print("CalX: "); Serial.print("CalX: ");
Serial.print(calibration[0]/good_count); Serial.print(calibration[0]/good_count);
Serial.print(" CalY: "); Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count); Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: "); Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count); Serial.println(calibration[2]/good_count);
#endif #endif
} }
if (good_count >= 5) { if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count; calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count; calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count; calibration[2] = calibration[2] * gain_multiple / good_count;
success = true; success = true;
} else { } else {
/* best guess */ /* best guess */
calibration[0] = 1.0; calibration[0] = 1.0;
calibration[1] = 1.0; calibration[1] = 1.0;
calibration[2] = 1.0; calibration[2] = 1.0;
} }
// leave test mode // leave test mode
if (!re_initialise()) { if (!re_initialise()) {
return false; return false;
} }
_initialised = true; _initialised = true;
return success; return success;
} }
// Read Sensor data // Read Sensor data
bool AP_Compass_HMC5843::read() bool AP_Compass_HMC5843::read()
{ {
if (!_initialised) { if (!_initialised) {
// someone has tried to enable a compass for the first time // someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't // mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!) // have the right orientation!)
return false; return false;
} }
if (!healthy) { if (!healthy) {
if (millis() < _retry_time) { if (millis() < _retry_time) {
return false; return false;
} }
if (!re_initialise()) { if (!re_initialise()) {
_retry_time = millis() + 1000; _retry_time = millis() + 1000;
return false; return false;
} }
} }
if (!read_raw()) { if (!read_raw()) {
// try again in 1 second, and set I2c clock speed slower // try again in 1 second, and set I2c clock speed slower
_retry_time = millis() + 1000; _retry_time = millis() + 1000;
I2c.setSpeed(false); I2c.setSpeed(false);
return false; return false;
} }
mag_x *= calibration[0]; mag_x *= calibration[0];
mag_y *= calibration[1]; mag_y *= calibration[1];
mag_z *= calibration[2]; mag_z *= calibration[2];
last_update = micros(); // record time of update last_update = micros(); // record time of update
// rotate to the desired orientation // rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z); Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) { if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90); rot_mag.rotate(ROTATION_YAW_90);
} }
rot_mag.rotate(_orientation); rot_mag.rotate(_orientation);
rot_mag += _offset.get(); rot_mag += _offset.get();
mag_x = rot_mag.x; mag_x = rot_mag.x;
mag_y = rot_mag.y; mag_y = rot_mag.y;
mag_z = rot_mag.z; mag_z = rot_mag.z;
healthy = true; healthy = true;
return true; return true;
} }
// set orientation // set orientation
void void
AP_Compass_HMC5843::set_orientation(enum Rotation rotation) AP_Compass_HMC5843::set_orientation(enum Rotation rotation)
{ {
_orientation = rotation; _orientation = rotation;
} }