From 344d06a114bb556b84467bc57c94615b54cbc390 Mon Sep 17 00:00:00 2001
From: koalo <koalo@koalo.de>
Date: Thu, 22 Aug 2019 01:21:42 +0200
Subject: [PATCH] feat(bhi160): Provide Gyroscope data
---
epicardium/epicardium.h | 6 +-
epicardium/modules/bhi.c | 39 +++++++++++-
epicardium/modules/stream.h | 3 +-
pycardium/modules/bhi160-sys.c | 8 +--
pycardium/modules/interrupt.c | 2 +
pycardium/modules/py/bhi160.py | 112 ++++++++++++++++++++++-----------
pycardium/modules/qstrdefs.h | 1 +
7 files changed, 126 insertions(+), 45 deletions(-)
diff --git a/epicardium/epicardium.h b/epicardium/epicardium.h
index 266a5a266..7af2a7989 100644
--- a/epicardium/epicardium.h
+++ b/epicardium/epicardium.h
@@ -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,
diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
index 6aa5f3fba..04e2ec54c 100644
--- a/epicardium/modules/bhi.c
+++ b/epicardium/modules/bhi.c
@@ -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;
}
diff --git a/epicardium/modules/stream.h b/epicardium/modules/stream.h
index 8b121bfcb..668f78623 100644
--- a/epicardium/modules/stream.h
+++ b/epicardium/modules/stream.h
@@ -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,
};
diff --git a/pycardium/modules/bhi160-sys.c b/pycardium/modules/bhi160-sys.c
index c5bde7147..439ac1d42 100644
--- a/pycardium/modules/bhi160-sys.c
+++ b/pycardium/modules/bhi160-sys.c
@@ -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++) {
diff --git a/pycardium/modules/interrupt.c b/pycardium/modules/interrupt.c
index 90dd700a8..087facf7b 100644
--- a/pycardium/modules/interrupt.c
+++ b/pycardium/modules/interrupt.c
@@ -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
diff --git a/pycardium/modules/py/bhi160.py b/pycardium/modules/py/bhi160.py
index a4eb371e1..5b07cc483 100644
--- a/pycardium/modules/py/bhi160.py
+++ b/pycardium/modules/py/bhi160.py
@@ -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()),
+ }
+ )
diff --git a/pycardium/modules/qstrdefs.h b/pycardium/modules/qstrdefs.h
index 84bd55b9c..94ead401b 100644
--- a/pycardium/modules/qstrdefs.h
+++ b/pycardium/modules/qstrdefs.h
@@ -58,6 +58,7 @@ Q(set_callback)
Q(enable_callback)
Q(disable_callback)
Q(BHI160_ACCELEROMETER)
+Q(BHI160_GYROSCOPE)
Q(RTC_ALARM)
/* bhi160 */
--
GitLab