1 /*
2 * Copyright (c) 2024 TDK Invensense
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/device.h>
9 #include <zephyr/devicetree.h>
10 #include <zephyr/drivers/sensor.h>
11 #include <stdio.h>
12
13 static struct sensor_trigger data_trigger;
14
15 /* Flag set from IMU device irq handler */
16 static volatile int irq_from_device;
17
18 /*
19 * Get a device structure from a devicetree node from alias
20 * "6dof_motion_drdy0".
21 */
get_6dof_motion_device(void)22 static const struct device *get_6dof_motion_device(void)
23 {
24 const struct device *const dev = DEVICE_DT_GET(DT_ALIAS(6dof_motion_drdy0));
25
26 if (!device_is_ready(dev)) {
27 printk("\nError: Device \"%s\" is not ready; "
28 "check the driver initialization logs for errors.\n",
29 dev->name);
30 return NULL;
31 }
32
33 printk("Found device \"%s\", getting sensor data\n", dev->name);
34 return dev;
35 }
36
now_str(void)37 static const char *now_str(void)
38 {
39 static char buf[16]; /* ...HH:MM:SS.MMM */
40 uint32_t now = k_uptime_get_32();
41 unsigned int ms = now % MSEC_PER_SEC;
42 unsigned int s;
43 unsigned int min;
44 unsigned int h;
45
46 now /= MSEC_PER_SEC;
47 s = now % 60U;
48 now /= 60U;
49 min = now % 60U;
50 now /= 60U;
51 h = now;
52
53 snprintf(buf, sizeof(buf), "%u:%02u:%02u.%03u", h, min, s, ms);
54 return buf;
55 }
56
handle_6dof_motion_drdy(const struct device * dev,const struct sensor_trigger * trig)57 static void handle_6dof_motion_drdy(const struct device *dev, const struct sensor_trigger *trig)
58 {
59 if (trig->type == SENSOR_TRIG_DATA_READY) {
60 int rc = sensor_sample_fetch_chan(dev, trig->chan);
61
62 if (rc < 0) {
63 printf("sample fetch failed: %d\n", rc);
64 printf("cancelling trigger due to failure: %d\n", rc);
65 (void)sensor_trigger_set(dev, trig, NULL);
66 return;
67 } else if (rc == 0) {
68 irq_from_device = 1;
69 }
70 }
71 }
72
main(void)73 int main(void)
74 {
75 const struct device *dev = get_6dof_motion_device();
76 struct sensor_value accel[3];
77 struct sensor_value gyro[3];
78 struct sensor_value temperature;
79
80 if (dev == NULL) {
81 return 0;
82 }
83
84 data_trigger = (struct sensor_trigger){
85 .type = SENSOR_TRIG_DATA_READY,
86 .chan = SENSOR_CHAN_ALL,
87 };
88 if (sensor_trigger_set(dev, &data_trigger, handle_6dof_motion_drdy) < 0) {
89 printf("Cannot configure data trigger!!!\n");
90 return 0;
91 }
92
93 k_sleep(K_MSEC(1000));
94
95 while (1) {
96
97 if (irq_from_device) {
98 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_XYZ, accel);
99 sensor_channel_get(dev, SENSOR_CHAN_GYRO_XYZ, gyro);
100 sensor_channel_get(dev, SENSOR_CHAN_DIE_TEMP, &temperature);
101
102 printf("[%s]: temp %.2f Cel "
103 " accel %f %f %f m/s/s "
104 " gyro %f %f %f rad/s\n",
105 now_str(), sensor_value_to_double(&temperature),
106 sensor_value_to_double(&accel[0]), sensor_value_to_double(&accel[1]),
107 sensor_value_to_double(&accel[2]), sensor_value_to_double(&gyro[0]),
108 sensor_value_to_double(&gyro[1]), sensor_value_to_double(&gyro[2]));
109 irq_from_device = 0;
110 }
111 }
112 return 0;
113 }
114
