Skip to content

GitLab

Verified Commit 344d06a1 authored by koalo's avatar koalo Committed by Rahix
Browse files

feat(bhi160): Provide Gyroscope data

parent 3c021925
Hide whitespace changes
Inline Side-by-side
epicardium/epicardium.h
View file @ 344d06a1
......@@ -157,9 +157,11 @@ API(API_INTERRUPT_DISABLE, int epic_interrupt_disable(api_int_id_t int_id));
/** TODO: BHI */
#define EPIC_INT_BHI160_ACCELEROMETER 4
API_ISR(EPIC_INT_BHI160_ACCELEROMETER, epic_isr_bhi160_accelerometer);
#define EPIC_INT_BHI160_GYROSCOPE 5
API_ISR(EPIC_INT_BHI160_GYROSCOPE, epic_isr_bhi160_gyroscope);
/* Number of defined interrupts. */
#define EPIC_INT_NUM 5
#define EPIC_INT_NUM 6
/* clang-format on */
/*
......@@ -923,7 +925,7 @@ enum bhi160_sensor_type {
BHI160_MAGNETOMETER = 1,
/** Orientation (**Unimplemented**) */
BHI160_ORIENTATION = 2,
/** Gyroscope (**Unimplemented**) */
/** Gyroscope */
BHI160_GYROSCOPE = 3,
/** Gravity (**Unimplemented**) */
BHI160_GRAVITY = 4,
......
epicardium/modules/bhi.c
View file @ 344d06a1
......@@ -73,6 +73,7 @@ static size_t bhi160_lookup_data_size(enum bhi160_sensor_type type)
case BHI160_ACCELEROMETER:
case BHI160_MAGNETOMETER:
case BHI160_ORIENTATION:
case BHI160_GYROSCOPE:
return sizeof(struct bhi160_data_vector);
default:
return 0;
......@@ -87,6 +88,8 @@ static bhy_virtual_sensor_t bhi160_lookup_vs_id(enum bhi160_sensor_type type)
switch (type) {
case BHI160_ACCELEROMETER:
return VS_ID_ACCELEROMETER;
case BHI160_GYROSCOPE:
return VS_ID_GYROSCOPE;
default:
return -1;
}
......@@ -100,6 +103,8 @@ static int bhi160_lookup_sd(enum bhi160_sensor_type type)
switch (type) {
case BHI160_ACCELEROMETER:
return SD_BHI160_ACCELEROMETER;
case BHI160_GYROSCOPE:
return SD_BHI160_GYROSCOPE;
default:
return -1;
}
......@@ -134,9 +139,14 @@ int epic_bhi160_enable_sensor(
return -ENOMEM;
}
stream_register(bhi160_lookup_sd(sensor_type), stream);
int streamret = stream_register(bhi160_lookup_sd(sensor_type), stream);
if (streamret < 0) {
xSemaphoreGive(bhi160_mutex);
hwlock_release(HWLOCK_I2C);
return streamret;
}
bhy_enable_virtual_sensor(
int bhyret = bhy_enable_virtual_sensor(
vs_id,
VS_WAKEUP,
config->sample_rate,
......@@ -145,6 +155,11 @@ int epic_bhi160_enable_sensor(
0,
config->dynamic_range /* dynamic range is sensor dependent */
);
if (bhyret != BHY_SUCCESS) {
xSemaphoreGive(bhi160_mutex);
hwlock_release(HWLOCK_I2C);
return bhyret;
}
xSemaphoreGive(bhi160_mutex);
} else {
hwlock_release(HWLOCK_I2C);
......@@ -152,7 +167,7 @@ int epic_bhi160_enable_sensor(
}
hwlock_release(HWLOCK_I2C);
return 0;
return bhi160_lookup_sd(sensor_type);
}
int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
......@@ -231,6 +246,24 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
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) {
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_GYROSCOPE].queue,
&data_vector,
BHI160_MUTEX_WAIT_MS
);
if (sensor_id == VS_ID_GYROSCOPE_WAKEUP) {
api_interrupt_trigger(EPIC_INT_BHI160_GYROSCOPE);
}
break;
default:
break;
}
......
epicardium/modules/stream.h
View file @ 344d06a1
......@@ -25,8 +25,9 @@ typedef unsigned int size_t;
* Please keep IDs in sequential order.
*/
enum stream_descriptor {
/** BHI160 Accelerometer */
/** BHI160 */
SD_BHI160_ACCELEROMETER,
SD_BHI160_GYROSCOPE,
/** Highest descriptor must always be ``SD_MAX``. */
SD_MAX,
};
......
pycardium/modules/bhi160-sys.c
View file @ 344d06a1
......@@ -23,17 +23,17 @@ STATIC mp_obj_t mp_bhi160_enable_sensor(size_t n_args, const mp_obj_t *args)
cfg.sample_rate = mp_obj_get_int(args[2]);
cfg.dynamic_range = mp_obj_get_int(args[3]);
int sd = epic_bhi160_enable_sensor(sensor_type, &cfg);
int stream_id = epic_bhi160_enable_sensor(sensor_type, &cfg);
return MP_OBJ_NEW_SMALL_INT(sd);
return MP_OBJ_NEW_SMALL_INT(stream_id);
}
STATIC mp_obj_t mp_bhi160_read_sensor(mp_obj_t stream_id_in)
{
struct bhi160_data_vector buf[100];
int sd = mp_obj_get_int(stream_id_in);
int stream_id = mp_obj_get_int(stream_id_in);
int n = epic_stream_read(sd, buf, sizeof(buf));
int n = epic_stream_read(stream_id, buf, sizeof(buf));
mp_obj_list_t *list = mp_obj_new_list(0, NULL);
for (int i = 0; i < n; i++) {
......
pycardium/modules/interrupt.c
View file @ 344d06a1
......@@ -87,6 +87,8 @@ static const mp_rom_map_elem_t interrupt_module_globals_table[] = {
MP_OBJ_NEW_SMALL_INT(EPIC_INT_RTC_ALARM) },
{ MP_ROM_QSTR(MP_QSTR_BHI160_ACCELEROMETER),
MP_OBJ_NEW_SMALL_INT(EPIC_INT_BHI160_ACCELEROMETER) },
{ MP_ROM_QSTR(MP_QSTR_BHI160_GYROSCOPE),
MP_OBJ_NEW_SMALL_INT(EPIC_INT_BHI160_GYROSCOPE) },
};
static MP_DEFINE_CONST_DICT(
interrupt_module_globals, interrupt_module_globals_table
......
pycardium/modules/py/bhi160.py
View file @ 344d06a1
......@@ -2,20 +2,21 @@ import sys_bhi160
import interrupt
class BHI160Accelerometer:
def __init__(
self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
):
interrupt.disable_callback(interrupt.BHI160_ACCELEROMETER)
interrupt.set_callback(
interrupt.BHI160_ACCELEROMETER, self._accelerometer_interrupt
)
self.acc_sd = sys_bhi160.enable_sensor(
0, sample_buffer_len, sample_rate, dynamic_range
class BHI160:
def enable_sensor(self):
interrupt.disable_callback(self.interrupt_id)
interrupt.set_callback(self.interrupt_id, self._interrupt)
self.stream_id = sys_bhi160.enable_sensor(
self.sensor_id, self.sample_buffer_len, self.sample_rate, self.dynamic_range
)
self._callback = callback
if callback:
interrupt.enable_callback(interrupt.BHI160_ACCELEROMETER)
if self.stream_id < 0:
raise ValueError("Enable sensor returned %i", self.stream_id)
self.active = True
if self._callback:
interrupt.enable_callback(self.interrupt_id)
def __enter__(self):
return self
......@@ -24,33 +25,74 @@ class BHI160Accelerometer:
self.close()
def close(self):
if self.acc_sd is not None:
self.acc_sd = None
self.acc_sd = sys_bhi160.disable_sensor(0)
interrupt.disable_callback(interrupt.BHI160_ACCELEROMETER)
interrupt.set_callback(interrupt.BHI160_ACCELEROMETER, None)
def convert(self, value):
return 2 * value / 32768.0
if self.active:
self.active = False
sys_bhi160.disable_sensor(self.sensor_id)
interrupt.disable_callback(self.interrupt_id)
interrupt.set_callback(self.interrupt_id, None)
def read(self):
result = []
if self.acc_sd is not None:
for sample in sys_bhi160.read_sensor(self.acc_sd):
result.append(
dict(
{
"x": self.convert(sample.x()),
"y": self.convert(sample.y()),
"z": self.convert(sample.z()),
}
)
)
if self.active:
for sample in sys_bhi160.read_sensor(self.stream_id):
result.append(self.convert(sample))
return result
def _accelerometer_interrupt(self, _):
if self.acc_sd is not None:
data = sys_bhi160.read_sensor(self.acc_sd)
def _interrupt(self, _):
if self.active:
data = self.read()
print(data)
if self._callback:
self._callback(data)
class BHI160Accelerometer(BHI160):
def __init__(
self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
):
self.sample_rate = sample_rate
self.dynamic_range = dynamic_range
self.callback = callback
self.sample_buffer_len = sample_buffer_len
self.sensor_id = 0
self.interrupt_id = interrupt.BHI160_ACCELEROMETER
self._callback = callback
self.enable_sensor()
def convert_single(self, value):
return 2 * value / 32768.0
def convert(self, sample):
return dict(
{
"x": self.convert_single(sample.x()),
"y": self.convert_single(sample.y()),
"z": self.convert_single(sample.z()),
}
)
class BHI160Gyroscope(BHI160):
def __init__(
self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
):
self.sample_rate = sample_rate
self.dynamic_range = dynamic_range
self.callback = callback
self.sample_buffer_len = sample_buffer_len
self.sensor_id = 3
self.interrupt_id = interrupt.BHI160_GYROSCOPE
self._callback = callback
self.enable_sensor()
def convert_single(self, value):
return 360 * value / 32768.0
def convert(self, sample):
return dict(
{
"x": self.convert_single(sample.x()),
"y": self.convert_single(sample.y()),
"z": self.convert_single(sample.z()),
}
)
pycardium/modules/qstrdefs.h
View file @ 344d06a1
......@@ -58,6 +58,7 @@ Q(set_callback)
Q(enable_callback)
Q(disable_callback)
Q(BHI160_ACCELEROMETER)
Q(BHI160_GYROSCOPE)
Q(RTC_ALARM)
/* bhi160 */
......
Markdown is supported
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