Statistics
| Branch: | Tag: | Revision:

amiro-os / components / accel / lis331dlh.cpp @ 58fe0e0b

History | View | Annotate | Download (11.2 KB)

1
#include <string.h>
2

    
3
#include <amiro/util/util.h>
4
#include <amiro/bus/spi/HWSPIDriver.hpp>
5
#include <amiro/accel/lis331dlh.hpp>
6
#include <chprintf.h>
7
#include <cmath>  // abs()
8

    
9
namespace amiro {
10

    
11
LIS331DLH::
12
LIS331DLH(HWSPIDriver *driver) :
13
  driver(driver),
14
  currentFullScaleConfiguration(FS_2G) {
15

    
16
}
17

    
18
LIS331DLH::
19
~LIS331DLH() {
20

    
21
}
22

    
23
chibios_rt::EvtSource*
24
LIS331DLH::
25
getEventSource() {
26
  return &this->eventSource;
27
}
28

    
29
int16_t
30
LIS331DLH::
31
getAcceleration(const uint8_t axis) {
32

    
33
  return this->accelerations[axis];
34

    
35
}
36

    
37
int16_t
38
LIS331DLH::
39
getAccelerationForce(const uint8_t axis) {
40

    
41
  int16_t result;
42

    
43
  switch(this->currentFullScaleConfiguration) {
44
  default:
45
  case(FS_2G):
46
    // Should be this->accelerations[axis] * 0.9765625
47
    result = this->accelerations[axis];
48
    break;
49
  case(FS_4G):
50
      // Should be this->accelerations[axis] * 1.953125
51
      result =  this->accelerations[axis] << 1;
52
    break;
53
  case(FS_8G):
54
      // Should be this->accelerations[axis] * 3.90625
55
      result =  this->accelerations[axis] << 2;
56
    break;
57
  }
58

    
59
  return result;
60

    
61
}
62

    
63
msg_t
64
LIS331DLH::
65
main() {
66

    
67
  this->setName("Lis331dlh");
68

    
69
  while (!this->shouldTerminate()) {
70

    
71
    updateSensorData();
72

    
73
    this->eventSource.broadcastFlags(0);
74

    
75
    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
76

    
77
  }
78
  return RDY_OK;
79
}
80

    
81
void
82
LIS331DLH::
83
updateSensorData() {
84

    
85
  const size_t buffer_size = offsetof(LIS331DLH::registers, out_z)
86
                             - offsetof(LIS331DLH::registers, status_reg)
87
                             + MEMBER_SIZE(LIS331DLH::registers, out_z)
88
                             + 1; /* addressing */
89
  uint8_t buffer[buffer_size];
90
  uint8_t sreg;
91

    
92
  // this might be three-wire so we need to send ones
93
  memset(buffer, 0xFFu, buffer_size);
94
  buffer[0] = offsetof(LIS331DLH::registers, status_reg) | LIS331DLH::SPI_MULT | LIS331DLH::SPI_READ;
95
  this->driver->exchange(buffer, buffer, buffer_size);
96

    
97
  // assemble data
98
  sreg = buffer[1];
99

    
100
  if (sreg & LIS331DLH::XDA)
101
    this->accelerations[LIS331DLH::AXIS_X] = int16_t((buffer[3] << 8) | buffer[2]) >> 4;
102

    
103
  if (sreg & LIS331DLH::YDA)
104
    this->accelerations[LIS331DLH::AXIS_Y] = int16_t((buffer[5] << 8) | buffer[4]) >> 4;
105

    
106
  if (sreg & LIS331DLH::ZDA)
107
    this->accelerations[LIS331DLH::AXIS_Z] = int16_t((buffer[7] << 8) | buffer[6]) >> 4;
108

    
109
}
110

    
111
msg_t
112
LIS331DLH::
113
configure(LIS331DLHConfig *config) {
114

    
115
  const size_t ctrl_reg_size = offsetof(LIS331DLH::registers, ctrl_reg5)
116
                               - offsetof(LIS331DLH::registers, ctrl_reg1)
117
                               + MEMBER_SIZE(LIS331DLH::registers, ctrl_reg5)
118
                               + 1; /* addressing */
119
  const size_t int_reg_size = offsetof(LIS331DLH::registers, int1_duration)
120
                              - offsetof(LIS331DLH::registers, int1_cfg)
121
                              + MEMBER_SIZE(LIS331DLH::registers, int1_duration) /* linear interrupt config */
122
                              + MEMBER_SIZE(LIS331DLH::registers, int1_src)
123
                              + MEMBER_SIZE(LIS331DLH::registers, int1_src) /* 2x size of int1_src */
124
                              + 1; /* addressing */
125
  const size_t buffer_size = ctrl_reg_size > int_reg_size ? ctrl_reg_size : int_reg_size;
126

    
127
  uint8_t buffer[buffer_size];
128
  InterruptConfig *int_cfg = &config->int1_cfg;
129
  uint8_t i;
130

    
131
  // write interrupt config first
132
  for (i = 0x00u; i < 2; i++, int_cfg++) {
133
    // this might be three-wire so we need to send ones
134
    memset(buffer, 0xFFu, buffer_size);
135
    if (!i)
136
      buffer[0] = offsetof(LIS331DLH::registers, int1_cfg);
137
    else
138
      buffer[0] = offsetof(LIS331DLH::registers, int2_cfg);
139
    buffer[5] = buffer[0] + offsetof(LIS331DLH::registers, int1_src) - offsetof(LIS331DLH::registers, int1_cfg);
140
    buffer[5] |= LIS331DLH::SPI_READ;
141
    buffer[6] = 0xFFu,
142
    buffer[0] |= LIS331DLH::SPI_MULT | LIS331DLH::SPI_WRITE;
143
    buffer[1] = int_cfg->config;
144
    buffer[2] = 0xFFu; /* skip source register */
145
    buffer[3] = int_cfg->ths;
146
    buffer[4] = int_cfg->duration;
147
    this->driver->write(&buffer[0], 5);
148
    // read intN_src register to clear IA
149
    this->driver->exchange(&buffer[5], &buffer[5], 2);
150
  }
151

    
152
  // write control config
153
  // this might be three-wire so we need to send ones
154
  memset(buffer, 0xFFu, buffer_size);
155
  buffer[0] = offsetof(LIS331DLH::registers, ctrl_reg1) | LIS331DLH::SPI_MULT | LIS331DLH::SPI_WRITE;
156
  buffer[1] = config->ctrl1;
157
  buffer[2] = config->ctrl2;
158
  buffer[3] = config->ctrl3;
159
  buffer[4] = config->ctrl4;
160
  buffer[5] = config->ctrl5;
161
  this->driver->write(buffer, 6);
162

    
163
  // reset hp filter
164
  buffer[0] = offsetof(LIS331DLH::registers, hp_filter_reset) | LIS331DLH::SPI_WRITE;
165
  this->driver->write(buffer, 1);
166

    
167
  // Store the full scale configuration for acceleration decoding
168
  this->currentFullScaleConfiguration = config->ctrl4 & LIS331DLH::FS_8G;
169

    
170
  return RDY_OK;
171

    
172
}
173

    
174
uint8_t
175
LIS331DLH::
176
getCheck() {
177

    
178
  const size_t buffer_size = 1 /* addressing */
179
                             + 1; /* who am i */
180
  uint8_t buffer[buffer_size];
181

    
182
  // Exchange the data with the LIS331DLH accelerometer
183
  // Specify the adress and the mode
184
  buffer[0] = offsetof(LIS331DLH::registers, who_am_i) | LIS331DLH::SPI_READ;
185
  this->driver->exchange(buffer, buffer, buffer_size);
186
  // Check
187
  if (buffer[1] == LIS331DLH::LIS331DLH_ID) {
188
    return LIS331DLH::CHECK_OK;
189
  } else {
190
    return LIS331DLH::CHECK_FAIL;
191
  }
192

    
193
}
194

    
195
void
196
LIS331DLH::
197
printSelfTest(LIS331DLHConfig* config) {
198

    
199
  // Running the build in test from LIS331DLH manual and compare the
200
  // measured values against table 3
201

    
202
  const uint8_t amountOfMeanValues = 10;
203
  const uint32_t sleepBetweenMeasurementMs = 500;
204
  int32_t accel;
205

    
206
  // Choose a defined config, if none is given
207
  LIS331DLH::LIS331DLHConfig test_accel_run_cfg;
208
  if (config == NULL) {
209
    LIS331DLH::LIS331DLHConfig accel_run_cfg_tmp = {
210
    /* ctrl1    */LIS331DLH::PM_ODR | LIS331DLH::DR_50HZ_37LP | LIS331DLH::ZEN
211
        | LIS331DLH::YEN | LIS331DLH::XEN,
212
    /* ctrl2    */0x00u,
213
    /* ctrl3    */LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_DRY,
214
    /* ctrl4    */LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_8G
215
        | LIS331DLH::SIM_4WI,
216
    /* ctrl5    */LIS331DLH::SLEEP_TO_WAKE_OFF,
217
    /* int1_cfg */
218
    {
219
    /* config   */LIS331DLH::AOI_OR_INT,
220
    /* ths      */0x00u,
221
    /* duration */0x00u, },
222
    /* int2_cfg */
223
    {
224
    /* config   */LIS331DLH::AOI_OR_INT,
225
    /* ths      */0x00u,
226
    /* duration */0x00u, }, };
227
    test_accel_run_cfg = accel_run_cfg_tmp;
228
  } else {
229
    // Save the given config
230
    test_accel_run_cfg = *config;
231
  }
232

    
233
  // 1. Get some standard values
234
  // Configure for the test
235
  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
236
      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI;
237
  this->configure(&test_accel_run_cfg);
238
  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
239
  // Grep some values and build the mean value
240
  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc std values\n");
241
  int32_t stdAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
242
      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
243
      LIS331DLH::AXIS_Z) };
