forked from InfiniTimeOrg/InfiniTime
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBma421.cpp
159 lines (129 loc) · 4.17 KB
/
Bma421.cpp
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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
#include "drivers/Bma421.h"
#include <libraries/delay/nrf_delay.h>
#include <libraries/log/nrf_log.h>
#include "drivers/TwiMaster.h"
#include <drivers/Bma421_C/bma423.h>
using namespace Pinetime::Drivers;
namespace {
int8_t user_i2c_read(uint8_t reg_addr, uint8_t* reg_data, uint32_t length, void* intf_ptr) {
auto bma421 = static_cast<Bma421*>(intf_ptr);
bma421->Read(reg_addr, reg_data, length);
return 0;
}
int8_t user_i2c_write(uint8_t reg_addr, const uint8_t* reg_data, uint32_t length, void* intf_ptr) {
auto bma421 = static_cast<Bma421*>(intf_ptr);
bma421->Write(reg_addr, reg_data, length);
return 0;
}
void user_delay(uint32_t period_us, void* /*intf_ptr*/) {
nrf_delay_us(period_us);
}
}
// Scale factors to convert accelerometer counts to milli-g
// from datasheet: https://files.pine64.org/doc/datasheet/pinetime/BST-BMA421-FL000.pdf
// The array index to use is stored in accel_conf.range
const short Bma421::accelScaleFactors[] = {
1024, // LSB/g +/- 2g range
512, // LSB/g +/- 4g range
256, // LSB/g +/- 8g range
128 // LSB/g +/- 16g range
};
Bma421::Bma421(TwiMaster& twiMaster, uint8_t twiAddress) : twiMaster {twiMaster}, deviceAddress {twiAddress} {
bma.intf = BMA4_I2C_INTF;
bma.bus_read = user_i2c_read;
bma.bus_write = user_i2c_write;
bma.variant = BMA42X_VARIANT;
bma.intf_ptr = this;
bma.delay_us = user_delay;
bma.read_write_len = 16;
}
void Bma421::Init() {
if (not isResetOk)
return; // Call SoftReset (and reset TWI device) first!
auto ret = bma423_init(&bma);
if (ret != BMA4_OK)
return;
switch (bma.chip_id) {
case BMA423_CHIP_ID:
deviceType = DeviceTypes::BMA421;
break;
case BMA425_CHIP_ID:
deviceType = DeviceTypes::BMA425;
break;
default:
deviceType = DeviceTypes::Unknown;
break;
}
ret = bma423_write_config_file(&bma);
if (ret != BMA4_OK)
return;
ret = bma4_set_interrupt_mode(BMA4_LATCH_MODE, &bma);
if (ret != BMA4_OK)
return;
ret = bma423_feature_enable(BMA423_STEP_CNTR, 1, &bma);
if (ret != BMA4_OK)
return;
ret = bma423_step_detector_enable(0, &bma);
if (ret != BMA4_OK)
return;
ret = bma4_set_accel_enable(1, &bma);
if (ret != BMA4_OK)
return;
accel_conf.odr = BMA4_OUTPUT_DATA_RATE_100HZ;
accel_conf.range = BMA4_ACCEL_RANGE_2G;
accel_conf.bandwidth = BMA4_ACCEL_NORMAL_AVG4;
accel_conf.perf_mode = BMA4_CIC_AVG_MODE;
ret = bma4_set_accel_config(&accel_conf, &bma);
if (ret != BMA4_OK)
return;
isOk = true;
}
void Bma421::Reset() {
uint8_t data = 0xb6;
twiMaster.Write(deviceAddress, 0x7E, &data, 1);
}
void Bma421::Read(uint8_t registerAddress, uint8_t* buffer, size_t size) {
twiMaster.Read(deviceAddress, registerAddress, buffer, size);
}
void Bma421::Write(uint8_t registerAddress, const uint8_t* data, size_t size) {
twiMaster.Write(deviceAddress, registerAddress, data, size);
}
Bma421::Values Bma421::Process() {
if (not isOk)
return {};
struct bma4_accel rawData;
struct bma4_accel data;
bma4_read_accel_xyz(&rawData, &bma);
// Scale the measured ADC counts to units of 'binary milli-g'
// where 1g = 1024 'binary milli-g' units.
// See https://github.com/InfiniTimeOrg/InfiniTime/pull/1950 for
// discussion of why we opted for scaling to 1024 rather than 1000.
data.x = 1024 * rawData.x / accelScaleFactors[accel_conf.range];
data.y = 1024 * rawData.y / accelScaleFactors[accel_conf.range];
data.z = 1024 * rawData.z / accelScaleFactors[accel_conf.range];
uint32_t steps = 0;
bma423_step_counter_output(&steps, &bma);
int32_t temperature;
bma4_get_temperature(&temperature, BMA4_DEG, &bma);
temperature = temperature / 1000;
uint8_t activity = 0;
bma423_activity_output(&activity, &bma);
// X and Y axis are swapped because of the way the sensor is mounted in the PineTime
return {steps, data.y, data.x, data.z};
}
bool Bma421::IsOk() const {
return isOk;
}
void Bma421::ResetStepCounter() {
bma423_reset_step_counter(&bma);
}
void Bma421::SoftReset() {
auto ret = bma4_soft_reset(&bma);
if (ret == BMA4_OK) {
isResetOk = true;
nrf_delay_ms(1);
}
}
Bma421::DeviceTypes Bma421::DeviceType() const {
return deviceType;
}