Skip to content

Commit 9050b2e

Browse files
dpgeorgedpgeorge
committed
stmhal: Improved interface to accelerometer.
1 parent 6fc5449 commit 9050b2e

4 files changed

Lines changed: 107 additions & 59 deletions

File tree

‎stmhal/accel.c‎

Lines changed: 104 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,12 @@
1111
#include "accel.h"
1212

1313
#define MMA_ADDR (0x98)
14+
#define MMA_REG_X (0)
15+
#define MMA_REG_Y (1)
16+
#define MMA_REG_Z (2)
17+
#define MMA_REG_TILT (3)
1418
#define MMA_REG_MODE (7)
19+
#define MMA_AXIS_SIGNED_VALUE(i) (((i) & 0x3f) | ((i) & 0x20 ? (~0x1f) : 0))
1520

1621
STATIC I2C_HandleTypeDef I2cHandle;
1722

@@ -26,11 +31,6 @@ void accel_init(void) {
2631
GPIO_InitStructure.Pull = GPIO_NOPULL;
2732
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
2833

29-
// wait 20ms, then turn on AVDD, then wait 20ms again
30-
HAL_Delay(20);
31-
GPIOB->BSRRL = GPIO_PIN_5;
32-
HAL_Delay(20);
33-
3434
// PB6=SCL, PB7=SDA
3535
GPIO_InitStructure.Pin = GPIO_PIN_6 | GPIO_PIN_7;
3636
GPIO_InitStructure.Mode = GPIO_MODE_AF_OD;
@@ -59,6 +59,14 @@ void accel_init(void) {
5959
printf("accel_init: HAL_I2C_Init failed\n");
6060
return;
6161
}
62+
}
63+
64+
STATIC void accel_init_device(void) {
65+
// turn off AVDD, wait 20ms, turn on AVDD, wait 20ms again
66+
GPIOB->BSRRH = GPIO_PIN_5; // turn off
67+
HAL_Delay(20);
68+
GPIOB->BSRRL = GPIO_PIN_5; // turn on
69+
HAL_Delay(20);
6270

6371
HAL_StatusTypeDef status;
6472

@@ -81,57 +89,109 @@ void accel_init(void) {
8189
/******************************************************************************/
8290
/* Micro Python bindings */
8391

84-
int accel_buf[12];
92+
#define NUM_AXIS (3)
93+
#define FILT_DEPTH (4)
8594

86-
mp_obj_t pyb_accel_read(void) {
87-
for (int i = 0; i <= 6; i += 3) {
88-
accel_buf[0 + i] = accel_buf[0 + i + 3];
89-
accel_buf[1 + i] = accel_buf[1 + i + 3];
90-
accel_buf[2 + i] = accel_buf[2 + i + 3];
91-
}
95+
typedef struct _pyb_accel_obj_t {
96+
mp_obj_base_t base;
97+
int16_t buf[NUM_AXIS * FILT_DEPTH];
98+
} pyb_accel_obj_t;
9299

93-
uint8_t data_[4];
94-
HAL_I2C_Mem_Read(&I2cHandle, MMA_ADDR, 0, I2C_MEMADD_SIZE_8BIT, data_, 4, 200);
95-
accel_buf[9] = data_[0] & 0x3f; if (accel_buf[9] & 0x20) accel_buf[9] |= ~0x1f;
96-
accel_buf[10] = data_[1] & 0x3f; if (accel_buf[10] & 0x20) accel_buf[10] |= ~0x1f;
97-
accel_buf[11] = data_[2] & 0x3f; if (accel_buf[11] & 0x20) accel_buf[11] |= ~0x1f;
98-
int jolt_info = data_[3];
100+
STATIC pyb_accel_obj_t pyb_accel_obj;
101+
102+
STATIC mp_obj_t read_axis(int axis) {
103+
uint8_t data[1];
104+
HAL_I2C_Mem_Read(&I2cHandle, MMA_ADDR, axis, I2C_MEMADD_SIZE_8BIT, data, 1, 200);
105+
return mp_obj_new_int(MMA_AXIS_SIGNED_VALUE(data[0]));
106+
}
107+
108+
STATIC mp_obj_t pyb_accel_x(mp_obj_t self_in) {
109+
return read_axis(MMA_REG_X);
110+
}
111+
112+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_accel_x_obj, pyb_accel_x);
113+
114+
STATIC mp_obj_t pyb_accel_y(mp_obj_t self_in) {
115+
return read_axis(MMA_REG_Y);
116+
}
99117

100-
mp_obj_t data[4];
101-
data[0] = mp_obj_new_int(accel_buf[0] + accel_buf[3] + accel_buf[6] + accel_buf[9]);
102-
data[1] = mp_obj_new_int(accel_buf[1] + accel_buf[4] + accel_buf[7] + accel_buf[10]);
103-
data[2] = mp_obj_new_int(accel_buf[2] + accel_buf[5] + accel_buf[8] + accel_buf[11]);
104-
data[3] = mp_obj_new_int(jolt_info);
118+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_accel_y_obj, pyb_accel_y);
105119

106-
return rt_build_tuple(4, data);
120+
STATIC mp_obj_t pyb_accel_z(mp_obj_t self_in) {
121+
return read_axis(MMA_REG_Z);
107122
}
108123

109-
MP_DEFINE_CONST_FUN_OBJ_0(pyb_accel_read_obj, pyb_accel_read);
124+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_accel_z_obj, pyb_accel_z);
110125

111-
/*
112-
mp_obj_t pyb_accel_read_all(void) {
113-
mp_obj_t data[11];
114-
accel_start(MMA_ADDR, 1);
115-
accel_send_byte(0);
116-
accel_restart(MMA_ADDR, 0);
117-
for (int i = 0; i <= 9; i++) {
118-
data[i] = mp_obj_new_int(accel_read_ack());
126+
STATIC mp_obj_t pyb_accel_tilt(mp_obj_t self_in) {
127+
uint8_t data[1];
128+
HAL_I2C_Mem_Read(&I2cHandle, MMA_ADDR, MMA_REG_TILT, I2C_MEMADD_SIZE_8BIT, data, 1, 200);
129+
return mp_obj_new_int(data[0]);
130+
}
131+
132+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_accel_tilt_obj, pyb_accel_tilt);
133+
134+
STATIC mp_obj_t pyb_accel_filtered_xyz(mp_obj_t self_in) {
135+
pyb_accel_obj_t *self = self_in;
136+
137+
memmove(self->buf, self->buf + NUM_AXIS, NUM_AXIS * (FILT_DEPTH - 1) * sizeof(int16_t));
138+
139+
uint8_t data[NUM_AXIS];
140+
HAL_I2C_Mem_Read(&I2cHandle, MMA_ADDR, MMA_REG_X, I2C_MEMADD_SIZE_8BIT, data, NUM_AXIS, 200);
141+
142+
mp_obj_t tuple[NUM_AXIS];
143+
for (int i = 0; i < NUM_AXIS; i++) {
144+
self->buf[NUM_AXIS * (FILT_DEPTH - 1) + i] = MMA_AXIS_SIGNED_VALUE(data[i]);
145+
int32_t val = 0;
146+
for (int j = 0; j < FILT_DEPTH; j++) {
147+
val += self->buf[i + NUM_AXIS * j];
148+
}
149+
tuple[i] = mp_obj_new_int(val);
119150
}
120-
data[10] = mp_obj_new_int(accel_read_nack());
121151

122-
return rt_build_tuple(11, data);
152+
return rt_build_tuple(3, tuple);
123153
}
124154

125-
MP_DEFINE_CONST_FUN_OBJ_0(pyb_accel_read_all_obj, pyb_accel_read_all);
155+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_accel_filtered_xyz_obj, pyb_accel_filtered_xyz);
126156

127-
mp_obj_t pyb_accel_write_mode(mp_obj_t o_int, mp_obj_t o_mode) {
128-
accel_start(MMA_ADDR, 1);
129-
accel_send_byte(6); // start at int
130-
accel_send_byte(mp_obj_get_int(o_int));
131-
accel_send_byte(mp_obj_get_int(o_mode));
132-
accel_stop();
157+
STATIC mp_obj_t pyb_accel_read_reg(mp_obj_t self_in, mp_obj_t reg) {
158+
uint8_t data[1];
159+
HAL_I2C_Mem_Read(&I2cHandle, MMA_ADDR, mp_obj_get_int(reg), I2C_MEMADD_SIZE_8BIT, data, 1, 200);
160+
return mp_obj_new_int(data[0]);
161+
}
162+
163+
MP_DEFINE_CONST_FUN_OBJ_2(pyb_accel_read_reg_obj, pyb_accel_read_reg);
164+
165+
STATIC mp_obj_t pyb_accel_write_reg(mp_obj_t self_in, mp_obj_t reg, mp_obj_t val) {
166+
uint8_t data[1];
167+
data[0] = mp_obj_get_int(val);
168+
HAL_I2C_Mem_Write(&I2cHandle, MMA_ADDR, mp_obj_get_int(reg), I2C_MEMADD_SIZE_8BIT, data, 1, 200);
133169
return mp_const_none;
134170
}
135171

136-
MP_DEFINE_CONST_FUN_OBJ_2(pyb_accel_write_mode_obj, pyb_accel_write_mode);
137-
*/
172+
MP_DEFINE_CONST_FUN_OBJ_3(pyb_accel_write_reg_obj, pyb_accel_write_reg);
173+
174+
STATIC const mp_method_t accel_methods[] = {
175+
{ "x", &pyb_accel_x_obj },
176+
{ "y", &pyb_accel_y_obj },
177+
{ "z", &pyb_accel_z_obj },
178+
{ "tilt", &pyb_accel_tilt_obj },
179+
{ "filtered_xyz", &pyb_accel_filtered_xyz_obj },
180+
{ "read_reg", &pyb_accel_read_reg_obj },
181+
{ "write_reg", &pyb_accel_write_reg_obj },
182+
{ NULL, NULL },
183+
};
184+
185+
STATIC const mp_obj_type_t accel_obj_type = {
186+
{ &mp_type_type },
187+
.name = MP_QSTR_Accel,
188+
.methods = accel_methods,
189+
};
190+
191+
STATIC mp_obj_t pyb_Accel(void) {
192+
pyb_accel_obj.base.type = &accel_obj_type;
193+
accel_init_device();
194+
return &pyb_accel_obj;
195+
}
196+
197+
MP_DEFINE_CONST_FUN_OBJ_0(pyb_Accel_obj, pyb_Accel);

‎stmhal/accel.h‎

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,3 @@
11
void accel_init(void);
2-
void accel_restart(uint8_t addr, int write);
3-
void accel_start(uint8_t addr, int write);
4-
void accel_send_byte(uint8_t data);
5-
uint8_t accel_read_ack(void);
6-
uint8_t accel_read_nack(void);
7-
void accel_stop(void);
82

9-
MP_DECLARE_CONST_FUN_OBJ(pyb_accel_read_obj);
10-
MP_DECLARE_CONST_FUN_OBJ(pyb_accel_read_all_obj);
11-
MP_DECLARE_CONST_FUN_OBJ(pyb_accel_write_mode_obj);
3+
MP_DECLARE_CONST_FUN_OBJ(pyb_Accel_obj);

‎stmhal/pybmodule.c‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -254,9 +254,7 @@ STATIC const mp_map_elem_t pyb_module_globals_table[] = {
254254
#endif
255255

256256
#if MICROPY_HW_HAS_MMA7660
257-
{ MP_OBJ_NEW_QSTR(MP_QSTR_accel), (mp_obj_t)&pyb_accel_read_obj },
258-
//{ MP_OBJ_NEW_QSTR(MP_QSTR_accel_read), (mp_obj_t)&pyb_accel_read_all_obj },
259-
//{ MP_OBJ_NEW_QSTR(MP_QSTR_accel_mode), (mp_obj_t)&pyb_accel_write_mode_obj },
257+
{ MP_OBJ_NEW_QSTR(MP_QSTR_Accel), (mp_obj_t)&pyb_Accel_obj },
260258
#endif
261259

262260
#if 0

‎stmhal/qstrdefsport.h‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,7 @@ Q(switch)
1717
Q(SW)
1818
Q(servo)
1919
Q(pwm)
20-
Q(accel)
21-
Q(accel_read)
22-
Q(accel_mode)
20+
Q(Accel)
2321
Q(hid)
2422
Q(time)
2523
Q(rng)

0 commit comments

Comments
 (0)