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
|
#ifndef HMC5983_H
#define HMC5983_H
#include <spi/spi.h>
#include <gpio/gpio.h>
#include <os/time.h>
class HMC5983 {
private:
Pin cs;
SPI_t& spi;
uint8_t raw_data[6];
void read_register(uint8_t reg, uint8_t len, uint8_t *data) {
cs.off();
if (len > 1) {
spi.transfer_byte(reg | (1 << 8) | (1 << 7));
} else {
spi.transfer_byte(reg | (1 << 8));
}
while(len--) {
*data++ = spi.transfer_byte();
}
cs.on();
}
void write_register(uint8_t reg, uint8_t data) {
cs.off();
spi.transfer_byte(reg);
spi.transfer_byte(data);
cs.on();
}
public:
float x, y, z;
uint8_t ident;
HMC5983(Pin cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {}
void init() {
spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
spi.reg.CR2 = (1 << 12) | ( 7 << 8);
Time::sleep(100);
ident = 'O';
while(ident != 'H'){
read_register(0x0A, 1, &ident);
}
write_register(0x00, (1 << 7) | (1 << 5) | (2 << 2));
write_register(0x01, 0x00);
write_register(0x02, 0x00);
Time::sleep(600);
}
uint8_t update() {
uint16_t raw_x, raw_y, raw_z;
uint8_t status;
spi.reg.CR1 = (1 << 9) | (1 <<8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
spi.reg.CR2 = (1 << 12) | ( 7 << 8);
// Read status;
read_register(0x09, 1, &status);
if (status & 1) {
read_register(0x03, 6, raw_data);
raw_x = raw_data[0] << 8;
raw_x |= raw_data[1];
raw_z = raw_data[2] << 8;
raw_z |= raw_data[3];
raw_y = raw_data[4] << 8;
raw_y |= raw_data[5];
x = float(raw_x);
y = float(raw_y);
z = float(raw_z);
}
return status;
}
};
#endif
|
