1 /*
2 * Copyright (c) 2018 Diego Sueiro
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <ctype.h>
8 #include <stdlib.h>
9 #include <string.h>
10
11 #include <zephyr/device.h>
12 #include <zephyr/drivers/sensor.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/rtio/rtio.h>
15 #include <zephyr/shell/shell.h>
16 #include <zephyr/sys/iterable_sections.h>
17 #include <zephyr/sys/util.h>
18
19 #include "sensor_shell.h"
20
21 LOG_MODULE_REGISTER(sensor_shell, CONFIG_SENSOR_LOG_LEVEL);
22
23 #define SENSOR_GET_HELP \
24 SHELL_HELP("Get sensor data.\n" \
25 "Channel names are optional. All channels are read when no channels are " \
26 "provided.", \
27 "<device_name> [<channel name 0> .. <channel name N>]")
28
29 #define SENSOR_STREAM_HELP \
30 SHELL_HELP("Start/stop streaming sensor data.\n" \
31 "Data ready trigger will be used if no triggers are provided.", \
32 "<device_name> on|off <trigger name> incl|drop|nop")
33
34 #define SENSOR_ATTR_GET_HELP \
35 SHELL_HELP("Get the sensor's channel attribute.", \
36 "<device_name> [<channel_name 0> <attribute_name 0> .. " \
37 "<channel_name N> <attribute_name N>]")
38
39 #define SENSOR_ATTR_SET_HELP \
40 SHELL_HELP("Set the sensor's channel attribute.", \
41 "<device_name> <channel_name> <attribute_name> <value>")
42
43 #define SENSOR_INFO_HELP \
44 SHELL_HELP("Get sensor info, such as vendor and model name, for all sensors.", \
45 "<device_name>")
46
47 #define SENSOR_TRIG_HELP \
48 SHELL_HELP("Get or set the trigger type on a sensor.\n" \
49 "Currently only supports `data_ready`.", \
50 "<device_name> <on/off> <trigger_name>")
51
52 static const char *const sensor_channel_name[SENSOR_CHAN_COMMON_COUNT] = {
53 [SENSOR_CHAN_ACCEL_X] = "accel_x",
54 [SENSOR_CHAN_ACCEL_Y] = "accel_y",
55 [SENSOR_CHAN_ACCEL_Z] = "accel_z",
56 [SENSOR_CHAN_ACCEL_XYZ] = "accel_xyz",
57 [SENSOR_CHAN_GYRO_X] = "gyro_x",
58 [SENSOR_CHAN_GYRO_Y] = "gyro_y",
59 [SENSOR_CHAN_GYRO_Z] = "gyro_z",
60 [SENSOR_CHAN_GYRO_XYZ] = "gyro_xyz",
61 [SENSOR_CHAN_MAGN_X] = "magn_x",
62 [SENSOR_CHAN_MAGN_Y] = "magn_y",
63 [SENSOR_CHAN_MAGN_Z] = "magn_z",
64 [SENSOR_CHAN_MAGN_XYZ] = "magn_xyz",
65 [SENSOR_CHAN_DIE_TEMP] = "die_temp",
66 [SENSOR_CHAN_AMBIENT_TEMP] = "ambient_temp",
67 [SENSOR_CHAN_PRESS] = "press",
68 [SENSOR_CHAN_PROX] = "prox",
69 [SENSOR_CHAN_HUMIDITY] = "humidity",
70 [SENSOR_CHAN_AMBIENT_LIGHT] = "ambient_light",
71 [SENSOR_CHAN_LIGHT] = "light",
72 [SENSOR_CHAN_IR] = "ir",
73 [SENSOR_CHAN_RED] = "red",
74 [SENSOR_CHAN_GREEN] = "green",
75 [SENSOR_CHAN_BLUE] = "blue",
76 [SENSOR_CHAN_ALTITUDE] = "altitude",
77 [SENSOR_CHAN_PM_1_0] = "pm_1_0",
78 [SENSOR_CHAN_PM_2_5] = "pm_2_5",
79 [SENSOR_CHAN_PM_10] = "pm_10",
80 [SENSOR_CHAN_DISTANCE] = "distance",
81 [SENSOR_CHAN_CO2] = "co2",
82 [SENSOR_CHAN_O2] = "o2",
83 [SENSOR_CHAN_VOC] = "voc",
84 [SENSOR_CHAN_GAS_RES] = "gas_resistance",
85 [SENSOR_CHAN_FLOW_RATE] = "flow_rate",
86 [SENSOR_CHAN_VOLTAGE] = "voltage",
87 [SENSOR_CHAN_VSHUNT] = "vshunt",
88 [SENSOR_CHAN_CURRENT] = "current",
89 [SENSOR_CHAN_POWER] = "power",
90 [SENSOR_CHAN_RESISTANCE] = "resistance",
91 [SENSOR_CHAN_ROTATION] = "rotation",
92 [SENSOR_CHAN_POS_DX] = "pos_dx",
93 [SENSOR_CHAN_POS_DY] = "pos_dy",
94 [SENSOR_CHAN_POS_DZ] = "pos_dz",
95 [SENSOR_CHAN_POS_DXYZ] = "pos_dxyz",
96 [SENSOR_CHAN_RPM] = "rpm",
97 [SENSOR_CHAN_FREQUENCY] = "frequency",
98 [SENSOR_CHAN_GAUGE_VOLTAGE] = "gauge_voltage",
99 [SENSOR_CHAN_GAUGE_AVG_CURRENT] = "gauge_avg_current",
100 [SENSOR_CHAN_GAUGE_STDBY_CURRENT] = "gauge_stdby_current",
101 [SENSOR_CHAN_GAUGE_MAX_LOAD_CURRENT] = "gauge_max_load_current",
102 [SENSOR_CHAN_GAUGE_TEMP] = "gauge_temp",
103 [SENSOR_CHAN_GAUGE_STATE_OF_CHARGE] = "gauge_state_of_charge",
104 [SENSOR_CHAN_GAUGE_FULL_CHARGE_CAPACITY] = "gauge_full_cap",
105 [SENSOR_CHAN_GAUGE_REMAINING_CHARGE_CAPACITY] = "gauge_remaining_cap",
106 [SENSOR_CHAN_GAUGE_NOM_AVAIL_CAPACITY] = "gauge_nominal_cap",
107 [SENSOR_CHAN_GAUGE_FULL_AVAIL_CAPACITY] = "gauge_full_avail_cap",
108 [SENSOR_CHAN_GAUGE_AVG_POWER] = "gauge_avg_power",
109 [SENSOR_CHAN_GAUGE_STATE_OF_HEALTH] = "gauge_state_of_health",
110 [SENSOR_CHAN_GAUGE_TIME_TO_EMPTY] = "gauge_time_to_empty",
111 [SENSOR_CHAN_GAUGE_TIME_TO_FULL] = "gauge_time_to_full",
112 [SENSOR_CHAN_GAUGE_CYCLE_COUNT] = "gauge_cycle_count",
113 [SENSOR_CHAN_GAUGE_DESIGN_VOLTAGE] = "gauge_design_voltage",
114 [SENSOR_CHAN_GAUGE_DESIRED_VOLTAGE] = "gauge_desired_voltage",
115 [SENSOR_CHAN_GAUGE_DESIRED_CHARGING_CURRENT] = "gauge_desired_charging_current",
116 [SENSOR_CHAN_GAME_ROTATION_VECTOR] = "game_rotation_vector",
117 [SENSOR_CHAN_GRAVITY_VECTOR] = "gravity_vector",
118 [SENSOR_CHAN_GBIAS_XYZ] = "gbias_xyz",
119 [SENSOR_CHAN_ENCODER_COUNT] = "encoder_count",
120 [SENSOR_CHAN_ALL] = "all",
121 };
122
123 static const char *const sensor_attribute_name[SENSOR_ATTR_COMMON_COUNT] = {
124 [SENSOR_ATTR_SAMPLING_FREQUENCY] = "sampling_frequency",
125 [SENSOR_ATTR_LOWER_THRESH] = "lower_thresh",
126 [SENSOR_ATTR_UPPER_THRESH] = "upper_thresh",
127 [SENSOR_ATTR_SLOPE_TH] = "slope_th",
128 [SENSOR_ATTR_SLOPE_DUR] = "slope_dur",
129 [SENSOR_ATTR_HYSTERESIS] = "hysteresis",
130 [SENSOR_ATTR_OVERSAMPLING] = "oversampling",
131 [SENSOR_ATTR_FULL_SCALE] = "full_scale",
132 [SENSOR_ATTR_OFFSET] = "offset",
133 [SENSOR_ATTR_CALIB_TARGET] = "calib_target",
134 [SENSOR_ATTR_CONFIGURATION] = "configuration",
135 [SENSOR_ATTR_CALIBRATION] = "calibration",
136 [SENSOR_ATTR_FEATURE_MASK] = "feature_mask",
137 [SENSOR_ATTR_ALERT] = "alert",
138 [SENSOR_ATTR_FF_DUR] = "ff_dur",
139 [SENSOR_ATTR_BATCH_DURATION] = "batch_dur",
140 [SENSOR_ATTR_GAIN] = "gain",
141 [SENSOR_ATTR_RESOLUTION] = "resolution",
142 };
143
144 enum sample_stats_state {
145 SAMPLE_STATS_STATE_UNINITIALIZED = 0,
146 SAMPLE_STATS_STATE_ENABLED,
147 SAMPLE_STATS_STATE_DISABLED,
148 };
149
150 struct sample_stats {
151 int64_t accumulator;
152 uint64_t sample_window_start;
153 uint32_t count;
154 enum sample_stats_state state;
155 };
156
157 static struct sample_stats sensor_stats[CONFIG_SENSOR_SHELL_MAX_TRIGGER_DEVICES][SENSOR_CHAN_ALL];
158
159 static const struct device *sensor_trigger_devices[CONFIG_SENSOR_SHELL_MAX_TRIGGER_DEVICES];
160
device_is_sensor(const struct device * dev)161 static bool device_is_sensor(const struct device *dev)
162 {
163 #ifdef CONFIG_SENSOR_INFO
164 STRUCT_SECTION_FOREACH(sensor_info, sensor) {
165 if (sensor->dev == dev) {
166 return true;
167 }
168 }
169 return false;
170 #else
171 return true;
172 #endif /* CONFIG_SENSOR_INFO */
173 }
174
find_sensor_trigger_device(const struct device * sensor)175 static int find_sensor_trigger_device(const struct device *sensor)
176 {
177 for (int i = 0; i < CONFIG_SENSOR_SHELL_MAX_TRIGGER_DEVICES; i++) {
178 if (sensor_trigger_devices[i] == sensor) {
179 return i;
180 }
181 }
182 return -1;
183 }
184
sensor_device_check(const struct device * dev)185 static bool sensor_device_check(const struct device *dev)
186 {
187 return DEVICE_API_IS(sensor, dev);
188 }
189
190 /* Forward declaration */
191 static void data_ready_trigger_handler(const struct device *sensor,
192 const struct sensor_trigger *trigger);
193
194 #define TRIGGER_DATA_ENTRY(trig_enum, str_name, handler_func) \
195 [(trig_enum)] = {.name = #str_name, \
196 .handler = (handler_func), \
197 .trigger = {.chan = SENSOR_CHAN_ALL, .type = (trig_enum)}}
198
199 /**
200 * @brief This table stores a mapping of string trigger names along with the sensor_trigger struct
201 * that gets passed to the driver to enable that trigger, plus a function pointer to a handler. If
202 * that pointer is NULL, this indicates there is not currently support for that trigger type in the
203 * sensor shell.
204 */
205 static const struct {
206 const char *name;
207 sensor_trigger_handler_t handler;
208 struct sensor_trigger trigger;
209 } sensor_trigger_table[SENSOR_TRIG_COMMON_COUNT] = {
210 TRIGGER_DATA_ENTRY(SENSOR_TRIG_TIMER, timer, NULL),
211 TRIGGER_DATA_ENTRY(SENSOR_TRIG_DATA_READY, data_ready, data_ready_trigger_handler),
212 TRIGGER_DATA_ENTRY(SENSOR_TRIG_DELTA, delta, NULL),
213 TRIGGER_DATA_ENTRY(SENSOR_TRIG_NEAR_FAR, near_far, NULL),
214 TRIGGER_DATA_ENTRY(SENSOR_TRIG_THRESHOLD, threshold, NULL),
215 TRIGGER_DATA_ENTRY(SENSOR_TRIG_TAP, tap, NULL),
216 TRIGGER_DATA_ENTRY(SENSOR_TRIG_DOUBLE_TAP, double_tap, NULL),
217 TRIGGER_DATA_ENTRY(SENSOR_TRIG_FREEFALL, freefall, NULL),
218 TRIGGER_DATA_ENTRY(SENSOR_TRIG_MOTION, motion, NULL),
219 TRIGGER_DATA_ENTRY(SENSOR_TRIG_STATIONARY, stationary, NULL),
220 TRIGGER_DATA_ENTRY(SENSOR_TRIG_FIFO_WATERMARK, fifo_wm, NULL),
221 TRIGGER_DATA_ENTRY(SENSOR_TRIG_FIFO_FULL, fifo_full, NULL),
222 TRIGGER_DATA_ENTRY(SENSOR_TRIG_TILT, tilt, NULL),
223 TRIGGER_DATA_ENTRY(SENSOR_TRIG_OVERFLOW, overflow, NULL),
224 };
225
226 /**
227 * Lookup the sensor trigger data by name
228 *
229 * @param name The name of the trigger
230 * @return < 0 on error
231 * @return >= 0 if found
232 */
sensor_trigger_name_lookup(const char * name)233 static int sensor_trigger_name_lookup(const char *name)
234 {
235 for (int i = 0; i < ARRAY_SIZE(sensor_trigger_table); ++i) {
236 if (strcmp(name, sensor_trigger_table[i].name) == 0) {
237 return i;
238 }
239 }
240 return -1;
241 }
242
243 enum dynamic_command_context {
244 NONE,
245 CTX_GET,
246 CTX_ATTR_GET_SET,
247 CTX_STREAM_ON_OFF,
248 };
249
250 static enum dynamic_command_context current_cmd_ctx = NONE;
251
252 /* Mutex for accessing shared RTIO/IODEV data structures */
253 K_MUTEX_DEFINE(cmd_get_mutex);
254
255 /* Create a single common config for one-shot reading */
256 static struct sensor_chan_spec iodev_sensor_shell_channels[SENSOR_CHAN_ALL];
257 static struct sensor_read_config iodev_sensor_shell_read_config = {
258 .sensor = NULL,
259 .is_streaming = false,
260 .channels = iodev_sensor_shell_channels,
261 .count = 0,
262 .max = ARRAY_SIZE(iodev_sensor_shell_channels),
263 };
264 RTIO_IODEV_DEFINE(iodev_sensor_shell_read, &__sensor_iodev_api, &iodev_sensor_shell_read_config);
265
266 /* Create the RTIO context to service the reading */
267 RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, 8, 8, 32, 64, 4);
268
parse_named_int(const char * name,const char * const heystack[],size_t count)269 static int parse_named_int(const char *name, const char *const heystack[], size_t count)
270 {
271 char *endptr;
272 int i;
273
274 /* Attempt to parse channel name as a number first */
275 i = strtoul(name, &endptr, 0);
276
277 if (*endptr == '\0') {
278 return i;
279 }
280
281 /* Channel name is not a number, look it up */
282 for (i = 0; i < count; i++) {
283 if (strcmp(name, heystack[i]) == 0) {
284 return i;
285 }
286 }
287
288 return -ENOTSUP;
289 }
290
parse_sensor_value(const char * val_str,struct sensor_value * out)291 static int parse_sensor_value(const char *val_str, struct sensor_value *out)
292 {
293 const bool is_negative = val_str[0] == '-';
294 const char *decimal_pos = strchr(val_str, '.');
295 int64_t value;
296 char *endptr;
297
298 /* Parse int portion */
299 value = strtol(val_str, &endptr, 0);
300
301 if (*endptr != '\0' && *endptr != '.') {
302 return -EINVAL;
303 }
304 if (value > INT32_MAX || value < INT32_MIN) {
305 return -EINVAL;
306 }
307 out->val1 = (int32_t)value;
308
309 if (decimal_pos == NULL) {
310 return 0;
311 }
312
313 /* Parse the decimal portion */
314 value = strtoul(decimal_pos + 1, &endptr, 0);
315 if (*endptr != '\0') {
316 return -EINVAL;
317 }
318 while (value < 100000) {
319 value *= 10;
320 }
321 if (value > INT32_C(999999)) {
322 return -EINVAL;
323 }
324 out->val2 = (int32_t)value;
325 if (is_negative) {
326 out->val2 *= -1;
327 }
328 return 0;
329 }
330
sensor_shell_processing_callback(int result,uint8_t * buf,uint32_t buf_len,void * userdata)331 void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t buf_len,
332 void *userdata)
333 {
334 struct sensor_shell_processing_context *ctx = userdata;
335 const struct sensor_decoder_api *decoder;
336 uint8_t decoded_buffer[128];
337 struct {
338 uint64_t base_timestamp_ns;
339 int count;
340 uint64_t timestamp_delta;
341 int64_t values[3];
342 int8_t shift;
343 } accumulator_buffer;
344 int rc;
345
346 ARG_UNUSED(buf_len);
347
348 if (result < 0) {
349 shell_error(ctx->sh, "Read failed");
350 return;
351 }
352
353 rc = sensor_get_decoder(ctx->dev, &decoder);
354 if (rc != 0) {
355 shell_error(ctx->sh, "Failed to get decoder for '%s'", ctx->dev->name);
356 return;
357 }
358
359 for (int trigger = 0; decoder->has_trigger != NULL && trigger < SENSOR_TRIG_COMMON_COUNT;
360 ++trigger) {
361 if (!decoder->has_trigger(buf, trigger)) {
362 continue;
363 }
364 shell_info(ctx->sh, "Trigger (%d / %s) detected", trigger,
365 (sensor_trigger_table[trigger].name == NULL
366 ? "UNKNOWN"
367 : sensor_trigger_table[trigger].name));
368 }
369
370
371
372 for (struct sensor_chan_spec ch = {0, 0}; ch.chan_type < SENSOR_CHAN_ALL; ch.chan_type++) {
373 uint32_t fit = 0;
374 size_t base_size;
375 size_t frame_size;
376 uint16_t frame_count;
377
378 rc = decoder->get_size_info(ch, &base_size, &frame_size);
379 if (rc != 0) {
380 LOG_DBG("skipping unsupported channel %s:%d",
381 sensor_channel_name[ch.chan_type], ch.chan_idx);
382 /* Channel not supported, skipping */
383 continue;
384 }
385
386 if (base_size > ARRAY_SIZE(decoded_buffer)) {
387 shell_error(ctx->sh,
388 "Channel (type %d, idx %d) requires %zu bytes to decode, but "
389 "only %zu are available",
390 ch.chan_type, ch.chan_idx, base_size,
391 ARRAY_SIZE(decoded_buffer));
392 continue;
393 }
394
395 while (decoder->get_frame_count(buf, ch, &frame_count) == 0) {
396 LOG_DBG("decoding %d frames from channel %s:%d",
397 frame_count, sensor_channel_name[ch.chan_type], ch.chan_idx);
398 fit = 0;
399 memset(&accumulator_buffer, 0, sizeof(accumulator_buffer));
400 while (decoder->decode(buf, ch, &fit, 1, decoded_buffer) > 0) {
401 switch (ch.chan_type) {
402 case SENSOR_CHAN_ACCEL_XYZ:
403 case SENSOR_CHAN_GYRO_XYZ:
404 case SENSOR_CHAN_MAGN_XYZ:
405 case SENSOR_CHAN_POS_DXYZ: {
406 struct sensor_three_axis_data *data =
407 (struct sensor_three_axis_data *)decoded_buffer;
408
409 if (accumulator_buffer.count == 0) {
410 accumulator_buffer.base_timestamp_ns =
411 data->header.base_timestamp_ns;
412 }
413 accumulator_buffer.count++;
414 accumulator_buffer.shift = data->shift;
415 accumulator_buffer.timestamp_delta +=
416 data->readings[0].timestamp_delta;
417 accumulator_buffer.values[0] += data->readings[0].values[0];
418 accumulator_buffer.values[1] += data->readings[0].values[1];
419 accumulator_buffer.values[2] += data->readings[0].values[2];
420 break;
421 }
422 case SENSOR_CHAN_PROX: {
423 struct sensor_byte_data *data =
424 (struct sensor_byte_data *)decoded_buffer;
425
426 if (accumulator_buffer.count == 0) {
427 accumulator_buffer.base_timestamp_ns =
428 data->header.base_timestamp_ns;
429 }
430 accumulator_buffer.count++;
431 accumulator_buffer.timestamp_delta +=
432 data->readings[0].timestamp_delta;
433 accumulator_buffer.values[0] += data->readings[0].is_near;
434 break;
435 }
436 default: {
437 struct sensor_q31_data *data =
438 (struct sensor_q31_data *)decoded_buffer;
439
440 if (accumulator_buffer.count == 0) {
441 accumulator_buffer.base_timestamp_ns =
442 data->header.base_timestamp_ns;
443 }
444 accumulator_buffer.count++;
445 accumulator_buffer.shift = data->shift;
446 accumulator_buffer.timestamp_delta +=
447 data->readings[0].timestamp_delta;
448 accumulator_buffer.values[0] += data->readings[0].value;
449 break;
450 }
451 }
452 }
453
454 /* Print the accumulated value average */
455 switch (ch.chan_type) {
456 case SENSOR_CHAN_ACCEL_XYZ:
457 case SENSOR_CHAN_GYRO_XYZ:
458 case SENSOR_CHAN_MAGN_XYZ:
459 case SENSOR_CHAN_POS_DXYZ: {
460 struct sensor_three_axis_data *data =
461 (struct sensor_three_axis_data *)decoded_buffer;
462
463 data->header.base_timestamp_ns =
464 accumulator_buffer.base_timestamp_ns;
465 data->header.reading_count = 1;
466 data->shift = accumulator_buffer.shift;
467 data->readings[0].timestamp_delta =
468 (uint32_t)(accumulator_buffer.timestamp_delta /
469 accumulator_buffer.count);
470 data->readings[0].values[0] = (q31_t)(accumulator_buffer.values[0] /
471 accumulator_buffer.count);
472 data->readings[0].values[1] = (q31_t)(accumulator_buffer.values[1] /
473 accumulator_buffer.count);
474 data->readings[0].values[2] = (q31_t)(accumulator_buffer.values[2] /
475 accumulator_buffer.count);
476 shell_info(ctx->sh,
477 "channel type=%d(%s) index=%d shift=%d num_samples=%d "
478 "value=%" PRIsensor_three_axis_data,
479 ch.chan_type, sensor_channel_name[ch.chan_type],
480 ch.chan_idx, data->shift, accumulator_buffer.count,
481 PRIsensor_three_axis_data_arg(*data, 0));
482 break;
483 }
484 case SENSOR_CHAN_PROX: {
485 struct sensor_byte_data *data =
486 (struct sensor_byte_data *)decoded_buffer;
487
488 data->header.base_timestamp_ns =
489 accumulator_buffer.base_timestamp_ns;
490 data->header.reading_count = 1;
491 data->readings[0].timestamp_delta =
492 (uint32_t)(accumulator_buffer.timestamp_delta /
493 accumulator_buffer.count);
494 data->readings[0].is_near =
495 accumulator_buffer.values[0] / accumulator_buffer.count;
496
497 shell_info(ctx->sh,
498 "channel type=%d(%s) index=%d num_samples=%d "
499 "value=%" PRIsensor_byte_data(is_near),
500 ch.chan_type, sensor_channel_name[ch.chan_type],
501 ch.chan_idx, accumulator_buffer.count,
502 PRIsensor_byte_data_arg(*data, 0, is_near));
503 break;
504 }
505 default: {
506 struct sensor_q31_data *data =
507 (struct sensor_q31_data *)decoded_buffer;
508
509 data->header.base_timestamp_ns =
510 accumulator_buffer.base_timestamp_ns;
511 data->header.reading_count = 1;
512 data->shift = accumulator_buffer.shift;
513 data->readings[0].timestamp_delta =
514 (uint32_t)(accumulator_buffer.timestamp_delta /
515 accumulator_buffer.count);
516 data->readings[0].value = (q31_t)(accumulator_buffer.values[0] /
517 accumulator_buffer.count);
518
519 shell_info(ctx->sh,
520 "channel type=%d(%s) index=%d shift=%d num_samples=%d "
521 "value=%" PRIsensor_q31_data,
522 ch.chan_type,
523 (ch.chan_type >= ARRAY_SIZE(sensor_channel_name))
524 ? ""
525 : sensor_channel_name[ch.chan_type],
526 ch.chan_idx,
527 data->shift, accumulator_buffer.count,
528 PRIsensor_q31_data_arg(*data, 0));
529 }
530 }
531 ++ch.chan_idx;
532 }
533 ch.chan_idx = 0;
534 }
535 }
536
cmd_get_sensor(const struct shell * sh,size_t argc,char * argv[])537 static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[])
538 {
539 static struct sensor_shell_processing_context ctx;
540 const struct device *dev;
541 int count = 0;
542 int err;
543
544 err = k_mutex_lock(&cmd_get_mutex, K_NO_WAIT);
545 if (err < 0) {
546 shell_error(sh, "Another sensor reading in progress");
547 return err;
548 }
549
550 dev = shell_device_get_binding(argv[1]);
551 if (dev == NULL || !sensor_device_check(dev)) {
552 shell_error(sh, "Sensor device unknown (%s)", argv[1]);
553 k_mutex_unlock(&cmd_get_mutex);
554 return -ENODEV;
555 }
556
557 if (!device_is_sensor(dev)) {
558 shell_error(sh, "Device is not a sensor (%s)", argv[1]);
559 k_mutex_unlock(&cmd_get_mutex);
560 return -ENODEV;
561 }
562
563 if (argc == 2) {
564 /* read all channel types */
565 for (int i = 0; i < ARRAY_SIZE(iodev_sensor_shell_channels); ++i) {
566 if (SENSOR_CHANNEL_3_AXIS(i)) {
567 continue;
568 }
569 iodev_sensor_shell_channels[count++] = (struct sensor_chan_spec){i, 0};
570 }
571 } else {
572 /* read specific channels */
573 for (int i = 2; i < argc; ++i) {
574 int chan = parse_named_int(argv[i], sensor_channel_name,
575 ARRAY_SIZE(sensor_channel_name));
576
577 if (chan < 0) {
578 shell_error(sh, "Failed to read channel (%s)", argv[i]);
579 continue;
580 }
581 iodev_sensor_shell_channels[count++] =
582 (struct sensor_chan_spec){chan, 0};
583 }
584 }
585
586 if (count == 0) {
587 shell_error(sh, "No channels to read, bailing");
588 k_mutex_unlock(&cmd_get_mutex);
589 return -EINVAL;
590 }
591 iodev_sensor_shell_read_config.sensor = dev;
592 iodev_sensor_shell_read_config.count = count;
593
594 ctx.dev = dev;
595 ctx.sh = sh;
596 err = sensor_read_async_mempool(&iodev_sensor_shell_read, &sensor_read_rtio, &ctx);
597 if (err < 0) {
598 shell_error(sh, "Failed to read sensor: %d", err);
599 }
600 if (!IS_ENABLED(CONFIG_SENSOR_SHELL_STREAM)) {
601 /*
602 * Streaming enables a thread that polls the RTIO context, so if it's enabled, we
603 * don't need a blocking read here.
604 */
605 sensor_processing_with_callback(&sensor_read_rtio,
606 sensor_shell_processing_callback);
607 }
608
609 k_mutex_unlock(&cmd_get_mutex);
610
611 return 0;
612 }
613
cmd_sensor_attr_set(const struct shell * sh,size_t argc,char * argv[])614 static int cmd_sensor_attr_set(const struct shell *sh, size_t argc, char *argv[])
615 {
616 const struct device *dev;
617 int rc;
618
619 dev = shell_device_get_binding(argv[1]);
620 if (dev == NULL || !sensor_device_check(dev)) {
621 shell_error(sh, "Sensor device unknown (%s)", argv[1]);
622 return -ENODEV;
623 }
624
625 if (!device_is_sensor(dev)) {
626 shell_error(sh, "Device is not a sensor (%s)", argv[1]);
627 k_mutex_unlock(&cmd_get_mutex);
628 return -ENODEV;
629 }
630
631 for (size_t i = 2; i < argc; i += 3) {
632 int channel = parse_named_int(argv[i], sensor_channel_name,
633 ARRAY_SIZE(sensor_channel_name));
634 int attr = parse_named_int(argv[i + 1], sensor_attribute_name,
635 ARRAY_SIZE(sensor_attribute_name));
636 struct sensor_value value = {0};
637
638 if (channel < 0) {
639 shell_error(sh, "Channel '%s' unknown", argv[i]);
640 return -EINVAL;
641 }
642 if (attr < 0) {
643 shell_error(sh, "Attribute '%s' unknown", argv[i + 1]);
644 return -EINVAL;
645 }
646 if (parse_sensor_value(argv[i + 2], &value)) {
647 shell_error(sh, "Sensor value '%s' invalid", argv[i + 2]);
648 return -EINVAL;
649 }
650
651 rc = sensor_attr_set(dev, channel, attr, &value);
652 if (rc) {
653 shell_error(sh, "Failed to set channel(%s) attribute(%s): %d",
654 sensor_channel_name[channel], sensor_attribute_name[attr], rc);
655 continue;
656 }
657 shell_info(sh, "%s channel=%s, attr=%s set to value=%s", dev->name,
658 sensor_channel_name[channel], sensor_attribute_name[attr], argv[i + 2]);
659 }
660 return 0;
661 }
662
cmd_sensor_attr_get_handler(const struct shell * sh,const struct device * dev,const char * channel_name,const char * attr_name,bool print_missing_attribute)663 static void cmd_sensor_attr_get_handler(const struct shell *sh, const struct device *dev,
664 const char *channel_name, const char *attr_name,
665 bool print_missing_attribute)
666 {
667 int channel =
668 parse_named_int(channel_name, sensor_channel_name, ARRAY_SIZE(sensor_channel_name));
669 int attr = parse_named_int(attr_name, sensor_attribute_name,
670 ARRAY_SIZE(sensor_attribute_name));
671 struct sensor_value value = {0};
672 int rc;
673
674 if (channel < 0) {
675 shell_error(sh, "Channel '%s' unknown", channel_name);
676 return;
677 }
678 if (attr < 0) {
679 shell_error(sh, "Attribute '%s' unknown", attr_name);
680 return;
681 }
682
683 rc = sensor_attr_get(dev, channel, attr, &value);
684
685 if (rc != 0) {
686 if (rc == -EINVAL && !print_missing_attribute) {
687 return;
688 }
689 shell_error(sh, "Failed to get channel(%s) attribute(%s): %d",
690 sensor_channel_name[channel], sensor_attribute_name[attr], rc);
691 return;
692 }
693
694 shell_info(sh, "%s(channel=%s, attr=%s) value=%.6f", dev->name,
695 sensor_channel_name[channel], sensor_attribute_name[attr],
696 sensor_value_to_double(&value));
697 }
698
cmd_sensor_attr_get(const struct shell * sh,size_t argc,char * argv[])699 static int cmd_sensor_attr_get(const struct shell *sh, size_t argc, char *argv[])
700 {
701 const struct device *dev;
702
703 dev = shell_device_get_binding(argv[1]);
704 if (dev == NULL || !sensor_device_check(dev)) {
705 shell_error(sh, "Sensor device unknown (%s)", argv[1]);
706 return -ENODEV;
707 }
708
709 if (!device_is_sensor(dev)) {
710 shell_error(sh, "Device is not a sensor (%s)", argv[1]);
711 k_mutex_unlock(&cmd_get_mutex);
712 return -ENODEV;
713 }
714
715 if (argc > 2) {
716 for (size_t i = 2; i < argc; i += 2) {
717 cmd_sensor_attr_get_handler(sh, dev, argv[i], argv[i + 1],
718 /*print_missing_attribute=*/true);
719 }
720 } else {
721 for (size_t channel_idx = 0; channel_idx < ARRAY_SIZE(sensor_channel_name);
722 ++channel_idx) {
723 for (size_t attr_idx = 0; attr_idx < ARRAY_SIZE(sensor_attribute_name);
724 ++attr_idx) {
725 cmd_sensor_attr_get_handler(sh, dev,
726 sensor_channel_name[channel_idx],
727 sensor_attribute_name[attr_idx],
728 /*print_missing_attribute=*/false);
729 }
730 }
731 }
732 return 0;
733 }
734
735 static void channel_name_get(size_t idx, struct shell_static_entry *entry);
736 SHELL_DYNAMIC_CMD_CREATE(dsub_channel_name, channel_name_get);
737
738 static void attribute_name_get(size_t idx, struct shell_static_entry *entry);
739 SHELL_DYNAMIC_CMD_CREATE(dsub_attribute_name, attribute_name_get);
740
channel_name_get(size_t idx,struct shell_static_entry * entry)741 static void channel_name_get(size_t idx, struct shell_static_entry *entry)
742 {
743 int cnt = 0;
744
745 entry->syntax = NULL;
746 entry->handler = NULL;
747 entry->help = NULL;
748 if (current_cmd_ctx == CTX_GET) {
749 entry->subcmd = &dsub_channel_name;
750 } else if (current_cmd_ctx == CTX_ATTR_GET_SET) {
751 entry->subcmd = &dsub_attribute_name;
752 } else {
753 entry->subcmd = NULL;
754 }
755
756 for (int i = 0; i < ARRAY_SIZE(sensor_channel_name); i++) {
757 if (sensor_channel_name[i] != NULL) {
758 if (cnt == idx) {
759 entry->syntax = sensor_channel_name[i];
760 break;
761 }
762 cnt++;
763 }
764 }
765 }
766
attribute_name_get(size_t idx,struct shell_static_entry * entry)767 static void attribute_name_get(size_t idx, struct shell_static_entry *entry)
768 {
769 int cnt = 0;
770
771 entry->syntax = NULL;
772 entry->handler = NULL;
773 entry->help = NULL;
774 entry->subcmd = &dsub_channel_name;
775
776 for (int i = 0; i < ARRAY_SIZE(sensor_attribute_name); i++) {
777 if (sensor_attribute_name[i] != NULL) {
778 if (cnt == idx) {
779 entry->syntax = sensor_attribute_name[i];
780 break;
781 }
782 cnt++;
783 }
784 }
785 }
786
787 static void trigger_opt_get_for_stream(size_t idx, struct shell_static_entry *entry);
788 SHELL_DYNAMIC_CMD_CREATE(dsub_trigger_opt_get_for_stream, trigger_opt_get_for_stream);
789
trigger_opt_get_for_stream(size_t idx,struct shell_static_entry * entry)790 static void trigger_opt_get_for_stream(size_t idx, struct shell_static_entry *entry)
791 {
792 entry->syntax = NULL;
793 entry->handler = NULL;
794 entry->help = NULL;
795 entry->subcmd = NULL;
796
797 switch (idx) {
798 case SENSOR_STREAM_DATA_INCLUDE:
799 entry->syntax = "incl";
800 break;
801 case SENSOR_STREAM_DATA_DROP:
802 entry->syntax = "drop";
803 break;
804 case SENSOR_STREAM_DATA_NOP:
805 entry->syntax = "nop";
806 break;
807 }
808 }
809
810 static void trigger_name_get_for_stream(size_t idx, struct shell_static_entry *entry);
811 SHELL_DYNAMIC_CMD_CREATE(dsub_trigger_name_for_stream, trigger_name_get_for_stream);
812
trigger_name_get_for_stream(size_t idx,struct shell_static_entry * entry)813 static void trigger_name_get_for_stream(size_t idx, struct shell_static_entry *entry)
814 {
815 int cnt = 0;
816
817 entry->syntax = NULL;
818 entry->handler = NULL;
819 entry->help = NULL;
820 entry->subcmd = &dsub_trigger_opt_get_for_stream;
821
822 for (int i = 0; i < ARRAY_SIZE(sensor_trigger_table); i++) {
823 if (sensor_trigger_table[i].name != NULL) {
824 if (cnt == idx) {
825 entry->syntax = sensor_trigger_table[i].name;
826 break;
827 }
828 cnt++;
829 }
830 }
831 }
832
stream_on_off(size_t idx,struct shell_static_entry * entry)833 static void stream_on_off(size_t idx, struct shell_static_entry *entry)
834 {
835 entry->syntax = NULL;
836 entry->handler = NULL;
837 entry->help = NULL;
838
839 if (idx == 0) {
840 entry->syntax = "on";
841 entry->subcmd = &dsub_trigger_name_for_stream;
842 } else if (idx == 1) {
843 entry->syntax = "off";
844 entry->subcmd = NULL;
845 }
846 }
847 SHELL_DYNAMIC_CMD_CREATE(dsub_stream_on_off, stream_on_off);
848
849 static void device_name_get(size_t idx, struct shell_static_entry *entry);
850
851 SHELL_DYNAMIC_CMD_CREATE(dsub_device_name, device_name_get);
852
device_name_get(size_t idx,struct shell_static_entry * entry)853 static void device_name_get(size_t idx, struct shell_static_entry *entry)
854 {
855 const struct device *dev = shell_device_filter(idx, sensor_device_check);
856
857 current_cmd_ctx = CTX_GET;
858 entry->syntax = (dev != NULL) ? dev->name : NULL;
859 entry->handler = NULL;
860 entry->help = NULL;
861 entry->subcmd = &dsub_channel_name;
862 }
863
device_name_get_for_attr(size_t idx,struct shell_static_entry * entry)864 static void device_name_get_for_attr(size_t idx, struct shell_static_entry *entry)
865 {
866 const struct device *dev = shell_device_filter(idx, sensor_device_check);
867
868 current_cmd_ctx = CTX_ATTR_GET_SET;
869 entry->syntax = (dev != NULL) ? dev->name : NULL;
870 entry->handler = NULL;
871 entry->help = NULL;
872 entry->subcmd = &dsub_channel_name;
873 }
874 SHELL_DYNAMIC_CMD_CREATE(dsub_device_name_for_attr, device_name_get_for_attr);
875
trigger_name_get(size_t idx,struct shell_static_entry * entry)876 static void trigger_name_get(size_t idx, struct shell_static_entry *entry)
877 {
878 int cnt = 0;
879
880 entry->syntax = NULL;
881 entry->handler = NULL;
882 entry->help = NULL;
883 entry->subcmd = NULL;
884
885 for (int i = 0; i < ARRAY_SIZE(sensor_trigger_table); i++) {
886 if (sensor_trigger_table[i].name != NULL) {
887 if (cnt == idx) {
888 entry->syntax = sensor_trigger_table[i].name;
889 break;
890 }
891 cnt++;
892 }
893 }
894 }
895
896 SHELL_DYNAMIC_CMD_CREATE(dsub_trigger_name, trigger_name_get);
897
trigger_on_off_get(size_t idx,struct shell_static_entry * entry)898 static void trigger_on_off_get(size_t idx, struct shell_static_entry *entry)
899 {
900 entry->handler = NULL;
901 entry->help = NULL;
902 entry->subcmd = &dsub_trigger_name;
903
904 switch (idx) {
905 case 0:
906 entry->syntax = "on";
907 break;
908 case 1:
909 entry->syntax = "off";
910 break;
911 default:
912 entry->syntax = NULL;
913 break;
914 }
915 }
916
917 SHELL_DYNAMIC_CMD_CREATE(dsub_trigger_onoff, trigger_on_off_get);
918
device_name_get_for_trigger(size_t idx,struct shell_static_entry * entry)919 static void device_name_get_for_trigger(size_t idx, struct shell_static_entry *entry)
920 {
921 const struct device *dev = shell_device_filter(idx, sensor_device_check);
922
923 entry->syntax = (dev != NULL) ? dev->name : NULL;
924 entry->handler = NULL;
925 entry->help = NULL;
926 entry->subcmd = &dsub_trigger_onoff;
927 }
928
929 SHELL_DYNAMIC_CMD_CREATE(dsub_trigger, device_name_get_for_trigger);
930
device_name_get_for_stream(size_t idx,struct shell_static_entry * entry)931 static void device_name_get_for_stream(size_t idx, struct shell_static_entry *entry)
932 {
933 const struct device *dev = shell_device_filter(idx, sensor_device_check);
934
935 current_cmd_ctx = CTX_STREAM_ON_OFF;
936 entry->syntax = (dev != NULL) ? dev->name : NULL;
937 entry->handler = NULL;
938 entry->help = NULL;
939 entry->subcmd = &dsub_stream_on_off;
940 }
941 SHELL_DYNAMIC_CMD_CREATE(dsub_device_name_for_stream, device_name_get_for_stream);
942
cmd_get_sensor_info(const struct shell * sh,size_t argc,char ** argv)943 static int cmd_get_sensor_info(const struct shell *sh, size_t argc, char **argv)
944 {
945 ARG_UNUSED(argc);
946 ARG_UNUSED(argv);
947
948 #ifdef CONFIG_SENSOR_INFO
949 const char *null_str = "(null)";
950
951 STRUCT_SECTION_FOREACH(sensor_info, sensor) {
952 shell_print(sh,
953 "device name: %s, vendor: %s, model: %s, "
954 "friendly name: %s",
955 sensor->dev->name, sensor->vendor ? sensor->vendor : null_str,
956 sensor->model ? sensor->model : null_str,
957 sensor->friendly_name ? sensor->friendly_name : null_str);
958 }
959 return 0;
960 #else
961 return -EINVAL;
962 #endif
963 }
964
data_ready_trigger_handler(const struct device * sensor,const struct sensor_trigger * trigger)965 static void data_ready_trigger_handler(const struct device *sensor,
966 const struct sensor_trigger *trigger)
967 {
968 const int64_t now = k_uptime_get();
969 struct sensor_value value;
970 int sensor_idx = find_sensor_trigger_device(sensor);
971 struct sample_stats *stats;
972 int sensor_name_len_before_at;
973 const char *sensor_name;
974
975 if (sensor_idx < 0) {
976 LOG_ERR("Unable to find sensor trigger device");
977 return;
978 }
979 stats = sensor_stats[sensor_idx];
980 sensor_name = sensor_trigger_devices[sensor_idx]->name;
981 if (sensor_name) {
982 sensor_name_len_before_at = strchr(sensor_name, '@') - sensor_name;
983 } else {
984 sensor_name_len_before_at = 0;
985 }
986
987 if (sensor_sample_fetch(sensor)) {
988 LOG_ERR("Failed to fetch samples on data ready handler");
989 }
990 for (int i = 0; i < SENSOR_CHAN_ALL; ++i) {
991 int rc;
992
993 /* Skip disabled channels */
994 if (stats[i].state == SAMPLE_STATS_STATE_DISABLED) {
995 continue;
996 }
997 /* Skip 3 axis channels */
998 if (SENSOR_CHANNEL_3_AXIS(i)) {
999 continue;
1000 }
1001
1002 rc = sensor_channel_get(sensor, i, &value);
1003 if (stats[i].state == SAMPLE_STATS_STATE_UNINITIALIZED) {
1004 if (rc == -ENOTSUP) {
1005 /*
1006 * Stop reading this channel if the driver told us
1007 * it's not supported.
1008 */
1009 stats[i].state = SAMPLE_STATS_STATE_DISABLED;
1010 } else if (rc == 0) {
1011 stats[i].state = SAMPLE_STATS_STATE_ENABLED;
1012 }
1013 }
1014 if (rc != 0) {
1015 /* Skip on any error. */
1016 continue;
1017 }
1018 /* Do something with the data */
1019 stats[i].accumulator += value.val1 * INT64_C(1000000) + value.val2;
1020 if (stats[i].count++ == 0) {
1021 stats[i].sample_window_start = now;
1022 } else if (now > stats[i].sample_window_start +
1023 CONFIG_SENSOR_SHELL_TRIG_PRINT_TIMEOUT_MS) {
1024 int64_t micro_value = stats[i].accumulator / stats[i].count;
1025
1026 value.val1 = micro_value / 1000000;
1027 value.val2 = (int32_t)llabs(micro_value - (value.val1 * 1000000));
1028 LOG_INF("sensor=%.*s, chan=%s, num_samples=%u, data=%d.%06d",
1029 sensor_name_len_before_at, sensor_name,
1030 sensor_channel_name[i],
1031 stats[i].count,
1032 value.val1, value.val2);
1033
1034 stats[i].accumulator = 0;
1035 stats[i].count = 0;
1036 }
1037 }
1038 }
1039
cmd_trig_sensor(const struct shell * sh,size_t argc,char ** argv)1040 static int cmd_trig_sensor(const struct shell *sh, size_t argc, char **argv)
1041 {
1042 const struct device *dev;
1043 int trigger;
1044 bool trigger_enabled = false;
1045 int err;
1046
1047 if (argc < 4) {
1048 shell_error(sh, "Wrong number of args");
1049 return -EINVAL;
1050 }
1051
1052 /* Parse device name */
1053 dev = shell_device_get_binding(argv[1]);
1054 if (dev == NULL || !sensor_device_check(dev)) {
1055 shell_error(sh, "Sensor device unknown (%s)", argv[1]);
1056 return -ENODEV;
1057 }
1058
1059 /* Map the trigger string to an enum value */
1060 trigger = sensor_trigger_name_lookup(argv[3]);
1061 if (trigger < 0 || sensor_trigger_table[trigger].handler == NULL) {
1062 shell_error(sh, "Unsupported trigger type (%s)", argv[3]);
1063 return -ENOTSUP;
1064 }
1065
1066 /* Parse on/off */
1067 if (strcmp(argv[2], "on") == 0) {
1068 /* find a free entry in sensor_trigger_devices[] */
1069 int sensor_idx = find_sensor_trigger_device(NULL);
1070
1071 if (sensor_idx < 0) {
1072 shell_error(sh, "Unable to support more simultaneous sensor trigger"
1073 " devices");
1074 err = -ENOTSUP;
1075 } else {
1076 struct sample_stats *stats = sensor_stats[sensor_idx];
1077
1078 sensor_trigger_devices[sensor_idx] = dev;
1079 /* reset stats state to UNINITIALIZED */
1080 for (unsigned int ch = 0; ch < SENSOR_CHAN_ALL; ch++) {
1081 stats[ch].state = SAMPLE_STATS_STATE_UNINITIALIZED;
1082 }
1083 err = sensor_trigger_set(dev, &sensor_trigger_table[trigger].trigger,
1084 sensor_trigger_table[trigger].handler);
1085 trigger_enabled = true;
1086 }
1087 } else if (strcmp(argv[2], "off") == 0) {
1088 /* Clear the handler for the given trigger on this device */
1089 err = sensor_trigger_set(dev, &sensor_trigger_table[trigger].trigger, NULL);
1090 if (!err) {
1091 /* find entry in sensor_trigger_devices[] and free it */
1092 int sensor_idx = find_sensor_trigger_device(dev);
1093
1094 if (sensor_idx < 0) {
1095 shell_error(sh, "Unable to find sensor device in trigger array");
1096 } else {
1097 sensor_trigger_devices[sensor_idx] = NULL;
1098 }
1099 }
1100 } else {
1101 shell_error(sh, "Pass 'on' or 'off' to enable/disable trigger");
1102 return -EINVAL;
1103 }
1104
1105 if (err) {
1106 shell_error(sh, "Error while setting trigger %d on device %s (%d)", trigger,
1107 argv[1], err);
1108 } else {
1109 shell_info(sh, "%s trigger idx=%d %s on device %s",
1110 trigger_enabled ? "Enabled" : "Disabled", trigger,
1111 sensor_trigger_table[trigger].name, argv[1]);
1112 }
1113
1114 return err;
1115 }
1116
1117 /* clang-format off */
1118 SHELL_STATIC_SUBCMD_SET_CREATE(sub_sensor,
1119 SHELL_CMD_ARG(get, &dsub_device_name, SENSOR_GET_HELP, cmd_get_sensor,
1120 2, 255),
1121 SHELL_CMD_ARG(attr_set, &dsub_device_name_for_attr, SENSOR_ATTR_SET_HELP,
1122 cmd_sensor_attr_set, 2, 255),
1123 SHELL_CMD_ARG(attr_get, &dsub_device_name_for_attr, SENSOR_ATTR_GET_HELP,
1124 cmd_sensor_attr_get, 2, 255),
1125 SHELL_COND_CMD(CONFIG_SENSOR_SHELL_STREAM, stream, &dsub_device_name_for_stream,
1126 SENSOR_STREAM_HELP, cmd_sensor_stream),
1127 SHELL_COND_CMD(CONFIG_SENSOR_INFO, info, NULL, SENSOR_INFO_HELP,
1128 cmd_get_sensor_info),
1129 SHELL_CMD_ARG(trig, &dsub_trigger, SENSOR_TRIG_HELP, cmd_trig_sensor,
1130 2, 255),
1131 SHELL_SUBCMD_SET_END
1132 );
1133 /* clang-format on */
1134
1135 SHELL_CMD_REGISTER(sensor, &sub_sensor, "Sensor commands", NULL);
1136
