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