Skip to content

Commit 928da1a

Browse files
LiquidCGSLiquidCGS
committed
Add support for AK8975 mag
1 parent 2fbb811 commit 928da1a

15 files changed

Lines changed: 1171 additions & 0 deletions

‎datasheets/AK8975-Datasheet.pdf‎

402 KB
Binary file not shown.

‎src/F_AK8975.cpp‎

Lines changed: 195 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,195 @@
1+
#include "F_AK8975.hpp"
2+
3+
4+
int AK8975::init(calData cal, uint8_t address)
5+
{
6+
//initialize address variable and calibration data.
7+
IMUAddress = address;
8+
9+
if (cal.valid == false)
10+
{
11+
calibration = { 0 };
12+
calibration.magScale[0] = 1.f;
13+
calibration.magScale[1] = 1.f;
14+
calibration.magScale[2] = 1.f;
15+
}
16+
else
17+
{
18+
calibration = cal;
19+
}
20+
21+
if (!(readByte(AK8975_ADDRESS, AK8975_WHO_AM_I) == AK8975_WHOAMI_DEFAULT_VALUE)) {
22+
return -1;
23+
}
24+
25+
// First extract the factory calibration for each magnetometer axis
26+
uint8_t rawData[3]; // x/y/z gyro calibration data stored here
27+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x00); // Power down magnetometer
28+
delay(10);
29+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x0F); // Enter Fuse ROM access mode
30+
delay(10);
31+
readBytes(AK8975_ADDRESS, AK8975_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
32+
factoryMagCal[0] = (float)(rawData[0] - 128) / 256. + 1.; // Return x-axis sensitivity adjustment values, etc.
33+
factoryMagCal[1] = (float)(rawData[1] - 128) / 256. + 1.;
34+
factoryMagCal[2] = (float)(rawData[2] - 128) / 256. + 1.;
35+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x00); // Power down magnetometer
36+
delay(10);
37+
// Configure the magnetometer for continuous read and highest resolution
38+
// set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
39+
// and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
40+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x01); // Set magnetometer to single measure mode.
41+
delay(10);
42+
return 0;
43+
}
44+
45+
void AK8975::update() {
46+
if (dataAvailable())
47+
{
48+
int16_t magCount[3] = { 0, 0, 0 }; // Stores the 16-bit signed magnetometer sensor output
49+
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
50+
readBytes(AK8975_ADDRESS, AK8975_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
51+
uint8_t c = rawData[6]; // End data read by reading ST2 register
52+
if (!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
53+
magCount[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
54+
magCount[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
55+
magCount[2] = ((int16_t)rawData[5] << 8) | rawData[4];
56+
}
57+
58+
float mx, my, mz;
59+
60+
mx = (float)(magCount[1] * mRes * factoryMagCal[1] - calibration.magBias[1]) * calibration.magScale[1];
61+
my = (float)(magCount[0] * mRes * factoryMagCal[0] - calibration.magBias[0]) * calibration.magScale[0]; // get actual magnetometer value, this depends on scale being set
62+
mz = -(float)(magCount[2] * mRes * factoryMagCal[2] - calibration.magBias[2]) * calibration.magScale[2];
63+
64+
switch (geometryIndex) {
65+
case 0:
66+
mag.magX = mx;
67+
mag.magY = my;
68+
mag.magZ = mz;
69+
break;
70+
case 1:
71+
mag.magX = -my;
72+
mag.magY = mx;
73+
mag.magZ = mz;
74+
break;
75+
case 2:
76+
mag.magX = mx;
77+
mag.magY = my;
78+
mag.magZ = mz;
79+
break;
80+
case 3:
81+
mag.magX = my;
82+
mag.magY = -mx;
83+
mag.magZ = mz;
84+
break;
85+
case 4:
86+
mag.magX = -mz;
87+
mag.magY = -my;
88+
mag.magZ = -mx;
89+
break;
90+
case 5:
91+
mag.magX = -mz;
92+
mag.magY = mx;
93+
mag.magZ = -my;
94+
break;
95+
case 6:
96+
mag.magX = -mz;
97+
mag.magY = my;
98+
mag.magZ = mx;
99+
break;
100+
case 7:
101+
mag.magX = -mz;
102+
mag.magY = -mx;
103+
mag.magZ = my;
104+
break;
105+
}
106+
}
107+
else
108+
{
109+
if(!readByte(AK8975_ADDRESS, AK8975_CNTL) & 0x01){
110+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x01); // Set magnetometer to single measure mode.
111+
}
112+
}
113+
// // Apply mag soft iron error compensation
114+
// mx = x * calibration.mag_softiron_matrix[0][0] + y * calibration.mag_softiron_matrix[0][1] + z * calibration.mag_softiron_matrix[0][2];
115+
// my = x * calibration.mag_softiron_matrix[1][0] + y * calibration.mag_softiron_matrix[1][1] + z * calibration.mag_softiron_matrix[1][2];
116+
// mz = x * calibration.mag_softiron_matrix[2][0] + y * calibration.mag_softiron_matrix[2][1] + z * calibration.mag_softiron_matrix[2][2];
117+
}
118+
119+
void AK8975::getAccel(AccelData* out)
120+
{
121+
return;
122+
}
123+
void AK8975::getGyro(GyroData* out)
124+
{
125+
return;
126+
}
127+
void AK8975::getMag(MagData* out)
128+
{
129+
memcpy(out, &mag, sizeof(mag));
130+
}
131+
132+
int AK8975::setAccelRange(int range) {
133+
return -1;
134+
}
135+
136+
int AK8975::setGyroRange(int range) {
137+
return -1;
138+
}
139+
140+
void AK8975::calibrateAccelGyro(calData* cal)
141+
{
142+
return;
143+
}
144+
145+
void AK8975::calibrateMag(calData* cal)
146+
{
147+
uint16_t ii = 0, sample_count = 0;
148+
int32_t mag_bias[3] = { 0, 0, 0 }, mag_scale[3] = { 0, 0, 0 };
149+
int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] = { 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 };
150+
151+
// shoot for ~fifteen seconds of mag data
152+
sample_count = 1024; // at 100 Hz ODR, new mag data is available every 10 ms
153+
154+
for (ii = 0; ii < sample_count; ii++)
155+
{
156+
writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x01); // Set magnetometer to single measure mode.
157+
delay(15); // at 100 Hz ODR, new mag data is available every 10 ms
158+
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
159+
if (readByte(AK8975_ADDRESS, AK8975_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
160+
readBytes(AK8975_ADDRESS, AK8975_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
161+
uint8_t c = rawData[6]; // End data read by reading ST2 register
162+
if (!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
163+
mag_temp[0] = ((int16_t)rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value
164+
mag_temp[1] = ((int16_t)rawData[3] << 8) | rawData[2]; // Data stored as little Endian
165+
mag_temp[2] = ((int16_t)rawData[5] << 8) | rawData[4];
166+
}
167+
for (int jj = 0; jj < 3; jj++)
168+
{
169+
if (mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
170+
if (mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
171+
}
172+
}
173+
}
174+
175+
// Get hard iron correction
176+
mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts
177+
mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts
178+
mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts
179+
180+
cal->magBias[0] = (float)mag_bias[0] * mRes * factoryMagCal[0]; // save mag biases in G for main program
181+
cal->magBias[1] = (float)mag_bias[1] * mRes * factoryMagCal[1];
182+
cal->magBias[2] = (float)mag_bias[2] * mRes * factoryMagCal[2];
183+
184+
// Get soft iron correction estimate
185+
mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; // get average x axis max chord length in counts
186+
mag_scale[1] = (mag_max[1] - mag_min[1]) / 2; // get average y axis max chord length in counts
187+
mag_scale[2] = (mag_max[2] - mag_min[2]) / 2; // get average z axis max chord length in counts
188+
189+
float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
190+
avg_rad /= 3.0;
191+
192+
cal->magScale[0] = avg_rad / ((float)mag_scale[0]);
193+
cal->magScale[1] = avg_rad / ((float)mag_scale[1]);
194+
cal->magScale[2] = avg_rad / ((float)mag_scale[2]);
195+
}

‎src/F_AK8975.hpp‎

Lines changed: 123 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
#pragma once
2+
3+
#ifndef _F_AK8975_H_
4+
#define _F_AK8975_H_
5+
6+
#include "IMUBase.hpp"
7+
/*
8+
9+
AK8975 REGISTERS
10+
11+
*/
12+
13+
//Magnetometer Registers
14+
#define AK8975_ADDRESS 0x0C
15+
#define AK8975_WHO_AM_I 0x00 // should return 0x48
16+
#define AK8975_WHOAMI_DEFAULT_VALUE 0x48
17+
#define AK8975_INFO 0x01
18+
#define AK8975_ST1 0x02 // data ready status bit 0
19+
#define AK8975_XOUT_L 0x03 // data
20+
#define AK8975_XOUT_H 0x04
21+
#define AK8975_YOUT_L 0x05
22+
#define AK8975_YOUT_H 0x06
23+
#define AK8975_ZOUT_L 0x07
24+
#define AK8975_ZOUT_H 0x08
25+
#define AK8975_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
26+
#define AK8975_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
27+
#define AK8975_ASTC 0x0C // Self test control
28+
#define AK8975_I2CDIS 0x0F // I2C disable
29+
#define AK8975_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
30+
#define AK8975_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
31+
#define AK8975_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
32+
33+
34+
class AK8975 : public IMUBase {
35+
public:
36+
AK8975() {};
37+
38+
// Inherited via IMUBase
39+
int init(calData cal, uint8_t address) override;
40+
41+
void update() override;
42+
void getAccel(AccelData* out) override;
43+
void getGyro(GyroData* out) override;
44+
void getMag(MagData* out) override;
45+
void getQuat(Quaternion* out) override {};
46+
float getTemp() override { return 0.f; };
47+
48+
int setGyroRange(int range) override;
49+
int setAccelRange(int range) override;
50+
int setIMUGeometry(int index) override { geometryIndex = index; return 0; };
51+
52+
void calibrateAccelGyro(calData* cal) override;
53+
void calibrateMag(calData* cal) override;
54+
55+
bool hasMagnetometer() override {
56+
return true;
57+
}
58+
bool hasTemperature() override {
59+
return false;
60+
}
61+
bool hasQuatOutput() override {
62+
return false;
63+
}
64+
65+
String IMUName() override {
66+
return "AK8975";
67+
}
68+
String IMUType() override {
69+
return "AK8975";
70+
}
71+
String IMUManufacturer() override {
72+
return "AsahiKASEI";
73+
}
74+
private:
75+
float mRes = 10. * 1229. / 8196.0; //mres value for full range (1229uT) readings
76+
77+
int geometryIndex = 0;
78+
float temperature = 0.f;
79+
MagData mag = { 0 };
80+
81+
calData calibration;
82+
uint8_t IMUAddress;
83+
84+
85+
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
86+
{
87+
Wire.beginTransmission(address); // Initialize the Tx buffer
88+
Wire.write(subAddress); // Put slave register address in Tx buffer
89+
Wire.write(data); // Put data in Tx buffer
90+
Wire.endTransmission(); // Send the Tx buffer
91+
}
92+
93+
uint8_t readByte(uint8_t address, uint8_t subAddress)
94+
{
95+
uint8_t data; // `data` will store the register data
96+
Wire.beginTransmission(address); // Initialize the Tx buffer
97+
Wire.write(subAddress); // Put slave register address in Tx buffer
98+
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
99+
Wire.requestFrom(address, (uint8_t)1); // Read one byte from slave register address
100+
data = Wire.read(); // Fill Rx buffer with result
101+
return data; // Return data read from slave register
102+
}
103+
104+
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest)
105+
{
106+
Wire.beginTransmission(address); // Initialize the Tx buffer
107+
Wire.write(subAddress); // Put slave register address in Tx buffer
108+
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
109+
uint8_t i = 0;
110+
Wire.requestFrom(address, count); // Read bytes from slave register address
111+
while (Wire.available()) {
112+
dest[i++] = Wire.read();
113+
} // Put read results in the Rx buffer
114+
}
115+
116+
float factoryMagCal[3] = { 0 };
117+
118+
bool dataAvailable()
119+
{
120+
return (readByte(AK8975_ADDRESS, AK8975_ST1) & 0x01);
121+
}
122+
};
123+
#endif /* _F_AK8975_H_ */

‎src/F_BMI055_AK8975.hpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
#pragma once
2+
3+
#ifndef _F_BMI055_QMC5883L_H_
4+
#define _F_BMI055_QMC5883L_H_
5+
6+
#include "F_BMI055.hpp"
7+
#include "F_QMC5883L.hpp"
8+
9+
class BMI055_QMC5883L : public IMUBase {
10+
public:
11+
BMI055_QMC5883L() {};
12+
13+
// Inherited via IMUBase
14+
int init(calData cal, uint8_t address) override {
15+
int e = IMU.init(cal, address);
16+
if (e) { return e; }
17+
e = MAG.init(cal, 0x0D);
18+
if (e) { return -2; }
19+
return 0;
20+
}
21+
22+
void update() override {
23+
IMU.update();
24+
MAG.update();
25+
}
26+
void getAccel(AccelData* out) override {
27+
IMU.getAccel(out);
28+
}
29+
void getGyro(GyroData* out) override{
30+
IMU.getGyro(out);
31+
}
32+
void getMag(MagData* out) override {
33+
MAG.getMag(out);
34+
}
35+
void getQuat(Quaternion* out) override {};
36+
float getTemp() override { return IMU.getTemp();; };
37+
38+
int setGyroRange(int range) override { return IMU.setGyroRange(range); }
39+
int setAccelRange(int range) override { return IMU.setAccelRange(range); }
40+
int setIMUGeometry(int index) override { return IMU.setIMUGeometry(index); }
41+
42+
void calibrateAccelGyro(calData* cal) override {
43+
IMU.calibrateAccelGyro(cal);
44+
}
45+
virtual void calibrateMag(calData* cal) override {
46+
MAG.calibrateMag(cal);
47+
}
48+
49+
bool hasMagnetometer() override {
50+
return true;
51+
}
52+
bool hasTemperature() override {
53+
return true;
54+
}
55+
bool hasQuatOutput() override {
56+
return false;
57+
}
58+
59+
String IMUName() override {
60+
return "BMI-055 + QMC5883L";
61+
}
62+
String IMUType() override {
63+
return "BMI055_QMC5883L";
64+
}
65+
String IMUManufacturer() override {
66+
return "Bosch + QST";
67+
}
68+
69+
private:
70+
BMI055 IMU;
71+
QMC5883L MAG;
72+
73+
calData calibration;
74+
uint8_t IMUAddress;
75+
};
76+
#endif /* _F_BMI055_QMC5883L_H_ */

0 commit comments

Comments
 (0)