mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_Compass/AP_Compass_HMC5843.cpp
This commit is contained in:
uncrustify
Pat Hickey
parent
7840eebaef
commit
4f9c6bbb19
|
|
@ -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 @@
|
|||
// 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];
|
||||
uint8_t buff[6];
|
||||
|
||||
if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
|
||||
healthy = false;
|
||||
return false;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
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);
|
||||
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;
|
||||
}
|
||||
// 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++;
|
||||
calibration[0] = 0;
|
||||
calibration[1] = 0;
|
||||
calibration[2] = 0;
|
||||
|
||||
// force positiveBias (compass should return 715 for all channels)
|
||||
if (! write_register(ConfigRegA, PositiveBiasConfig))
|
||||
continue; // compass not responding on the bus
|
||||
delay(50);
|
||||
while ( success == 0 && numAttempts < 20 && good_count < 5)
|
||||
{
|
||||
// record number of attempts at initialisation
|
||||
numAttempts++;
|
||||
|
||||
// set gains
|
||||
if (! write_register(ConfigRegB, calibration_gain) ||
|
||||
! write_register(ModeRegister, SingleConversion))
|
||||
continue;
|
||||
// force positiveBias (compass should return 715 for all channels)
|
||||
if (!write_register(ConfigRegA, PositiveBiasConfig))
|
||||
continue; // compass not responding on the bus
|
||||
delay(50);
|
||||
|
||||
// read values from the compass
|
||||
delay(50);
|
||||
if (!read_raw())
|
||||
continue; // we didn't read valid values
|
||||
// set gains
|
||||
if (!write_register(ConfigRegB, calibration_gain) ||
|
||||
!write_register(ModeRegister, SingleConversion))
|
||||
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);
|
||||
cal[1] = fabs(expected_yz / (float)mag_y);
|
||||
cal[2] = fabs(expected_yz / (float)mag_z);
|
||||
float cal[3];
|
||||
|
||||
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];
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
// leave test mode
|
||||
if (!re_initialise()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_initialised = true;
|
||||
_initialised = true;
|
||||
|
||||
return success;
|
||||
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 (!_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;
|
||||
}
|
||||
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];
|
||||
mag_x *= calibration[0];
|
||||
mag_y *= calibration[1];
|
||||
mag_z *= calibration[2];
|
||||
|
||||
last_update = micros(); // record time of update
|
||||
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);
|
||||
// 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;
|
||||
rot_mag += _offset.get();
|
||||
mag_x = rot_mag.x;
|
||||
mag_y = rot_mag.y;
|
||||
mag_z = rot_mag.z;
|
||||
healthy = true;
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
// set orientation
|
||||
void
|
||||
AP_Compass_HMC5843::set_orientation(enum Rotation rotation)
|
||||
{
|
||||
_orientation = rotation;
|
||||
_orientation = rotation;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue