amiro-os / components / magneto / hmc5883l.cpp @ 3aee55de
History | View | Annotate | Download (4.443 KB)
1 | 58fe0e0b | Thomas Schöpping | #include <ch.hpp> |
---|---|---|---|
2 | #include <hal.h> |
||
3 | #include <chdebug.h> |
||
4 | |||
5 | #include <amiro/bus/i2c/I2CParams.hpp> |
||
6 | #include <amiro/bus/i2c/I2CDriver.hpp> |
||
7 | #include <amiro/magneto/hmc5883l.hpp> |
||
8 | b4885314 | Thomas Schöpping | #include <amiro/Constants.h> |
9 | 58fe0e0b | Thomas Schöpping | |
10 | using namespace chibios_rt; |
||
11 | |||
12 | namespace amiro {
|
||
13 | |||
14 | HMC5883L::~HMC5883L() { |
||
15 | |||
16 | } |
||
17 | |||
18 | HMC5883L::HMC5883L(I2CDriver *driver, const HMC5883LConfig *config)
|
||
19 | : BaseStaticThread<256>(),
|
||
20 | driver(driver), |
||
21 | config(config) { |
||
22 | |||
23 | this->txParams.addr = HMC5883L::SLA;
|
||
24 | } |
||
25 | |||
26 | EvtSource* |
||
27 | HMC5883L:: |
||
28 | getEventSource() { |
||
29 | |||
30 | return &this->eventSource; |
||
31 | } |
||
32 | |||
33 | msg_t |
||
34 | HMC5883L:: |
||
35 | main(void) {
|
||
36 | |||
37 | msg_t res; |
||
38 | |||
39 | this->setName("Hmc5883l"); |
||
40 | |||
41 | /* exit if writing configuration fails */
|
||
42 | res = this->writeConf();
|
||
43 | |||
44 | if (res) {
|
||
45 | return RDY_RESET;
|
||
46 | } |
||
47 | |||
48 | while (!this->shouldTerminate()) { |
||
49 | |||
50 | this->updateSensorData();
|
||
51 | |||
52 | this->eventSource.broadcastFlags(0); |
||
53 | |||
54 | 8dbafe16 | Thomas Schöpping | this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD);
|
55 | 58fe0e0b | Thomas Schöpping | } |
56 | |||
57 | return RDY_OK;
|
||
58 | } |
||
59 | |||
60 | int16_t |
||
61 | HMC5883L:: |
||
62 | getMagnetization(const uint8_t axis) {
|
||
63 | int16_t returnBuffer; |
||
64 | chSysLock(); |
||
65 | returnBuffer = this->data[axis];
|
||
66 | chSysUnlock(); |
||
67 | return returnBuffer;
|
||
68 | |||
69 | } |
||
70 | |||
71 | int32_t |
||
72 | HMC5883L:: |
||
73 | getMagnetizationGauss(const uint8_t axis) {
|
||
74 | |||
75 | int16_t magLsbBuffer = this->getMagnetization(axis);
|
||
76 | int32_t returnBuffer = 0;
|
||
77 | |||
78 | // The gains are taken from table 9 of the manual
|
||
79 | switch (this->config->ctrlB & GN_7_GA) { |
||
80 | case HMC5883L::GN_0_GA:
|
||
81 | returnBuffer = int32_t(0.73 * 1000) * int32_t(magLsbBuffer); |
||
82 | break;
|
||
83 | case HMC5883L::GN_1_GA:
|
||
84 | returnBuffer = int32_t(0.92 * 1000) * int32_t(magLsbBuffer); |
||
85 | break;
|
||
86 | case HMC5883L::GN_2_GA:
|
||
87 | returnBuffer = int32_t(1.22 * 1000) * int32_t(magLsbBuffer); |
||
88 | break;
|
||
89 | case HMC5883L::GN_3_GA:
|
||
90 | returnBuffer = int32_t(1.52 * 1000) * int32_t(magLsbBuffer); |
||
91 | break;
|
||
92 | case HMC5883L::GN_4_GA:
|
||
93 | returnBuffer = int32_t(2.27 * 1000) * int32_t(magLsbBuffer); |
||
94 | break;
|
||
95 | case HMC5883L::GN_5_GA:
|
||
96 | returnBuffer = int32_t(2.56 * 1000) * int32_t(magLsbBuffer); |
||
97 | break;
|
||
98 | case HMC5883L::GN_6_GA:
|
||
99 | returnBuffer = int32_t(3.03 * 1000) * int32_t(magLsbBuffer); |
||
100 | break;
|
||
101 | case HMC5883L::GN_7_GA:
|
||
102 | returnBuffer = int32_t(4.35 * 1000) * int32_t(magLsbBuffer); |
||
103 | break;
|
||
104 | } |
||
105 | |||
106 | return returnBuffer;
|
||
107 | |||
108 | } |
||
109 | |||
110 | void
|
||
111 | HMC5883L:: |
||
112 | updateSensorData() { |
||
113 | |||
114 | const uint8_t sizeOfRxData = offsetof(HMC5883L::registers, yLsb)
|
||
115 | - offsetof(HMC5883L::registers, xMsb) |
||
116 | + 1;
|
||
117 | |||
118 | uint8_t rxBuffer[sizeOfRxData]; |
||
119 | |||
120 | /*Address of data register*/
|
||
121 | uint8_t txBuffer = offsetof(HMC5883L::registers, xMsb); /* Address of XMSB */
|
||
122 | |||
123 | // Define the RXTX structure
|
||
124 | this->txParams.txbuf = &txBuffer;
|
||
125 | this->txParams.txbytes = sizeof(txBuffer); |
||
126 | this->txParams.rxbuf = rxBuffer;
|
||
127 | this->txParams.rxbytes = sizeof(rxBuffer); |
||
128 | |||
129 | this->driver->acquireBus();
|
||
130 | this->driver->masterTransmit(&this->txParams); |
||
131 | this->driver->releaseBus();
|
||
132 | |||
133 | // Copy the bytes in the right order
|
||
134 | chSysLock(); // TODO: check if this is really necessary b/c it inserts a DMB!!
|
||
135 | data[HMC5883L::AXIS_X] = (rxBuffer[0] << 8) + rxBuffer[1]; |
||
136 | data[HMC5883L::AXIS_Z] = (rxBuffer[2] << 8) + rxBuffer[3]; |
||
137 | data[HMC5883L::AXIS_Y] = (rxBuffer[4] << 8) + rxBuffer[5]; |
||
138 | chSysUnlock(); |
||
139 | } |
||
140 | |||
141 | uint8_t HMC5883L::getCheck() { |
||
142 | |||
143 | I2CTxParams txStructure; |
||
144 | msg_t res; |
||
145 | const uint8_t txBuffer = offsetof(HMC5883L::registers, idA);
|
||
146 | const uint8_t sizeOfRxData = offsetof(HMC5883L::registers, idC) - offsetof(HMC5883L::registers, idA) + 1; |
||
147 | uint8_t rxBuffer[sizeOfRxData]; |
||
148 | |||
149 | txStructure.addr = SLA; |
||
150 | txStructure.txbuf = &txBuffer; |
||
151 | txStructure.txbytes = sizeof(txBuffer);
|
||
152 | txStructure.rxbuf = rxBuffer; |
||
153 | txStructure.rxbytes = sizeof(rxBuffer);
|
||
154 | |||
155 | // get the identifier
|
||
156 | this->driver->acquireBus();
|
||
157 | res = this->driver->masterTransmit(&txStructure);
|
||
158 | this->driver->releaseBus();
|
||
159 | // Check
|
||
160 | if (rxBuffer[0] == ID_IRA && rxBuffer[1] == ID_IRB && rxBuffer[2] == ID_IRC && res == RDY_OK) { |
||
161 | return HMC5883L::CHECK_OK;
|
||
162 | } else {
|
||
163 | return HMC5883L::CHECK_FAIL;
|
||
164 | } |
||
165 | |||
166 | } |
||
167 | |||
168 | |||
169 | msg_t |
||
170 | HMC5883L:: |
||
171 | writeConf() { |
||
172 | |||
173 | uint8_t txBuffer[4];
|
||
174 | msg_t res; |
||
175 | |||
176 | // write control config
|
||
177 | txBuffer[0] = offsetof(HMC5883L::registers, ctrlA); /* Address of ctrlA */ |
||
178 | txBuffer[1] = this->config->ctrlA; |
||
179 | txBuffer[2] = this->config->ctrlB; |
||
180 | txBuffer[3] = this->config->mode; |
||
181 | |||
182 | this->txParams.txbuf = txBuffer;
|
||
183 | this->txParams.txbytes = sizeof(txBuffer); |
||
184 | this->txParams.rxbytes = 0; |
||
185 | |||
186 | this->driver->acquireBus();
|
||
187 | res = this->driver->masterTransmit(&this->txParams); |
||
188 | this->driver->releaseBus();
|
||
189 | |||
190 | return res;
|
||
191 | |||
192 | } |
||
193 | |||
194 | } /* amiro */ |