Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 9050b2e

Browse filesBrowse files
dpgeorgedpgeorge
committed
stmhal: Improved interface to accelerometer.
1 parent 6fc5449 commit 9050b2e
Copy full SHA for 9050b2e

4 files changed

+107-59Lines changed: 107 additions & 59 deletions

File tree

Expand file treeCollapse file tree
Open diff view settings
Filter options
Expand file treeCollapse file tree
Open diff view settings
Collapse file

‎stmhal/accel.c‎

Copy file name to clipboardExpand all lines: stmhal/accel.c
+104-44Lines 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);
Collapse file

‎stmhal/accel.h‎

Copy file name to clipboard
+1-9Lines 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);
Collapse file

‎stmhal/pybmodule.c‎

Copy file name to clipboardExpand all lines: stmhal/pybmodule.c
+1-3Lines 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
Collapse file

‎stmhal/qstrdefsport.h‎

Copy file name to clipboardExpand all lines: stmhal/qstrdefsport.h
+1-3Lines 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 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.