1 /*
2 * Copyright (c) 2019 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/device.h>
9 #include <zephyr/drivers/sensor.h>
10 #include <stdio.h>
11
now_str(void)12 static const char *now_str(void)
13 {
14 static char buf[16]; /* ...HH:MM:SS.MMM */
15 uint32_t now = k_uptime_get_32();
16 unsigned int ms = now % MSEC_PER_SEC;
17 unsigned int s;
18 unsigned int min;
19 unsigned int h;
20
21 now /= MSEC_PER_SEC;
22 s = now % 60U;
23 now /= 60U;
24 min = now % 60U;
25 now /= 60U;
26 h = now;
27
28 snprintf(buf, sizeof(buf), "%u:%02u:%02u.%03u",
29 h, min, s, ms);
30 return buf;
31 }
32
process_mpu6050(const struct device * dev)33 static int process_mpu6050(const struct device *dev)
34 {
35 struct sensor_value temperature;
36 struct sensor_value accel[3];
37 struct sensor_value gyro[3];
38 int rc = sensor_sample_fetch(dev);
39
40 if (rc == 0) {
41 rc = sensor_channel_get(dev, SENSOR_CHAN_ACCEL_XYZ,
42 accel);
43 }
44 if (rc == 0) {
45 rc = sensor_channel_get(dev, SENSOR_CHAN_GYRO_XYZ,
46 gyro);
47 }
48 if (rc == 0) {
49 rc = sensor_channel_get(dev, SENSOR_CHAN_DIE_TEMP,
50 &temperature);
51 }
52 if (rc == 0) {
53 printf("[%s]:%g Cel\n"
54 " accel %f %f %f m/s/s\n"
55 " gyro %f %f %f rad/s\n",
56 now_str(),
57 sensor_value_to_double(&temperature),
58 sensor_value_to_double(&accel[0]),
59 sensor_value_to_double(&accel[1]),
60 sensor_value_to_double(&accel[2]),
61 sensor_value_to_double(&gyro[0]),
62 sensor_value_to_double(&gyro[1]),
63 sensor_value_to_double(&gyro[2]));
64 } else {
65 printf("sample fetch/get failed: %d\n", rc);
66 }
67
68 return rc;
69 }
70
71 #ifdef CONFIG_MPU6050_TRIGGER
72 static struct sensor_trigger trigger;
73
handle_mpu6050_drdy(const struct device * dev,const struct sensor_trigger * trig)74 static void handle_mpu6050_drdy(const struct device *dev,
75 const struct sensor_trigger *trig)
76 {
77 int rc = process_mpu6050(dev);
78
79 if (rc != 0) {
80 printf("cancelling trigger due to failure: %d\n", rc);
81 (void)sensor_trigger_set(dev, trig, NULL);
82 return;
83 }
84 }
85 #endif /* CONFIG_MPU6050_TRIGGER */
86
main(void)87 int main(void)
88 {
89 const struct device *const mpu6050 = DEVICE_DT_GET_ONE(invensense_mpu6050);
90
91 if (!device_is_ready(mpu6050)) {
92 printf("Device %s is not ready\n", mpu6050->name);
93 return 0;
94 }
95
96 #ifdef CONFIG_MPU6050_TRIGGER
97 trigger = (struct sensor_trigger) {
98 .type = SENSOR_TRIG_DATA_READY,
99 .chan = SENSOR_CHAN_ALL,
100 };
101 if (sensor_trigger_set(mpu6050, &trigger,
102 handle_mpu6050_drdy) < 0) {
103 printf("Cannot configure trigger\n");
104 return 0;
105 }
106 printk("Configured for triggered sampling.\n");
107 #endif
108
109 while (!IS_ENABLED(CONFIG_MPU6050_TRIGGER)) {
110 int rc = process_mpu6050(mpu6050);
111
112 if (rc != 0) {
113 break;
114 }
115 k_sleep(K_SECONDS(2));
116 }
117
118 /* triggered runs with its own thread after exit */
119 return 0;
120 }
121