244

    
245
  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
246
    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
247
    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
248
      accel = int32_t(this->getAcceleration(i));
249
      stdAccValues[i] = (stdAccValues[i] * n + accel) / (n + 1);
250
      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
251
                accel < 0 ? -accel : accel, stdAccValues[i]);
252
    }
253
    chprintf((BaseSequentialStream*) &SD1, "\n");
254
  }
255

    
256
  // 2. Apply negative offset
257
  // Configure for the test
258
  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
259
      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI | LIS331DLH::ST_ENABLE
260
      | LIS331DLH::STSIGN_NEG;
261
  this->configure(&test_accel_run_cfg);
262
  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
263
  // Grep some values and build the mean value
264
  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc neg values\n");
265
  int16_t negAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
266
      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
267
      LIS331DLH::AXIS_Z) };
268

    
269
  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
270
    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
271
    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
272
      accel = int32_t(this->getAcceleration(i));
273
      negAccValues[i] = (negAccValues[i] * n + accel) / (n + 1);
274
      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
275
                accel < 0 ? -accel : accel, negAccValues[i]);
276
    }
277
    chprintf((BaseSequentialStream*) &SD1, "\n");
278
  }
279

    
280
  // 2. Apply positive offset
281
  // Configure for the test
282
  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
283
      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI | LIS331DLH::ST_ENABLE
