amiro-os / components / gyro / l3g4200d.cpp @ 58fe0e0b
History | View | Annotate | Download (2.85 KB)
1 |
#include <string.h> |
---|---|
2 |
|
3 |
#include <amiro/util/util.h> |
4 |
#include <amiro/bus/spi/HWSPIDriver.hpp> |
5 |
#include <amiro/gyro/l3g4200d.hpp> |
6 |
|
7 |
namespace amiro {
|
8 |
|
9 |
L3G4200D::L3G4200D(HWSPIDriver *driver) |
10 |
: driver(driver) { |
11 |
|
12 |
} |
13 |
|
14 |
L3G4200D::~L3G4200D() { |
15 |
|
16 |
} |
17 |
|
18 |
chibios_rt::EvtSource* |
19 |
L3G4200D::getEventSource() { |
20 |
return &this->eventSource; |
21 |
} |
22 |
|
23 |
msg_t L3G4200D::main() { |
24 |
|
25 |
this->setName("L3g4200d"); |
26 |
|
27 |
while (!this->shouldTerminate()) { |
28 |
|
29 |
updateSensorData(); |
30 |
|
31 |
this->eventSource.broadcastFlags(1); |
32 |
|
33 |
this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200)); |
34 |
|
35 |
} |
36 |
return RDY_OK;
|
37 |
} |
38 |
|
39 |
int16_t |
40 |
L3G4200D:: |
41 |
getAngularRate(const uint8_t axis) {
|
42 |
|
43 |
return this->angularRate[axis]; |
44 |
|
45 |
} |
46 |
|
47 |
void L3G4200D::updateSensorData() {
|
48 |
|
49 |
const size_t buffer_size = offsetof(L3G4200D::registers, OUT_Z)
|
50 |
- offsetof(L3G4200D::registers, STATUS_REG) |
51 |
+ MEMBER_SIZE(L3G4200D::registers, OUT_Z) |
52 |
+ 1; /* addressing */ |
53 |
uint8_t buffer[buffer_size]; |
54 |
uint8_t sreg; |
55 |
|
56 |
/*Address of first data register*/
|
57 |
memset(buffer, 0xFF, sizeof(buffer)); |
58 |
buffer[0] = offsetof(L3G4200D::registers, STATUS_REG) | L3G4200D::SPI_READ | L3G4200D::SPI_MULT;
|
59 |
|
60 |
this->driver->exchange(buffer, buffer, buffer_size);
|
61 |
|
62 |
// assemble data
|
63 |
sreg = buffer[1];
|
64 |
|
65 |
if (sreg & L3G4200D::XDA)
|
66 |
this->angularRate[L3G4200D::AXIS_X] = (buffer[3] << 8) + buffer[2]; |
67 |
|
68 |
if (sreg & L3G4200D::YDA)
|
69 |
this->angularRate[L3G4200D::AXIS_Y] = (buffer[5] << 8) + buffer[4]; |
70 |
|
71 |
if (sreg & L3G4200D::ZDA)
|
72 |
this->angularRate[L3G4200D::AXIS_Z] = (buffer[7] << 8) + buffer[6]; |
73 |
|
74 |
} |
75 |
|
76 |
msg_t L3G4200D::configure(const L3G4200DConfig *config) {
|
77 |
|
78 |
const size_t ctrl_reg_size = offsetof(L3G4200D::registers, CTRL_REG5)
|
79 |
- offsetof(L3G4200D::registers, CTRL_REG1) |
80 |
+ MEMBER_SIZE(L3G4200D::registers, CTRL_REG5) |
81 |
+ 1; /* addressing */ |
82 |
|
83 |
const size_t buffer_size = ctrl_reg_size;
|
84 |
|
85 |
uint8_t buffer[buffer_size]; |
86 |
|
87 |
// write control config
|
88 |
// this might be three-wire so we need to send ones
|
89 |
memset(buffer, 0xFFu, buffer_size);
|
90 |
buffer[0] = offsetof(L3G4200D::registers, CTRL_REG1) | L3G4200D::SPI_MULT | L3G4200D::SPI_WRITE;
|
91 |
buffer[1] = config->ctrl1;
|
92 |
buffer[2] = config->ctrl2;
|
93 |
buffer[3] = config->ctrl3;
|
94 |
buffer[4] = config->ctrl4;
|
95 |
buffer[5] = config->ctrl5;
|
96 |
this->driver->write(buffer, 6); |
97 |
|
98 |
return RDY_OK;
|
99 |
|
100 |
} |
101 |
|
102 |
uint8_t L3G4200D::getCheck() { |
103 |
|
104 |
const size_t buffer_size = 1 /* addressing */ |
105 |
+ 1; /* who am i */ |
106 |
uint8_t buffer[buffer_size]; |
107 |
|
108 |
// Exchange the data with the L3G4200D gyroscope
|
109 |
// Specify the adress and the mode
|
110 |
buffer[0] = offsetof(L3G4200D::registers, WHO_AM_I) | L3G4200D::SPI_READ;
|
111 |
this->driver->exchange(buffer, buffer, buffer_size);
|
112 |
// Check
|
113 |
if (buffer[1] == L3G4200D::L3G4200D_ID) { |
114 |
return L3G4200D::CHECK_OK;
|
115 |
} else {
|
116 |
return L3G4200D::CHECK_FAIL;
|
117 |
} |
118 |
|
119 |
} |
120 |
|
121 |
} /* amiro */
|