-
-
Notifications
You must be signed in to change notification settings - Fork 1.4k
/
accgyro_bma280.c
52 lines (39 loc) · 1.22 KB
/
accgyro_bma280.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "bus_i2c.h"
#include "accgyro.h"
#include "accgyro_bma280.h"
// BMA280, default I2C address mode 0x18
#define BMA280_ADDRESS 0x18
#define BMA280_ACC_X_LSB 0x02
#define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F
static void bma280Init(void);
static void bma280Read(int16_t *accelData);
bool bma280Detect(acc_t *acc)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(BMA280_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xFB)
return false;
acc->init = bma280Init;
acc->read = bma280Read;
return true;
}
static void bma280Init(void)
{
i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc_1G = 512 * 8;
}
static void bma280Read(int16_t *accelData)
{
uint8_t buf[6];
i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
// Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
}