284
      | LIS331DLH::STSIGN_POS;
285
  this->configure(&test_accel_run_cfg);
286
  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
287
  // Grep some values and build the mean value
288
  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc pos values\n");
289
  int16_t posAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
290
      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
291
      LIS331DLH::AXIS_Z) };
292

    
293
  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
294
    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
295
    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
296
      accel = int32_t(this->getAcceleration(i));
297
      posAccValues[i] = (posAccValues[i] * n + accel) / (n + 1);
298
      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
299
                accel < 0 ? -accel : accel, posAccValues[i]);
300
    }
301
    chprintf((BaseSequentialStream*) &SD1, "\n");
302
  }
303

    
304
  // Get the amplitude change and compare it to the standard values from LIS331DLH manual table 3
305
  int32_t amp;
306
  const int32_t ampXmax = 550, ampYmax = 550, ampZmax = 750;
307
  const int32_t ampXmin = 120, ampYmin = 120, ampZmin = 140;
308

    
309
  // TEST
310
  chprintf((BaseSequentialStream*) &SD1, "\n\nACC: Testresult\n");
311
  amp = std::abs(
312
      stdAccValues[LIS331DLH::AXIS_X] - negAccValues[LIS331DLH::AXIS_X]);
313
  if (amp < ampXmin || amp > ampXmax)
314
    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative x-axis faulty\n");
315
  amp = std::abs(
316
      stdAccValues[LIS331DLH::AXIS_Y] - negAccValues[LIS331DLH::AXIS_Y]);
317
  if (amp < ampYmin || amp > ampYmax)
318
    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative y-axis faulty\n");
319
  amp = std::abs(
320
      stdAccValues[LIS331DLH::AXIS_Z] - negAccValues[LIS331DLH::AXIS_Z]);
321
  if (amp < ampZmin || amp > ampZmax)
322
    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative z-axis faulty\n");
323
  amp = std::abs(
324
      stdAccValues[LIS331DLH::AXIS_X] - posAccValues[LIS331DLH::AXIS_X]);
325
  if (amp < ampXmin || amp > ampXmax)
326
    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive x-axis faulty\n");
327
  amp = std::abs(
328
      stdAccValues[LIS331DLH::AXIS_Y] - posAccValues[LIS331DLH::AXIS_Y]);
329
  if (amp < ampYmin || amp > ampYmax)
330
    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive y-axis faulty\n");
331
  amp = std::abs(
332
      stdAccValues[LIS331DLH::AXIS_Z] - posAccValues[LIS331DLH::AXIS_Z]);
333
  if (amp < ampZmin || amp > ampZmax)
334
    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive z-axis faulty\n");
335

    
336
  // Write back the original config
337
  this->configure(config);
338
}
339

    
340
} /* amiro */