Statistics
| Branch: | Tag: | Revision:

amiro-os / components / magneto / hmc5883l.cpp @ b8b3a9c9

History | View | Annotate | Download (4.443 KB)

1
#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
#include <amiro/Constants.h>
9

    
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
    this->waitAnyEventTimeout(ALL_EVENTS, CAN::UPDATE_PERIOD);
55
  }
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 */