-
Notifications
You must be signed in to change notification settings - Fork 37
/
HMC5883L.c
242 lines (200 loc) · 11 KB
/
HMC5883L.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
/* Copyright (C) 2015 Kristian Sloth Lauszus. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Sloth Lauszus
Web : http://www.lauszus.com
e-mail : [email protected]
*/
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
#if USE_MAG
#include "Config.h"
#include "EEPROM.h"
#include "HMC5883L.h"
#include "I2C.h"
#include "Time.h"
#include "Types.h"
#include "driverlib/gpio.h"
#include "driverlib/interrupt.h"
#include "inc/hw_ints.h"
#if UART_DEBUG
#include "utils/uartstdio.h" // Add "UART_BUFFERED" to preprocessor
#endif
#define HMC5883L_ADDRESS 0x1E // Address of magnetometer
#define HMC5883L_CONF_REG_A 0x00
#define HMC5883L_CONF_REG_B 0x01
#define HMC5883L_MODE_REG 0x02
#define HMC5883L_OUTPUT_REG_X_MSB 0x03
#define HMC5883L_OUTPUT_REG_X_LSB 0x04
#define HMC5883L_OUTPUT_REG_Z_MSB 0x05
#define HMC5883L_OUTPUT_REG_Z_LSB 0x06
#define HMC5883L_OUTPUT_REG_Y_MSB 0x07
#define HMC5883L_OUTPUT_REG_Y_LSB 0x08
#define HMC5883L_STATUS_REG_ 0x09
#define HMC5883L_ID_REG_A 0x0A
#define HMC5883L_ID_REG_B 0x0B
#define HMC5883L_ID_REG_C 0x0C
// Configuration Register A
#define HMC5883L_CONF_REG_A_MA_8 (3 << 5) // 8 sample averages
#define HMC5883L_CONF_REG_A_D0_15 (4 << 2) // Data output rate to 15 Hz (default)
#define HMC5883L_CONF_REG_A_MS_NORM (0 << 0) // Normal measurement mode (default)
#define HMC5883L_CONF_REG_A_MS_POS (1 << 0) // Positive bias mode
#define HMC5883L_CONF_REG_A_MS_NEG (2 << 0) // Negative bias mode
// Configuration Register B
#define HMC5883L_CONF_REG_B_GN_13 (1 << 5) // 1.3 Ga gain (default)
#define HMC5883L_CONF_REG_B_GN_25 (3 << 5) // 2.5 Ga gain
// Mode Register
#define HMC5883L_MODE_REG_HS (1 << 7) // I2C high speed mode (400 kHz)
#define HMC5883L_MODE_REG_MD_CONT (0 << 0) // Continuous measurement mode
#define HMC5883L_MODE_REG_MD_SINGLE (1 << 0) // Single measurement mode
#define HMC5883L_X_SELF_TEST_GAUSS 1.16f // X axis level when bias current is applied
#define HMC5883L_Y_SELF_TEST_GAUSS 1.16f // Y axis level when bias current is applied
#define HMC5883L_Z_SELF_TEST_GAUSS 1.08f // Z axis level when bias current is applied
#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when Gain = 5 (4.7 Ga)
#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when Gain = 5 (4.7 Ga)
#define GAIN_13_GA_LSB_GAIN 1090.0f // Gain (LSB/Gauss) for 1.3 Ga gain (default)
#define GAIN_25_GA_LSB_GAIN 660.0f // Gain (LSB/Gauss) for 2.5 Ga gain
static struct hmc5883l_t {
sensorRaw_t magRaw; // Raw magnetometer readings
sensor_t magGain; // Magnetometer gain
} hmc5883l;
static volatile bool dataReady;
// Returns true when data is ready to be read
bool dataReadyHMC5883L(void) {
bool dataReadyTemp = dataReady;
dataReady = false; // Clear flag
return dataReadyTemp;
}
static void drdyHandler(void) {
GPIOIntClear(GPIO_HMC5883L_DRDY_BASE, GPIO_HMC5883L_DRDY_PIN); // Clear interrupt source
dataReady = true;
}
// X-axis should be facing forward
// Y-axis should be facing to the right
// Z-axis should be facing downward
static void hmc5883lBoardOrientation(sensor_t *sensor) {
sensor_t sensorTemp = *sensor;
sensor->axis.X = sensorTemp.axis.Y;
sensor->axis.Y = -sensorTemp.axis.X;
sensor->axis.Z = sensorTemp.axis.Z;
}
static void getHMC5883LDataRaw(sensorRaw_t *magRaw) {
uint8_t buf[6];
i2cReadData(HMC5883L_ADDRESS, HMC5883L_OUTPUT_REG_X_MSB, buf, 6); // Get magnetometer values
magRaw->axis.X = (int16_t)((buf[0] << 8) | buf[1]);
magRaw->axis.Z = (int16_t)((buf[2] << 8) | buf[3]);
magRaw->axis.Y = (int16_t)((buf[4] << 8) | buf[5]);
}
void getHMC5883LData(sensor_t *mag, bool calibrating) {
getHMC5883LDataRaw(&hmc5883l.magRaw); // Get raw reading
for (uint8_t axis = 0; axis < 3; axis++)
mag->data[axis] = (float)hmc5883l.magRaw.data[axis] * hmc5883l.magGain.data[axis]; // Apply gain
hmc5883lBoardOrientation(mag); // Apply board orientation
/*
// The value should be positive when pointing at north
UARTprintf("%d\t%d\t%d\n", hmc5883l.magRaw.axis.X, hmc5883l.magRaw.axis.Y, hmc5883l.magRaw.axis.Z);
UARTFlushTx(false);
*/
if (!calibrating) { // If we are not calibrating, then subtract zero values
for (uint8_t axis = 0; axis < 3; axis++)
mag->data[axis] -= cfg.magZero.data[axis]; // Subtract zero value stored in EEPROM
}
#if 0 && UART_DEBUG
UARTprintf("%d.%03u\t%d.%03u\t%d.%03u\n",
(int16_t)mag->axis.X, (uint16_t)(abs(mag->axis.X * 1000.0f) % 1000),
(int16_t)mag->axis.Y, (uint16_t)(abs(mag->axis.Y * 1000.0f) % 1000),
(int16_t)mag->axis.Z, (uint16_t)(abs(mag->axis.Z * 1000.0f) % 1000));
UARTFlushTx(false);
#endif
}
static bool checkLimit(sensorRaw_t sensorRaw, int16_t low, int16_t high) {
bool status = true;
for (uint8_t axis = 0; axis < 3; axis++) {
if (sensorRaw.data[axis] < low || sensorRaw.data[axis] > high) {
status = false;
break;
}
}
return status;
}
// Inspired by: https://code.google.com/p/open-headtracker and https://github.com/cleanflight/cleanflight/blob/master/src/main/drivers/compass_hmc5883l.c
bool initHMC5883L(void) {
uint8_t buf[3]; // Buffer for I2C data
i2cReadData(HMC5883L_ADDRESS, HMC5883L_ID_REG_A, buf, 3);
if (buf[0] != 'H' || buf[1] != '4' || buf[2] != '3') { // Read identification registers
#if 0 && UART_DEBUG
UARTprintf("Could not find HMC5883L: %c%c%c\n", buf[0], buf[1], buf[2]);
#endif
return false;
}
// Enable interrupt for DRDY (Data Ready Interrupt Pin)
SysCtlPeripheralEnable(GPIO_HMC5883L_DRDY_PERIPH); // Enable GPIO peripheral
SysCtlDelay(2); // Insert a few cycles after enabling the peripheral to allow the clock to be fully activated
GPIOPinTypeGPIOInput(GPIO_HMC5883L_DRDY_BASE, GPIO_HMC5883L_DRDY_PIN); // Set DRDY pin as input
GPIOIntTypeSet(GPIO_HMC5883L_DRDY_BASE, GPIO_HMC5883L_DRDY_PIN, GPIO_FALLING_EDGE); // Enable interrupt on falling
IntPrioritySet(INT_GPIOF, 2); // Set interrupt priority to 2
GPIOIntEnable(GPIO_HMC5883L_DRDY_BASE, GPIO_HMC5883L_DRDY_PIN); // Enable interrupt
GPIOIntRegister(GPIO_HMC5883L_DRDY_BASE, drdyHandler); // Register interrupt handler
// Self test according to datasheet: http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/HMC5883L_3-Axis_Digital_Compass_IC.pdf page 19
// Only difference is that I use 2.5 Ga
static const int16_t LOW_LIMIT = SELF_TEST_LOW_LIMIT * GAIN_25_GA_LSB_GAIN;
static const int16_t HIGH_LIMIT = SELF_TEST_HIGH_LIMIT * GAIN_25_GA_LSB_GAIN;
i2cWrite(HMC5883L_ADDRESS, HMC5883L_CONF_REG_A, HMC5883L_CONF_REG_A_MA_8 | HMC5883L_CONF_REG_A_D0_15 | HMC5883L_CONF_REG_A_MS_POS); // Set to 8 samples, update rate to 15 Hz (default) and positive bias configuration
i2cWrite(HMC5883L_ADDRESS, HMC5883L_CONF_REG_B, HMC5883L_CONF_REG_B_GN_25); // Set gain to 2.5 Ga
i2cWrite(HMC5883L_ADDRESS, HMC5883L_MODE_REG, HMC5883L_MODE_REG_HS | HMC5883L_MODE_REG_MD_CONT); // Configure device for high speed I2C mode (400 kHz) in continuous mode
while (!dataReadyHMC5883L()); // Wait for data
getHMC5883LDataRaw(&hmc5883l.magRaw); // First values are discarded, as it is from previous setting
delay(100); // This delays seems to prevent it from failing from time to time
while (!dataReadyHMC5883L()); // Wait for data
getHMC5883LDataRaw(&hmc5883l.magRaw); // Read positive bias values
if (!checkLimit(hmc5883l.magRaw, LOW_LIMIT, HIGH_LIMIT)) {
#if UART_DEBUG
UARTprintf("HMC5883L self test high limit failed: %d < %d %d %d < %d\n", LOW_LIMIT, hmc5883l.magRaw.axis.X, hmc5883l.magRaw.axis.Y, hmc5883l.magRaw.axis.Z, HIGH_LIMIT);
#endif
while (1);
}
sensor_t mag_total = { .data = { 0, 0, 0 } };
mag_total.axis.X += hmc5883l.magRaw.axis.X;
mag_total.axis.Y += hmc5883l.magRaw.axis.Y;
mag_total.axis.Z += hmc5883l.magRaw.axis.Z;
i2cWrite(HMC5883L_ADDRESS, HMC5883L_CONF_REG_A, HMC5883L_CONF_REG_A_MA_8 | HMC5883L_CONF_REG_A_D0_15 | HMC5883L_CONF_REG_A_MS_NEG); // Set to 8 samples, update rate to 15 Hz (default) and negative bias configuration
while (!dataReadyHMC5883L()); // Wait for data
getHMC5883LDataRaw(&hmc5883l.magRaw); // First values are discarded, as it is from previous setting
while (!dataReadyHMC5883L()); // Wait for data
getHMC5883LDataRaw(&hmc5883l.magRaw); // Read negative bias values
if (!checkLimit(hmc5883l.magRaw, -HIGH_LIMIT, -LOW_LIMIT)) {
#if UART_DEBUG
UARTprintf("HMC5883L self test low limit failed: %d < %d %d %d < %d\n", -HIGH_LIMIT, hmc5883l.magRaw.axis.X, hmc5883l.magRaw.axis.Y, hmc5883l.magRaw.axis.Z, -LOW_LIMIT);
#endif
while (1);
}
mag_total.axis.X -= hmc5883l.magRaw.axis.X;
mag_total.axis.Y -= hmc5883l.magRaw.axis.Y;
mag_total.axis.Z -= hmc5883l.magRaw.axis.Z;
// Calculate gain
hmc5883l.magGain.axis.X = GAIN_25_GA_LSB_GAIN * HMC5883L_X_SELF_TEST_GAUSS / (mag_total.axis.X / 2.0f);
hmc5883l.magGain.axis.Y = GAIN_25_GA_LSB_GAIN * HMC5883L_Y_SELF_TEST_GAUSS / (mag_total.axis.Y / 2.0f);
hmc5883l.magGain.axis.Z = GAIN_25_GA_LSB_GAIN * HMC5883L_Z_SELF_TEST_GAUSS / (mag_total.axis.Z / 2.0f);
i2cWrite(HMC5883L_ADDRESS, HMC5883L_CONF_REG_A, HMC5883L_CONF_REG_A_MA_8 | HMC5883L_CONF_REG_A_D0_15 | HMC5883L_CONF_REG_A_MS_NORM); // Set to 8 samples, update rate to 15 Hz (default) and normal measurement configuration
i2cWrite(HMC5883L_ADDRESS, HMC5883L_CONF_REG_B, HMC5883L_CONF_REG_B_GN_13); // Set gain to 1.3 Ga (default)
while (!dataReadyHMC5883L()); // Wait for data
getHMC5883LDataRaw(&hmc5883l.magRaw); // First values are discarded, as it is from previous gain
#if 0 && UART_DEBUG
UARTprintf("Mag cal: %d\t%d\t%d\n", (int16_t)mag_total.axis.X, (int16_t)mag_total.axis.Y, (int16_t)mag_total.axis.Z);
UARTprintf("Mag gain: %d.%03u\t%d.%03u\t%d.%03u\n",
(int16_t)hmc5883l.magGain.axis.X, (uint16_t)(abs(hmc5883l.magGain.axis.X * 1000.0f) % 1000),
(int16_t)hmc5883l.magGain.axis.Y, (uint16_t)(abs(hmc5883l.magGain.axis.Y * 1000.0f) % 1000),
(int16_t)hmc5883l.magGain.axis.Z, (uint16_t)(abs(hmc5883l.magGain.axis.Z * 1000.0f) % 1000));
UARTFlushTx(false);
#endif
return true;
}
#endif // USE_MAG