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