Statistics
| Branch: | Tag: | Revision:

amiro-os / components / magneto / hmc5883l.cpp @ 58fe0e0b

History | View | Annotate | Download (4.41 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

    
9
using namespace chibios_rt;
10

    
11
namespace amiro {
12

    
13
HMC5883L::~HMC5883L() {
14

    
15
}
16

    
17
HMC5883L::HMC5883L(I2CDriver *driver, const HMC5883LConfig *config)
18
    : BaseStaticThread<256>(),
19
      driver(driver),
20
      config(config) {
21

    
22
  this->txParams.addr = HMC5883L::SLA;
23
}
24

    
25
EvtSource*
26
HMC5883L::
27
getEventSource() {
28

    
29
  return &this->eventSource;
30
}
31

    
32
msg_t
33
HMC5883L::
34
main(void) {
35

    
36
  msg_t res;
37

    
38
  this->setName("Hmc5883l");
39

    
40
  /* exit if writing configuration fails */
41
  res = this->writeConf();
42

    
43
  if (res) {
44
    return RDY_RESET;
45
  }
46

    
47
  while (!this->shouldTerminate()) {
48

    
49
    this->updateSensorData();
50

    
51
    this->eventSource.broadcastFlags(0);
52

    
53
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
54
  }
55

    
56
  return RDY_OK;
57
}
58

    
59
int16_t
60
HMC5883L::
61
getMagnetization(const uint8_t axis) {
62
  int16_t returnBuffer;
63
  chSysLock();
64
  returnBuffer = this->data[axis];
65
  chSysUnlock();
66
  return returnBuffer;
67

    
68
}
69

    
70
int32_t
71
HMC5883L::
72
getMagnetizationGauss(const uint8_t axis) {
73

    
74
  int16_t magLsbBuffer = this->getMagnetization(axis);
75
  int32_t returnBuffer = 0;
76

    
77
  // The gains are taken from table 9 of the manual
78
  switch (this->config->ctrlB & GN_7_GA) {
79
    case HMC5883L::GN_0_GA:
80
      returnBuffer = int32_t(0.73 * 1000) * int32_t(magLsbBuffer);
81
      break;
82
    case HMC5883L::GN_1_GA:
83
      returnBuffer = int32_t(0.92 * 1000) * int32_t(magLsbBuffer);
84
      break;
85
    case HMC5883L::GN_2_GA:
86
      returnBuffer = int32_t(1.22 * 1000) * int32_t(magLsbBuffer);
87
      break;
88
    case HMC5883L::GN_3_GA:
89
      returnBuffer = int32_t(1.52 * 1000) * int32_t(magLsbBuffer);
90
      break;
91
    case HMC5883L::GN_4_GA:
92
      returnBuffer = int32_t(2.27 * 1000) * int32_t(magLsbBuffer);
93
      break;
94
    case HMC5883L::GN_5_GA:
95
      returnBuffer = int32_t(2.56 * 1000) * int32_t(magLsbBuffer);
96
      break;
97
    case HMC5883L::GN_6_GA:
98
      returnBuffer = int32_t(3.03 * 1000) * int32_t(magLsbBuffer);
99
      break;
100
    case HMC5883L::GN_7_GA:
101
      returnBuffer = int32_t(4.35 * 1000) * int32_t(magLsbBuffer);
102
      break;
103
  }
104

    
105
  return returnBuffer;
106

    
107
}
108

    
109
void
110
HMC5883L::
111
updateSensorData() {
112

    
113
  const uint8_t sizeOfRxData =  offsetof(HMC5883L::registers, yLsb)
114
                              - offsetof(HMC5883L::registers, xMsb)
115
                              + 1;
116

    
117
  uint8_t rxBuffer[sizeOfRxData];
118

    
119
  /*Address of data register*/
120
  uint8_t txBuffer = offsetof(HMC5883L::registers, xMsb); /* Address of XMSB */
121

    
122
  // Define the RXTX structure
123
  this->txParams.txbuf = &txBuffer;
124
  this->txParams.txbytes = sizeof(txBuffer);
125
  this->txParams.rxbuf = rxBuffer;
126
  this->txParams.rxbytes = sizeof(rxBuffer);
127

    
128
  this->driver->acquireBus();
129
  this->driver->masterTransmit(&this->txParams);
130
  this->driver->releaseBus();
131

    
132
  // Copy the bytes in the right order
133
  chSysLock(); // TODO: check if this is really necessary b/c it inserts a DMB!!
134
  data[HMC5883L::AXIS_X] = (rxBuffer[0] << 8) + rxBuffer[1];
135
  data[HMC5883L::AXIS_Z] = (rxBuffer[2] << 8) + rxBuffer[3];
136
  data[HMC5883L::AXIS_Y] = (rxBuffer[4] << 8) + rxBuffer[5];
137
  chSysUnlock();
138
}
139

    
140
uint8_t HMC5883L::getCheck() {
141

    
142
  I2CTxParams txStructure;
143
  msg_t res;
144
  const uint8_t txBuffer = offsetof(HMC5883L::registers, idA);
145
  const uint8_t sizeOfRxData = offsetof(HMC5883L::registers, idC) - offsetof(HMC5883L::registers, idA) + 1;
146
  uint8_t rxBuffer[sizeOfRxData];
147

    
148
  txStructure.addr = SLA;
149
  txStructure.txbuf = &txBuffer;
150
  txStructure.txbytes = sizeof(txBuffer);
151
  txStructure.rxbuf = rxBuffer;
152
  txStructure.rxbytes = sizeof(rxBuffer);
153

    
154
  // get the identifier
155
  this->driver->acquireBus();
156
  res = this->driver->masterTransmit(&txStructure);
157
  this->driver->releaseBus();
158
  // Check
159
  if (rxBuffer[0] == ID_IRA && rxBuffer[1] == ID_IRB && rxBuffer[2] == ID_IRC && res == RDY_OK) {
160
    return HMC5883L::CHECK_OK;
161
  } else {
162
    return HMC5883L::CHECK_FAIL;
163
  }
164

    
165
}
166

    
167

    
168
msg_t
169
HMC5883L::
170
writeConf() {
171

    
172
  uint8_t txBuffer[4];
173
  msg_t res;
174

    
175
  // write control config
176
  txBuffer[0] = offsetof(HMC5883L::registers, ctrlA);  /* Address of ctrlA */
177
  txBuffer[1] = this->config->ctrlA;
178
  txBuffer[2] = this->config->ctrlB;
179
  txBuffer[3] = this->config->mode;
180

    
181
  this->txParams.txbuf = txBuffer;
182
  this->txParams.txbytes = sizeof(txBuffer);
183
  this->txParams.rxbytes = 0;
184

    
185
  this->driver->acquireBus();
186
  res = this->driver->masterTransmit(&this->txParams);
187
  this->driver->releaseBus();
188

    
189
  return res;
190

    
191
}
192

    
193
} /* amiro */