mirror of
https://github.com/cesanta/mongoose.git
synced 2024-12-21 13:42:15 +08:00
106 lines
3.0 KiB
C
106 lines
3.0 KiB
C
/*
|
|
* Copyright (c) 2014-2016 Cesanta Software Limited
|
|
* All rights reserved
|
|
*/
|
|
|
|
#include "bm222.h"
|
|
|
|
#include "mongoose.h"
|
|
#include "cs_dbg.h"
|
|
|
|
#include "i2c_if.h"
|
|
|
|
#define BM222_REG_FIFO_STATUS 0x0e
|
|
|
|
#define BM222_REG_PMU_BW 0x10
|
|
#define BM222_PMU_BW_7_81_HZ 0x8
|
|
#define BM222_PMU_BW_31_25_HZ 0xA
|
|
#define BM222_PMU_BW_62_5_HZ 0xB
|
|
#define BM222_PMU_BW_125_HZ 0xC
|
|
|
|
#define BM222_REG_BGW_SOFTRESET 0x14
|
|
#define BM222_DO_SOFT_RESET 0xB6
|
|
|
|
#define BM222_REG_FIFO_CONFIG_1 0x3e
|
|
#define BM222_FIFO_MODE_FIFO 0x40
|
|
#define BM222_FIFO_DATA_ALL 0
|
|
|
|
#define BM222_REG_FIFO_DATA 0x3f
|
|
|
|
struct bm222_ctx *bm222_init(uint8_t addr) {
|
|
struct bm222_ctx *ctx = (struct bm222_ctx *) calloc(1, sizeof(*ctx));
|
|
if (ctx == NULL) return NULL;
|
|
ctx->addr = addr;
|
|
{
|
|
unsigned char val[2] = {BM222_REG_BGW_SOFTRESET, BM222_DO_SOFT_RESET};
|
|
if (I2C_IF_Write(addr, val, 2, 1) != 0) goto out_err;
|
|
osi_Sleep(2 /* ms */); /* t_w,up1 = 1.8 ms */
|
|
}
|
|
if (!bm222_fifo_init(ctx)) return false;
|
|
{
|
|
unsigned char val[2] = {BM222_REG_PMU_BW, BM222_PMU_BW_125_HZ};
|
|
if (I2C_IF_Write(addr, val, 2, 1) != 0) goto out_err;
|
|
}
|
|
return ctx;
|
|
out_err:
|
|
free(ctx);
|
|
return NULL;
|
|
}
|
|
|
|
bool bm222_fifo_init(struct bm222_ctx *ctx) {
|
|
unsigned char val[2] = {BM222_REG_FIFO_CONFIG_1,
|
|
BM222_FIFO_MODE_FIFO | BM222_FIFO_DATA_ALL};
|
|
return (I2C_IF_Write(ctx->addr, val, 2, 1) == 0);
|
|
}
|
|
|
|
bool bm222_get_data(struct bm222_ctx *ctx) {
|
|
unsigned char reg = BM222_REG_FIFO_STATUS;
|
|
unsigned char val;
|
|
if (I2C_IF_ReadFrom(ctx->addr, ®, 1, &val, 1) < 0) return false;
|
|
unsigned char len = (val & 0x7f);
|
|
unsigned char overflow = (val & 0x80 ? 1 : 0);
|
|
LOG(LL_DEBUG, ("FIFO len: %d (ovf? %d)", len, overflow));
|
|
reg = BM222_REG_FIFO_DATA;
|
|
int8_t fifo[32 * 6], *v = fifo;
|
|
if (I2C_IF_ReadFrom(ctx->addr, ®, 1, (unsigned char *) fifo, len * 6) <
|
|
0) {
|
|
return false;
|
|
}
|
|
double now = mg_time();
|
|
/* Of potentially multiple samples, pick one with maximum difference. */
|
|
int32_t max_d = 0;
|
|
bool sampled = false;
|
|
struct bm222_sample *ls = ctx->data + ctx->last_index, *s = NULL;
|
|
if (ls->ts == 0) {
|
|
/* The very first sample. */
|
|
ls->ts = now;
|
|
ls->x = v[1];
|
|
ls->y = v[3];
|
|
ls->z = v[5];
|
|
v += 6;
|
|
len--;
|
|
sampled = true;
|
|
s = ls;
|
|
}
|
|
for (; len > 0; v += 6, len--) {
|
|
int32_t dx = ((int32_t) v[1]) - ((int32_t) ls->x);
|
|
int32_t dy = ((int32_t) v[3]) - ((int32_t) ls->y);
|
|
int32_t dz = ((int32_t) v[5]) - ((int32_t) ls->z);
|
|
int32_t d = dx * dx + dy * dy + dz * dz;
|
|
if ((d > 2 && d > max_d) || (!sampled && now - ls->ts > 1.0)) {
|
|
if (!sampled) {
|
|
ctx->last_index = (ctx->last_index + 1) % BM222_NUM_SAMPLES;
|
|
s = ctx->data + ctx->last_index;
|
|
sampled = true;
|
|
}
|
|
s->ts = now;
|
|
s->x = v[1];
|
|
s->y = v[3];
|
|
s->z = v[5];
|
|
if (d > max_d) max_d = d;
|
|
LOG(LL_VERBOSE_DEBUG, ("dx %d dy %d dz %d d %d", dx, dy, dz, d));
|
|
}
|
|
}
|
|
return (overflow ? bm222_fifo_init(ctx) : true); /* Clear the ovf flag. */
|
|
}
|