samples/sensor: Add sample to read from FIFO stream
Add sample to read from up to 10 FIFO streaming devices, using the
sensor_stream() API and the RTIO framework:
https://docs.zephyrproject.org/latest/services/rtio/index.html
The devices has to be aliased as streamN (0 <= N <= 9) in DT, and
will be instantiated using SENSOR_DT_STREAM_IODEV() macro.
Currently the sample gets/prints data on SENSOR_TRIG_FIFO_WATERMARK
trigger basis for the following sensor channels:
- SENSOR_CHAN_ACCEL_XYZ
- SENSOR_CHAN_GYRO_XYZ
- SENSOR_CHAN_DIE_TEMP
- SENSOR_CHAN_GAME_ROTATION_VECTOR
- SENSOR_CHAN_GRAVITY_VECTOR
- SENSOR_CHAN_GBIAS_XYZ
Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
Armando Visconti
Benjamin Cabé
parent
c69a13d57e
commit
7c1191c926
9 changed files with 510 additions and 0 deletions
10
samples/sensor/stream_fifo/CMakeLists.txt
Normal file
10
samples/sensor/stream_fifo/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,10 @@
|
|||
# Copyright (c) 2024 STMicroelectronics
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
cmake_minimum_required(VERSION 3.20.0)
|
||||
|
||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
||||
project(stream_fifo)
|
||||
|
||||
FILE(GLOB app_sources src/*.c)
|
||||
target_sources(app PRIVATE ${app_sources})
|
||||
143
samples/sensor/stream_fifo/README.rst
Normal file
143
samples/sensor/stream_fifo/README.rst
Normal file
|
|
@ -0,0 +1,143 @@
|
|||
.. zephyr:code-sample:: stream_fifo
|
||||
:name: Generic device FIFO streaming
|
||||
:relevant-api: sensor_interface
|
||||
|
||||
Get accelerometer/gyroscope/temperature FIFO data frames from a sensor using
|
||||
SENSOR_TRIG_FIFO_WATERMARK as a trigger.
|
||||
|
||||
Overview
|
||||
********
|
||||
|
||||
This sample application demonstrates how to stream FIFO data using the :ref:`RTIO framework <rtio>`.
|
||||
|
||||
The streaming is started using the sensor_stream() API and it is self-sustained by the
|
||||
SENSOR_TRIG_FIFO_WATERMARK trigger.
|
||||
|
||||
Currently the sample gets/prints data for the following sensor channels:
|
||||
|
||||
- SENSOR_CHAN_ACCEL_XYZ
|
||||
- SENSOR_CHAN_GYRO_XYZ
|
||||
- SENSOR_CHAN_DIE_TEMP
|
||||
- SENSOR_CHAN_GAME_ROTATION_VECTOR
|
||||
- SENSOR_CHAN_GRAVITY_VECTOR
|
||||
- SENSOR_CHAN_GBIAS_XYZ
|
||||
|
||||
Building and Running
|
||||
********************
|
||||
|
||||
This sample supports up to 10 FIFO streaming devices. Each device needs
|
||||
to be aliased as :samp:`stream{N}` where ``N`` goes from ``0`` to ``9``. For example:
|
||||
|
||||
.. code-block:: devicetree
|
||||
|
||||
/ {
|
||||
aliases {
|
||||
stream0 = &lsm6dsv16x_6b_x_nucleo_iks4a1;
|
||||
};
|
||||
};
|
||||
|
||||
.. note::
|
||||
Note that NUM_SENSORS defined in main.c must match ``N`` and should be set accordingly.
|
||||
|
||||
Example devicetree overlays and configurations are already available for nucleo_f401re and
|
||||
nucleo_h503rb in the boards directory:
|
||||
|
||||
- :zephyr_file:`samples/sensor/stream_fifo/boards/nucleo_f401re.overlay`
|
||||
|
||||
DT overlay file for the nucleo_f401re board.
|
||||
|
||||
- :zephyr_file:`samples/sensor/stream_fifo/boards/nucleo_f401re.conf`
|
||||
|
||||
Configuration file for the nucleo_f401re board.
|
||||
|
||||
- :zephyr_file:`samples/sensor/stream_fifo/boards/nucleo_h503rb.overlay`
|
||||
|
||||
DT overlay file for the nucleo_h503rb board.
|
||||
|
||||
- :zephyr_file:`samples/sensor/stream_fifo/boards/nucleo_h503rb.conf`
|
||||
|
||||
Configuration file for the nucleo_h503rb board.
|
||||
|
||||
For example, build and run sample for nucleo_h503rb with:
|
||||
|
||||
.. zephyr-app-commands::
|
||||
:zephyr-app: samples/sensor/stream_fifo
|
||||
:board: nucleo_h503rb
|
||||
:goals: build flash
|
||||
:compact:
|
||||
|
||||
Sample Output
|
||||
=============
|
||||
|
||||
The following example output is for lsm6dsv16x IMU device with accelerometer, gyroscope
|
||||
and temperature sensor including also the Sensor Fusion Low Power (SFLP) information.
|
||||
The FIFO watermark is set to 64. The board used is a nucleo_h503rb
|
||||
equipped with a :ref:`x-nucleo-iks4a1` shield.
|
||||
|
||||
.. code-block:: console
|
||||
|
||||
FIFO count - 64
|
||||
XL data for lsm6dsv16x@6b 1680572433340ns (0.373229, -0.009569, 9.909734)
|
||||
XL data for lsm6dsv16x@6b 1680639100006ns (0.354089, -0.023924, 9.909734)
|
||||
XL data for lsm6dsv16x@6b 1680705766672ns (0.368444, 0.000000, 9.933659)
|
||||
XL data for lsm6dsv16x@6b 1680772433338ns (0.373229, 0.000000, 9.924089)
|
||||
XL data for lsm6dsv16x@6b 1680839100004ns (0.368444, -0.004784, 9.924089)
|
||||
XL data for lsm6dsv16x@6b 1680905766670ns (0.363659, -0.009569, 9.924089)
|
||||
XL data for lsm6dsv16x@6b 1680972433336ns (0.358874, -0.004784, 9.928874)
|
||||
XL data for lsm6dsv16x@6b 1681039100002ns (0.363659, 0.004784, 9.928874)
|
||||
GY data for lsm6dsv16x@6b 1680572433340ns (0.001524, -0.000609, 0.004269)
|
||||
GY data for lsm6dsv16x@6b 1680639100006ns (-0.001219, 0.002134, 0.004879)
|
||||
GY data for lsm6dsv16x@6b 1680705766672ns (0.001219, -0.001219, 0.004879)
|
||||
GY data for lsm6dsv16x@6b 1680772433338ns (-0.000914, 0.001219, 0.003964)
|
||||
GY data for lsm6dsv16x@6b 1680839100004ns (0.000914, -0.001219, 0.004574)
|
||||
GY data for lsm6dsv16x@6b 1680905766670ns (0.001829, 0.000914, 0.005489)
|
||||
GY data for lsm6dsv16x@6b 1680972433336ns (-0.000609, 0.000304, 0.004574)
|
||||
GY data for lsm6dsv16x@6b 1681039100002ns (0.001829, 0.000304, 0.004879)
|
||||
TP data for lsm6dsv16x@6b 1680572433340ns 24.347656 °C
|
||||
TP data for lsm6dsv16x@6b 1680639100006ns 24.324218 °C
|
||||
TP data for lsm6dsv16x@6b 1680705766672ns 24.316406 °C
|
||||
TP data for lsm6dsv16x@6b 1680772433338ns 24.296875 °C
|
||||
ROT data for lsm6dsv16x@6b 1680639100006ns (-0.000008, -0.018661, 0.021575, 0.999593)
|
||||
ROT data for lsm6dsv16x@6b 1680705766672ns (-0.000139, -0.018524, 0.021606, 0.999594)
|
||||
ROT data for lsm6dsv16x@6b 1680772433338ns (-0.000055, -0.018569, 0.021621, 0.999593)
|
||||
ROT data for lsm6dsv16x@6b 1680839100004ns (-0.000050, -0.018539, 0.021606, 0.999594)
|
||||
ROT data for lsm6dsv16x@6b 1680905766670ns (-0.000003, -0.018569, 0.021621, 0.999593)
|
||||
ROT data for lsm6dsv16x@6b 1680972433336ns (0.000044, -0.018493, 0.021667, 0.999594)
|
||||
ROT data for lsm6dsv16x@6b 1681039100002ns (0.000013, -0.018432, 0.021667, 0.999595)
|
||||
ROT data for lsm6dsv16x@6b 1681105766668ns (0.000113, -0.018402, 0.021682, 0.999595)
|
||||
GV data for lsm6dsv16x@6b 1680639100006ns (37.270999, -0.792999, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1680705766672ns (36.965999, -1.037000, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1680772433338ns (37.088001, -0.854000, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1680839100004ns (37.027000, -0.854000, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1680905766670ns (37.088001, -0.792999, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1680972433336ns (36.904998, -0.670999, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1681039100002ns (36.783000, -0.732000, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1681105766668ns (36.722000, -0.548999, 998.447998)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680572433340ns (0.000303, -0.000151, 0.004179)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680639100006ns (0.000303, 0.000000, 0.004179)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680705766672ns (0.000303, -0.000075, 0.004179)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680772433338ns (0.000227, 0.000000, 0.004179)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680839100004ns (0.000303, -0.000075, 0.004179)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680905766670ns (0.000303, 0.000000, 0.004255)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1680972433336ns (0.000303, 0.000000, 0.004255)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1681039100002ns (0.000379, 0.000000, 0.004255)
|
||||
XL data for lsm6dsv16x@6b 1681105766668ns (0.358874, -0.019139, 9.928874)
|
||||
XL data for lsm6dsv16x@6b 1681172433334ns (0.382799, -0.004784, 9.962369)
|
||||
XL data for lsm6dsv16x@6b 1681239100000ns (0.354089, 0.000000, 9.914519)
|
||||
GY data for lsm6dsv16x@6b 1681105766668ns (0.000304, 0.002134, 0.004574)
|
||||
GY data for lsm6dsv16x@6b 1681172433334ns (-0.000914, 0.000914, 0.004574)
|
||||
GY data for lsm6dsv16x@6b 1681239100000ns (0.002744, -0.002439, 0.004879)
|
||||
TP data for lsm6dsv16x@6b 1680839100004ns 24.339843 °C
|
||||
TP data for lsm6dsv16x@6b 1680905766670ns 24.339843 °C
|
||||
TP data for lsm6dsv16x@6b 1680972433336ns 24.289062 °C
|
||||
TP data for lsm6dsv16x@6b 1681039100002ns 24.296875 °C
|
||||
ROT data for lsm6dsv16x@6b 1681172433334ns (0.000049, -0.018310, 0.021697, 0.999596)
|
||||
ROT data for lsm6dsv16x@6b 1681239100000ns (0.000020, -0.018371, 0.021697, 0.999595)
|
||||
GV data for lsm6dsv16x@6b 1681172433334ns (36.539001, -0.670999, 998.447998)
|
||||
GV data for lsm6dsv16x@6b 1681239100000ns (36.660999, -0.732000, 998.447998)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1681105766668ns (0.000379, 0.000000, 0.004331)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1681172433334ns (0.000303, 0.000075, 0.004331)
|
||||
GY GBIAS data for lsm6dsv16x@6b 1681239100000ns (0.000379, 0.000000, 0.004331)
|
||||
TP data for lsm6dsv16x@6b 1681105766668ns 24.289062 °C
|
||||
TP data for lsm6dsv16x@6b 1681172433334ns 24.324218 °C
|
||||
TP data for lsm6dsv16x@6b 1681239100000ns 24.281250 °C
|
||||
6
samples/sensor/stream_fifo/boards/nucleo_f401re.conf
Normal file
6
samples/sensor/stream_fifo/boards/nucleo_f401re.conf
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
# Copyright (c) 2024 STMicroelectronics
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
CONFIG_I2C_RTIO=y
|
||||
CONFIG_LSM6DSV16X_STREAM=y
|
||||
CONFIG_LSM6DSV16X_ENABLE_TEMP=y
|
||||
38
samples/sensor/stream_fifo/boards/nucleo_f401re.overlay
Normal file
38
samples/sensor/stream_fifo/boards/nucleo_f401re.overlay
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
/*
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#include <zephyr/dt-bindings/sensor/lsm6dsv16x.h>
|
||||
|
||||
|
||||
/*
|
||||
* Nucleo F401RE board + shield iks4a1
|
||||
*
|
||||
* This devicetree overlay file will be automatically picked by the Zephyr
|
||||
* build system when building the sample for the nucleo_f401re board.
|
||||
*/
|
||||
|
||||
/ {
|
||||
aliases {
|
||||
stream0 = &lsm6dsv16x_6b_x_nucleo_iks4a1;
|
||||
};
|
||||
};
|
||||
|
||||
&arduino_i2c {
|
||||
lsm6dsv16x_6b_x_nucleo_iks4a1: lsm6dsv16x@6b {
|
||||
compatible = "st,lsm6dsv16x";
|
||||
reg = <0x6b>;
|
||||
accel-odr = <LSM6DSV16X_DT_ODR_AT_480Hz>;
|
||||
accel-range = <LSM6DSV16X_DT_FS_16G>;
|
||||
gyro-odr = <LSM6DSV16X_DT_ODR_AT_480Hz>;
|
||||
gyro-range = <LSM6DSV16X_DT_FS_500DPS>;
|
||||
fifo-watermark = <64>;
|
||||
accel-fifo-batch-rate = <LSM6DSV16X_DT_XL_BATCHED_AT_60Hz>;
|
||||
gyro-fifo-batch-rate = <LSM6DSV16X_DT_GY_BATCHED_AT_60Hz>;
|
||||
temp-fifo-batch-rate = <LSM6DSV16X_DT_TEMP_BATCHED_AT_15Hz>;
|
||||
int2-gpios = <&arduino_header 10 GPIO_ACTIVE_HIGH>; /* D4 (PB5) */
|
||||
drdy-pin = <2>;
|
||||
drdy-pulsed;
|
||||
};
|
||||
};
|
||||
6
samples/sensor/stream_fifo/boards/nucleo_h503rb.conf
Normal file
6
samples/sensor/stream_fifo/boards/nucleo_h503rb.conf
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
# Copyright (c) 2024 STMicroelectronics
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
CONFIG_I2C_RTIO=y
|
||||
CONFIG_LSM6DSV16X_STREAM=y
|
||||
CONFIG_LSM6DSV16X_ENABLE_TEMP=y
|
||||
41
samples/sensor/stream_fifo/boards/nucleo_h503rb.overlay
Normal file
41
samples/sensor/stream_fifo/boards/nucleo_h503rb.overlay
Normal file
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#include <zephyr/dt-bindings/sensor/lsm6dsv16x.h>
|
||||
|
||||
|
||||
/*
|
||||
* Nucleo F401RE board + shield iks4a1
|
||||
*
|
||||
* This devicetree overlay file will be automatically picked by the Zephyr
|
||||
* build system when building the sample for the nucleo_f401re board.
|
||||
*/
|
||||
|
||||
/ {
|
||||
aliases {
|
||||
stream0 = &lsm6dsv16x_6b_x_nucleo_iks4a1;
|
||||
};
|
||||
};
|
||||
|
||||
&arduino_i2c {
|
||||
lsm6dsv16x_6b_x_nucleo_iks4a1: lsm6dsv16x@6b {
|
||||
compatible = "st,lsm6dsv16x";
|
||||
reg = <0x6b>;
|
||||
accel-odr = <LSM6DSV16X_DT_ODR_AT_480Hz>;
|
||||
accel-range = <LSM6DSV16X_DT_FS_16G>;
|
||||
gyro-odr = <LSM6DSV16X_DT_ODR_AT_480Hz>;
|
||||
gyro-range = <LSM6DSV16X_DT_FS_500DPS>;
|
||||
fifo-watermark = <64>;
|
||||
accel-fifo-batch-rate = <LSM6DSV16X_DT_XL_BATCHED_AT_15Hz>;
|
||||
gyro-fifo-batch-rate = <LSM6DSV16X_DT_GY_BATCHED_AT_15Hz>;
|
||||
temp-fifo-batch-rate = <LSM6DSV16X_DT_TEMP_BATCHED_AT_15Hz>;
|
||||
|
||||
sflp-odr = <LSM6DSV16X_DT_SFLP_ODR_AT_15Hz>;
|
||||
sflp-fifo-enable = <LSM6DSV16X_DT_SFLP_FIFO_GAME_ROTATION_GRAVITY_GBIAS>;
|
||||
int2-gpios = <&arduino_header 10 GPIO_ACTIVE_HIGH>; /* D4 (PB5) */
|
||||
drdy-pin = <2>;
|
||||
drdy-pulsed;
|
||||
};
|
||||
};
|
||||
7
samples/sensor/stream_fifo/prj.conf
Normal file
7
samples/sensor/stream_fifo/prj.conf
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
# Copyright (c) 2024 STMicroelectronics
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
CONFIG_STDOUT_CONSOLE=y
|
||||
CONFIG_SENSOR=y
|
||||
CONFIG_SENSOR_ASYNC_API=y
|
||||
CONFIG_CBPRINTF_FP_SUPPORT=y
|
||||
12
samples/sensor/stream_fifo/sample.yaml
Normal file
12
samples/sensor/stream_fifo/sample.yaml
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
sample:
|
||||
name: Stream FIFO sample
|
||||
tests:
|
||||
sample.sensor.stream_fifo:
|
||||
harness: console
|
||||
tags: sensors
|
||||
filter: dt_alias_exists("stream0")
|
||||
harness_config:
|
||||
type: one_line
|
||||
regex:
|
||||
- "^\\s*[0-9A-Za-z_,+-.]*@[0-9A-Fa-f]* \\[m\/s\\^2\\]: \
|
||||
\\(\\s*-?[0-9\\.]*,\\s*-?[0-9\\.]*,\\s*-?[0-9\\.]*\\)$"
|
||||
247
samples/sensor/stream_fifo/src/main.c
Normal file
247
samples/sensor/stream_fifo/src/main.c
Normal file
|
|
@ -0,0 +1,247 @@
|
|||
/*
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/sys/util_macro.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
|
||||
#define STREAMDEV_ALIAS(i) DT_ALIAS(_CONCAT(stream, i))
|
||||
#define STREAMDEV_DEVICE(i, _) \
|
||||
IF_ENABLED(DT_NODE_EXISTS(STREAMDEV_ALIAS(i)), (DEVICE_DT_GET(STREAMDEV_ALIAS(i)),))
|
||||
#define NUM_SENSORS 1
|
||||
|
||||
/* support up to 10 sensors */
|
||||
static const struct device *const sensors[] = { LISTIFY(10, STREAMDEV_DEVICE, ()) };
|
||||
|
||||
#define STREAM_IODEV_SYM(id) CONCAT(stream_iodev, id)
|
||||
#define STREAM_IODEV_PTR(id, _) &STREAM_IODEV_SYM(id)
|
||||
|
||||
#define STREAM_TRIGGERS \
|
||||
{ SENSOR_TRIG_FIFO_FULL, SENSOR_STREAM_DATA_NOP }, \
|
||||
{ SENSOR_TRIG_FIFO_WATERMARK, SENSOR_STREAM_DATA_INCLUDE }
|
||||
|
||||
#define STREAM_DEFINE_IODEV(id, _) \
|
||||
SENSOR_DT_STREAM_IODEV( \
|
||||
STREAM_IODEV_SYM(id), \
|
||||
STREAMDEV_ALIAS(id), \
|
||||
STREAM_TRIGGERS);
|
||||
|
||||
LISTIFY(NUM_SENSORS, STREAM_DEFINE_IODEV, (;));
|
||||
|
||||
struct rtio_iodev *iodevs[NUM_SENSORS] = { LISTIFY(NUM_SENSORS, STREAM_IODEV_PTR, (,)) };
|
||||
|
||||
RTIO_DEFINE_WITH_MEMPOOL(stream_ctx, NUM_SENSORS, NUM_SENSORS,
|
||||
NUM_SENSORS * 20, 256, sizeof(void *));
|
||||
|
||||
struct sensor_chan_spec accel_chan = { SENSOR_CHAN_ACCEL_XYZ, 0 };
|
||||
struct sensor_chan_spec gyro_chan = { SENSOR_CHAN_GYRO_XYZ, 0 };
|
||||
struct sensor_chan_spec temp_chan = { SENSOR_CHAN_DIE_TEMP, 0 };
|
||||
struct sensor_chan_spec rot_vector_chan = { SENSOR_CHAN_GAME_ROTATION_VECTOR, 0 };
|
||||
struct sensor_chan_spec gravity_chan = { SENSOR_CHAN_GRAVITY_VECTOR, 0 };
|
||||
struct sensor_chan_spec gbias_chan = { SENSOR_CHAN_GBIAS_XYZ, 0 };
|
||||
|
||||
#define TASK_STACK_SIZE 2048ul
|
||||
static K_THREAD_STACK_ARRAY_DEFINE(thread_stack, NUM_SENSORS, TASK_STACK_SIZE);
|
||||
static struct k_thread thread_id[NUM_SENSORS];
|
||||
|
||||
static void print_stream(void *p1, void *p2, void *p3)
|
||||
{
|
||||
const struct device *dev = (const struct device *)p1;
|
||||
struct rtio_iodev *iodev = (struct rtio_iodev *)p2;
|
||||
int rc = 0;
|
||||
const struct sensor_decoder_api *decoder;
|
||||
struct rtio_cqe *cqe;
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
struct rtio_sqe *handle;
|
||||
uint8_t accel_buf[128] = { 0 };
|
||||
uint8_t gyro_buf[128] = { 0 };
|
||||
uint8_t temp_buf[64] = { 0 };
|
||||
uint8_t rot_vect_buf[128] = { 0 };
|
||||
uint8_t gravity_buf[128] = { 0 };
|
||||
uint8_t gbias_buf[128] = { 0 };
|
||||
struct sensor_three_axis_data *accel_data = (struct sensor_three_axis_data *)accel_buf;
|
||||
struct sensor_three_axis_data *gyro_data = (struct sensor_three_axis_data *)gyro_buf;
|
||||
struct sensor_q31_data *temp_data = (struct sensor_q31_data *)temp_buf;
|
||||
struct sensor_game_rotation_vector_data *rot_vect_data =
|
||||
(struct sensor_game_rotation_vector_data *)rot_vect_buf;
|
||||
struct sensor_three_axis_data *gravity_data = (struct sensor_three_axis_data *)gravity_buf;
|
||||
struct sensor_three_axis_data *gbias_data = (struct sensor_three_axis_data *)gbias_buf;
|
||||
|
||||
/* Start the stream */
|
||||
printk("sensor_stream\n");
|
||||
sensor_stream(iodev, &stream_ctx, NULL, &handle);
|
||||
|
||||
while (1) {
|
||||
cqe = rtio_cqe_consume_block(&stream_ctx);
|
||||
|
||||
if (cqe->result != 0) {
|
||||
printk("async read failed %d\n", cqe->result);
|
||||
return;
|
||||
}
|
||||
|
||||
rc = rtio_cqe_get_mempool_buffer(&stream_ctx, cqe, &buf, &buf_len);
|
||||
|
||||
if (rc != 0) {
|
||||
printk("get mempool buffer failed %d\n", rc);
|
||||
return;
|
||||
}
|
||||
|
||||
const struct device *sensor = dev;
|
||||
|
||||
rtio_cqe_release(&stream_ctx, cqe);
|
||||
|
||||
rc = sensor_get_decoder(sensor, &decoder);
|
||||
|
||||
if (rc != 0) {
|
||||
printk("sensor_get_decoder failed %d\n", rc);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Frame iterator values when data comes from a FIFO */
|
||||
uint32_t accel_fit = 0, gyro_fit = 0;
|
||||
uint32_t temp_fit = 0;
|
||||
uint32_t rot_vect_fit = 0, gravity_fit = 0, gbias_fit = 0;
|
||||
|
||||
/* Number of sensor data frames */
|
||||
uint16_t xl_count, gy_count, tp_count;
|
||||
uint16_t rot_vect_count, gravity_count, gbias_count, frame_count;
|
||||
|
||||
rc = decoder->get_frame_count(buf, accel_chan, &xl_count);
|
||||
rc += decoder->get_frame_count(buf, gyro_chan, &gy_count);
|
||||
rc += decoder->get_frame_count(buf, temp_chan, &tp_count);
|
||||
rc += decoder->get_frame_count(buf, rot_vector_chan, &rot_vect_count);
|
||||
rc += decoder->get_frame_count(buf, gravity_chan, &gravity_count);
|
||||
rc += decoder->get_frame_count(buf, gbias_chan, &gbias_count);
|
||||
|
||||
if (rc != 0) {
|
||||
printk("sensor_get_frame failed %d\n", rc);
|
||||
return;
|
||||
}
|
||||
|
||||
frame_count = xl_count + gy_count + tp_count;
|
||||
frame_count += rot_vect_count + gravity_count + gbias_count;
|
||||
|
||||
/* If a tap has occurred lets print it out */
|
||||
if (decoder->has_trigger(buf, SENSOR_TRIG_TAP)) {
|
||||
printk("Tap! Sensor %s\n", dev->name);
|
||||
}
|
||||
|
||||
/* Decode all available sensor FIFO frames */
|
||||
printk("FIFO count - %d\n", frame_count);
|
||||
|
||||
int i = 0;
|
||||
|
||||
while (i < frame_count) {
|
||||
int8_t c = 0;
|
||||
|
||||
/* decode and print Accelerometer FIFO frames */
|
||||
c = decoder->decode(buf, accel_chan, &accel_fit, 8, accel_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("XL data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
|
||||
", %" PRIq(6) ")\n", dev->name,
|
||||
PRIsensor_three_axis_data_arg(*accel_data, k));
|
||||
}
|
||||
i += c;
|
||||
|
||||
/* decode and print Gyroscope FIFO frames */
|
||||
c = decoder->decode(buf, gyro_chan, &gyro_fit, 8, gyro_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("GY data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
|
||||
", %" PRIq(6) ")\n", dev->name,
|
||||
PRIsensor_three_axis_data_arg(*gyro_data, k));
|
||||
}
|
||||
i += c;
|
||||
|
||||
/* decode and print Temperature FIFO frames */
|
||||
c = decoder->decode(buf, temp_chan, &temp_fit, 4, temp_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("TP data for %s %lluns %s%d.%d °C\n", dev->name,
|
||||
PRIsensor_q31_data_arg(*temp_data, k));
|
||||
}
|
||||
i += c;
|
||||
|
||||
/* decode and print Game Rotation Vector FIFO frames */
|
||||
c = decoder->decode(buf, rot_vector_chan, &rot_vect_fit, 8, rot_vect_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("ROT data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
|
||||
", %" PRIq(6) ", %" PRIq(6) ")\n", dev->name,
|
||||
PRIsensor_game_rotation_vector_data_arg(*rot_vect_data, k));
|
||||
}
|
||||
i += c;
|
||||
|
||||
/* decode and print Gravity Vector FIFO frames */
|
||||
c = decoder->decode(buf, gravity_chan, &gravity_fit, 8, gravity_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("GV data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
|
||||
", %" PRIq(6) ")\n", dev->name,
|
||||
PRIsensor_three_axis_data_arg(*gravity_data, k));
|
||||
}
|
||||
i += c;
|
||||
|
||||
/* decode and print Gyroscope GBIAS FIFO frames */
|
||||
c = decoder->decode(buf, gbias_chan, &gbias_fit, 8, gbias_data);
|
||||
|
||||
for (int k = 0; k < c; k++) {
|
||||
printk("GY GBIAS data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
|
||||
", %" PRIq(6) ")\n", dev->name,
|
||||
PRIsensor_three_axis_data_arg(*gbias_data, k));
|
||||
}
|
||||
i += c;
|
||||
}
|
||||
|
||||
rtio_release_buffer(&stream_ctx, buf, buf_len);
|
||||
}
|
||||
}
|
||||
|
||||
static void check_sensor_is_off(const struct device *dev)
|
||||
{
|
||||
int ret;
|
||||
struct sensor_value odr;
|
||||
|
||||
ret = sensor_attr_get(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
|
||||
|
||||
/* Check if accel is off */
|
||||
if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
|
||||
printk("%s WRN : accelerometer device is off\n", dev->name);
|
||||
}
|
||||
|
||||
ret = sensor_attr_get(dev, SENSOR_CHAN_GYRO_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
|
||||
|
||||
/* Check if gyro is off */
|
||||
if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
|
||||
printk("%s WRN : gyroscope device is off\n", dev->name);
|
||||
}
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
|
||||
if (!device_is_ready(sensors[i])) {
|
||||
printk("sensor: device %s not ready.\n", sensors[i]->name);
|
||||
return 0;
|
||||
}
|
||||
check_sensor_is_off(sensors[i]);
|
||||
|
||||
k_thread_create(&thread_id[i], thread_stack[i], TASK_STACK_SIZE, print_stream,
|
||||
(void *)sensors[i], (void *)iodevs[i], NULL, K_PRIO_COOP(5),
|
||||
K_INHERIT_PERMS, K_FOREVER);
|
||||
k_thread_start(&thread_id[i]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in a new issue