Verified Commit 99b6e7c9 authored by koalo's avatar koalo Committed by Rahix
Browse files

feat(bhi160): Prepare for other data types

parent 344d06a1
......@@ -154,7 +154,7 @@ API(API_INTERRUPT_DISABLE, int epic_interrupt_disable(api_int_id_t int_id));
#define EPIC_INT_UART_RX 2
/** RTC Alarm interrupt. See :c:func:`epic_isr_rtc_alarm` */
#define EPIC_INT_RTC_ALARM 3
/** TODO: BHI */
/** BHI */
#define EPIC_INT_BHI160_ACCELEROMETER 4
API_ISR(EPIC_INT_BHI160_ACCELEROMETER, epic_isr_bhi160_accelerometer);
#define EPIC_INT_BHI160_GYROSCOPE 5
......@@ -943,6 +943,10 @@ enum bhi160_sensor_type {
BHI160_GEOMAGNETIC_ROTATION_VECTOR = 10,
};
enum bhi160_data_type {
BHI160_DATA_TYPE_VECTOR
};
/**
* BHI160 Sensor Data Types
* ------------------------
......@@ -953,6 +957,8 @@ enum bhi160_sensor_type {
* range. See the individual sensor's documentation for details.
*/
struct bhi160_data_vector {
enum bhi160_data_type data_type;
/** X */
int16_t x;
/** Y */
......
......@@ -139,8 +139,9 @@ int epic_bhi160_enable_sensor(
return -ENOMEM;
}
int streamret = stream_register(bhi160_lookup_sd(sensor_type), stream);
if (streamret < 0) {
int streamret =
stream_register(bhi160_lookup_sd(sensor_type), stream);
if (streamret < 0) {
xSemaphoreGive(bhi160_mutex);
hwlock_release(HWLOCK_I2C);
return streamret;
......@@ -214,54 +215,61 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
* Timestamp of the next samples, counting at 32 kHz.
* Currently unused.
*/
static uint32_t timestamp = 0;
static uint32_t timestamp = 0;
enum bhi160_sensor_type sensor_type = 0;
int epic_int = 0;
bool wakeup = false;
switch (sensor_id) {
case VS_ID_TIMESTAMP_MSW:
case VS_ID_TIMESTAMP_MSW_WAKEUP:
wakeup = true;
/* fall through */
case VS_ID_TIMESTAMP_MSW:
MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
timestamp = sensor_data->data_scalar_u16.data << 16;
break;
case VS_ID_TIMESTAMP_LSW:
case VS_ID_TIMESTAMP_LSW_WAKEUP:
wakeup = true;
/* fall through */
case VS_ID_TIMESTAMP_LSW:
MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
timestamp = (timestamp & 0xFFFF0000) |
sensor_data->data_scalar_u16.data;
break;
case VS_ID_ACCELEROMETER:
case VS_ID_ACCELEROMETER_WAKEUP:
MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
if (bhi160_streams[BHI160_ACCELEROMETER].queue == NULL) {
case VS_ID_GYROSCOPE_WAKEUP:
wakeup = true;
/* fall through */
case VS_ID_ACCELEROMETER:
case VS_ID_GYROSCOPE:
switch (sensor_id) {
case VS_ID_ACCELEROMETER_WAKEUP:
case VS_ID_ACCELEROMETER:
sensor_type = BHI160_ACCELEROMETER;
epic_int = EPIC_INT_BHI160_ACCELEROMETER;
break;
case VS_ID_GYROSCOPE_WAKEUP:
case VS_ID_GYROSCOPE:
sensor_type = BHI160_GYROSCOPE;
epic_int = EPIC_INT_BHI160_GYROSCOPE;
break;
}
data_vector.x = sensor_data->data_vector.x;
data_vector.y = sensor_data->data_vector.y;
data_vector.z = sensor_data->data_vector.z;
xQueueSend(
bhi160_streams[BHI160_ACCELEROMETER].queue,
&data_vector,
BHI160_MUTEX_WAIT_MS
);
if (sensor_id == VS_ID_ACCELEROMETER_WAKEUP) {
api_interrupt_trigger(EPIC_INT_BHI160_ACCELEROMETER);
}
break;
case VS_ID_GYROSCOPE:
case VS_ID_GYROSCOPE_WAKEUP:
MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
if (bhi160_streams[BHI160_GYROSCOPE].queue == NULL) {
if (bhi160_streams[sensor_type].queue == NULL) {
break;
}
data_vector.x = sensor_data->data_vector.x;
data_vector.y = sensor_data->data_vector.y;
data_vector.z = sensor_data->data_vector.z;
data_vector.data_type = BHI160_DATA_TYPE_VECTOR;
data_vector.x = sensor_data->data_vector.x;
data_vector.y = sensor_data->data_vector.y;
data_vector.z = sensor_data->data_vector.z;
xQueueSend(
bhi160_streams[BHI160_GYROSCOPE].queue,
bhi160_streams[sensor_type].queue,
&data_vector,
BHI160_MUTEX_WAIT_MS
);
if (sensor_id == VS_ID_GYROSCOPE_WAKEUP) {
api_interrupt_trigger(EPIC_INT_BHI160_GYROSCOPE);
if (wakeup) {
api_interrupt_trigger(epic_int);
}
break;
default:
......
......@@ -7,13 +7,6 @@
extern const mp_obj_type_t mp_type_bhi160_sample;
typedef struct _bhi160_sample_obj_t {
mp_obj_base_t base;
int x;
int y;
int z;
} bhi160_sample_obj_t;
STATIC mp_obj_t mp_bhi160_enable_sensor(size_t n_args, const mp_obj_t *args)
{
int sensor_type = mp_obj_get_int(args[0]);
......@@ -37,55 +30,20 @@ STATIC mp_obj_t mp_bhi160_read_sensor(mp_obj_t stream_id_in)
mp_obj_list_t *list = mp_obj_new_list(0, NULL);
for (int i = 0; i < n; i++) {
bhi160_sample_obj_t *o = m_new_obj(bhi160_sample_obj_t);
o->base.type = &mp_type_bhi160_sample;
o->x = buf[i].x;
o->y = buf[i].y;
o->z = buf[i].z;
mp_obj_list_append(list, MP_OBJ_FROM_PTR(o));
if (buf[i].data_type != BHI160_DATA_TYPE_VECTOR) {
// other data types are currently not supported
mp_raise_OSError(EINVAL);
}
mp_obj_t tuple[3];
tuple[0] = mp_obj_new_int(buf[i].x);
tuple[1] = mp_obj_new_int(buf[i].y);
tuple[2] = mp_obj_new_int(buf[i].z);
mp_obj_list_append(list, mp_obj_new_tuple(3, tuple));
}
return MP_OBJ_FROM_PTR(list);
}
STATIC mp_obj_t mp_bhi160_x(mp_obj_t type)
{
bhi160_sample_obj_t *self = MP_OBJ_TO_PTR(type);
return MP_OBJ_NEW_SMALL_INT(self->x);
}
static MP_DEFINE_CONST_FUN_OBJ_1(mp_bhi160_x_obj, mp_bhi160_x);
STATIC mp_obj_t mp_bhi160_y(mp_obj_t type)
{
bhi160_sample_obj_t *self = MP_OBJ_TO_PTR(type);
return MP_OBJ_NEW_SMALL_INT(self->y);
}
static MP_DEFINE_CONST_FUN_OBJ_1(mp_bhi160_y_obj, mp_bhi160_y);
STATIC mp_obj_t mp_bhi160_z(mp_obj_t type)
{
bhi160_sample_obj_t *self = MP_OBJ_TO_PTR(type);
return MP_OBJ_NEW_SMALL_INT(self->z);
}
static MP_DEFINE_CONST_FUN_OBJ_1(mp_bhi160_z_obj, mp_bhi160_z);
STATIC const mp_rom_map_elem_t bhi160_sample_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR_x), MP_ROM_PTR(&mp_bhi160_x_obj) },
{ MP_ROM_QSTR(MP_QSTR_y), MP_ROM_PTR(&mp_bhi160_y_obj) },
{ MP_ROM_QSTR(MP_QSTR_z), MP_ROM_PTR(&mp_bhi160_z_obj) },
};
STATIC MP_DEFINE_CONST_DICT(
bhi160_sample_locals_dict, bhi160_sample_locals_dict_table
);
const mp_obj_type_t mp_type_bhi160_sample = {
{ &mp_type_type },
.name = MP_QSTR_BHI160Sample,
.locals_dict = (mp_obj_dict_t *)&bhi160_sample_locals_dict,
};
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(
mp_bhi160_enable_sensor_obj, 4, 4, mp_bhi160_enable_sensor
);
......
......@@ -65,9 +65,9 @@ class BHI160Accelerometer(BHI160):
def convert(self, sample):
return dict(
{
"x": self.convert_single(sample.x()),
"y": self.convert_single(sample.y()),
"z": self.convert_single(sample.z()),
"x": self.convert_single(sample[0]),
"y": self.convert_single(sample[1]),
"z": self.convert_single(sample[2]),
}
)
......@@ -91,8 +91,8 @@ class BHI160Gyroscope(BHI160):
def convert(self, sample):
return dict(
{
"x": self.convert_single(sample.x()),
"y": self.convert_single(sample.y()),
"z": self.convert_single(sample.z()),
"x": self.convert_single(sample[0]),
"y": self.convert_single(sample[1]),
"z": self.convert_single(sample[2]),
}
)
......@@ -65,7 +65,6 @@ Q(RTC_ALARM)
Q(sys_bhi160)
Q(enable_sensor)
Q(read_sensor)
Q(BHI160Sample)
Q(x)
Q(y)
Q(z)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment