Statistics
| Branch: | Tag: | Revision:

mongoose / examples / CC3200 / bm222.c @ eaef5bd1

History | View | Annotate | Download (2.96 KB)

1
/*
2
 * Copyright (c) 2014-2016 Cesanta Software Limited
3
 * All rights reserved
4
 */
5

    
6
#include "bm222.h"
7

    
8
#include "mongoose.h"
9
#include "cs_dbg.h"
10

    
11
#include "i2c_if.h"
12

    
13
#define BM222_REG_FIFO_STATUS 0x0e
14

    
15
#define BM222_REG_PMU_BW 0x10
16
#define BM222_PMU_BW_7_81_HZ 0x8
17
#define BM222_PMU_BW_31_25_HZ 0xA
18
#define BM222_PMU_BW_62_5_HZ 0xB
19
#define BM222_PMU_BW_125_HZ 0xC
20

    
21
#define BM222_REG_BGW_SOFTRESET 0x14
22
#define BM222_DO_SOFT_RESET 0xB6
23

    
24
#define BM222_REG_FIFO_CONFIG_1 0x3e
25
#define BM222_FIFO_MODE_FIFO 0x40
26
#define BM222_FIFO_DATA_ALL 0
27

    
28
#define BM222_REG_FIFO_DATA 0x3f
29

    
30
struct bm222_ctx *bm222_init(uint8_t addr) {
31
  struct bm222_ctx *ctx = (struct bm222_ctx *) calloc(1, sizeof(*ctx));
32
  if (ctx == NULL) return NULL;
33
  ctx->addr = addr;
34
  {
35
    unsigned char val[2] = {BM222_REG_BGW_SOFTRESET, BM222_DO_SOFT_RESET};
36
    if (I2C_IF_Write(addr, val, 2, 1) != 0) goto out_err;
37
    osi_Sleep(2 /* ms */); /* t_w,up1 = 1.8 ms */
38
  }
39
  if (!bm222_fifo_init(ctx)) return false;
40
  {
41
    unsigned char val[2] = {BM222_REG_PMU_BW, BM222_PMU_BW_125_HZ};
42
    if (I2C_IF_Write(addr, val, 2, 1) != 0) goto out_err;
43
  }
44
  return ctx;
45
out_err:
46
  free(ctx);
47
  return NULL;
48
}
49

    
50
bool bm222_fifo_init(struct bm222_ctx *ctx) {
51
  unsigned char val[2] = {BM222_REG_FIFO_CONFIG_1,
52
                          BM222_FIFO_MODE_FIFO | BM222_FIFO_DATA_ALL};
53
  return (I2C_IF_Write(ctx->addr, val, 2, 1) == 0);
54
}
55

    
56
bool bm222_get_data(struct bm222_ctx *ctx) {
57
  unsigned char reg = BM222_REG_FIFO_STATUS;
58
  unsigned char val;
59
  if (I2C_IF_ReadFrom(ctx->addr, &reg, 1, &val, 1) < 0) return false;
60
  unsigned char len = (val & 0x7f);
61
  unsigned char overflow = (val & 0x80 ? 1 : 0);
62
  LOG(LL_DEBUG, ("FIFO len: %d (ovf? %d)", len, overflow));
63
  reg = BM222_REG_FIFO_DATA;
64
  int8_t fifo[32 * 6], *v = fifo;
65
  if (I2C_IF_ReadFrom(ctx->addr, &reg, 1, (unsigned char *) fifo, len * 6) <
66
      0) {
67
    return false;
68
  }
69
  double now = mg_time();
70
  /* Of potentially multiple samples, pick one with maximum difference. */
71
  int32_t max_d = 0;
72
  bool sampled = false;
73
  struct bm222_sample *ls = ctx->data + ctx->last_index, *s = NULL;
74
  if (ls->ts == 0) {
75
    /* The very first sample. */
76
    ls->ts = now;
77
    ls->x = v[1];
78
    ls->y = v[3];
79
    ls->z = v[5];
80
    v += 6;
81
    len--;
82
    sampled = true;
83
    s = ls;
84
  }
85
  for (; len > 0; v += 6, len--) {
86
    int32_t dx = ((int32_t) v[1]) - ((int32_t) ls->x);
87
    int32_t dy = ((int32_t) v[3]) - ((int32_t) ls->y);
88
    int32_t dz = ((int32_t) v[5]) - ((int32_t) ls->z);
89
    int32_t d = dx * dx + dy * dy + dz * dz;
90
    if ((d > 2 && d > max_d) || (!sampled && now - ls->ts > 1.0)) {
91
      if (!sampled) {
92
        ctx->last_index = (ctx->last_index + 1) % BM222_NUM_SAMPLES;
93
        s = ctx->data + ctx->last_index;
94
        sampled = true;
95
      }
96
      s->ts = now;
97
      s->x = v[1];
98
      s->y = v[3];
99
      s->z = v[5];
100
      if (d > max_d) max_d = d;
101
      LOG(LL_VERBOSE_DEBUG, ("dx %d dy %d dz %d d %d", dx, dy, dz, d));
102
    }
103
  }
104
  return (overflow ? bm222_fifo_init(ctx) : true); /* Clear the ovf flag. */
105
}