| /* |
| * Copyright (c) 2023 Google LLC |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include "icm42688_decoder.h" |
| #include "icm42688_reg.h" |
| #include "icm42688.h" |
| #include <errno.h> |
| |
| #include <zephyr/logging/log.h> |
| LOG_MODULE_REGISTER(ICM42688_DECODER, CONFIG_SENSOR_LOG_LEVEL); |
| |
| #define DT_DRV_COMPAT invensense_icm42688 |
| |
| static int icm42688_get_shift(enum sensor_channel channel, int accel_fs, int gyro_fs, int8_t *shift) |
| { |
| switch (channel) { |
| case SENSOR_CHAN_ACCEL_XYZ: |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| switch (accel_fs) { |
| case ICM42688_ACCEL_FS_2G: |
| *shift = 5; |
| return 0; |
| case ICM42688_ACCEL_FS_4G: |
| *shift = 6; |
| return 0; |
| case ICM42688_ACCEL_FS_8G: |
| *shift = 7; |
| return 0; |
| case ICM42688_ACCEL_FS_16G: |
| *shift = 8; |
| return 0; |
| default: |
| return -EINVAL; |
| } |
| case SENSOR_CHAN_GYRO_XYZ: |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| switch (gyro_fs) { |
| case ICM42688_GYRO_FS_15_625: |
| *shift = -1; |
| return 0; |
| case ICM42688_GYRO_FS_31_25: |
| *shift = 0; |
| return 0; |
| case ICM42688_GYRO_FS_62_5: |
| *shift = 1; |
| return 0; |
| case ICM42688_GYRO_FS_125: |
| *shift = 2; |
| return 0; |
| case ICM42688_GYRO_FS_250: |
| *shift = 3; |
| return 0; |
| case ICM42688_GYRO_FS_500: |
| *shift = 4; |
| return 0; |
| case ICM42688_GYRO_FS_1000: |
| *shift = 5; |
| return 0; |
| case ICM42688_GYRO_FS_2000: |
| *shift = 6; |
| return 0; |
| default: |
| return -EINVAL; |
| } |
| case SENSOR_CHAN_DIE_TEMP: |
| *shift = 9; |
| return 0; |
| default: |
| return -EINVAL; |
| } |
| } |
| |
| int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel chan, int32_t reading, |
| q31_t *out) |
| { |
| int32_t whole; |
| int32_t fraction; |
| int64_t intermediate; |
| int8_t shift; |
| int rc; |
| |
| rc = icm42688_get_shift(chan, cfg->accel_fs, cfg->gyro_fs, &shift); |
| if (rc != 0) { |
| return rc; |
| } |
| |
| switch (chan) { |
| case SENSOR_CHAN_ACCEL_XYZ: |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| icm42688_accel_ms(cfg, reading, &whole, &fraction); |
| break; |
| case SENSOR_CHAN_GYRO_XYZ: |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| icm42688_gyro_rads(cfg, reading, &whole, &fraction); |
| break; |
| case SENSOR_CHAN_DIE_TEMP: |
| icm42688_temp_c(reading, &whole, &fraction); |
| break; |
| default: |
| return -ENOTSUP; |
| } |
| intermediate = ((int64_t)whole * INT64_C(1000000) + fraction); |
| if (shift < 0) { |
| intermediate = |
| intermediate * ((int64_t)INT32_MAX + 1) * (1 << -shift) / INT64_C(1000000); |
| } else if (shift > 0) { |
| intermediate = |
| intermediate * ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000)); |
| } |
| *out = CLAMP(intermediate, INT32_MIN, INT32_MAX); |
| |
| return 0; |
| } |
| |
| static int icm42688_get_channel_position(enum sensor_channel chan) |
| { |
| switch (chan) { |
| case SENSOR_CHAN_DIE_TEMP: |
| return 0; |
| case SENSOR_CHAN_ACCEL_XYZ: |
| case SENSOR_CHAN_ACCEL_X: |
| return 1; |
| case SENSOR_CHAN_ACCEL_Y: |
| return 2; |
| case SENSOR_CHAN_ACCEL_Z: |
| return 3; |
| case SENSOR_CHAN_GYRO_XYZ: |
| case SENSOR_CHAN_GYRO_X: |
| return 4; |
| case SENSOR_CHAN_GYRO_Y: |
| return 5; |
| case SENSOR_CHAN_GYRO_Z: |
| return 6; |
| default: |
| return 0; |
| } |
| } |
| |
| static uint8_t icm42688_encode_channel(enum sensor_channel chan) |
| { |
| uint8_t encode_bmask = 0; |
| |
| switch (chan) { |
| case SENSOR_CHAN_DIE_TEMP: |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| encode_bmask = BIT(icm42688_get_channel_position(chan)); |
| break; |
| case SENSOR_CHAN_ACCEL_XYZ: |
| encode_bmask = BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_X)) | |
| BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Y)) | |
| BIT(icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Z)); |
| break; |
| case SENSOR_CHAN_GYRO_XYZ: |
| encode_bmask = BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_X)) | |
| BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Y)) | |
| BIT(icm42688_get_channel_position(SENSOR_CHAN_GYRO_Z)); |
| break; |
| default: |
| break; |
| } |
| |
| return encode_bmask; |
| } |
| |
| int icm42688_encode(const struct device *dev, const enum sensor_channel *const channels, |
| const size_t num_channels, uint8_t *buf) |
| { |
| struct icm42688_dev_data *data = dev->data; |
| struct icm42688_encoded_data *edata = (struct icm42688_encoded_data *)buf; |
| |
| edata->channels = 0; |
| |
| for (int i = 0; i < num_channels; i++) { |
| edata->channels |= icm42688_encode_channel(channels[i]); |
| } |
| |
| edata->header.is_fifo = false; |
| edata->header.accel_fs = data->cfg.accel_fs; |
| edata->header.gyro_fs = data->cfg.gyro_fs; |
| edata->header.timestamp = k_ticks_to_ns_floor64(k_uptime_ticks()); |
| |
| return 0; |
| } |
| |
| static int icm42688_one_shot_decode(const uint8_t *buffer, enum sensor_channel channel, |
| size_t channel_idx, uint32_t *fit, |
| uint16_t max_count, void *data_out) |
| { |
| const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer; |
| const struct icm42688_decoder_header *header = &edata->header; |
| struct icm42688_cfg cfg = { |
| .accel_fs = edata->header.accel_fs, |
| .gyro_fs = edata->header.gyro_fs, |
| }; |
| uint8_t channel_request; |
| int rc; |
| |
| if (*fit != 0) { |
| return 0; |
| } |
| if (max_count == 0 || channel_idx != 0) { |
| return -EINVAL; |
| } |
| |
| switch (channel) { |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| case SENSOR_CHAN_ACCEL_XYZ: { |
| channel_request = icm42688_encode_channel(SENSOR_CHAN_ACCEL_XYZ); |
| if ((channel_request & edata->channels) != channel_request) { |
| return -ENODATA; |
| } |
| |
| struct sensor_three_axis_data *out = data_out; |
| |
| out->header.base_timestamp_ns = edata->header.timestamp; |
| out->header.reading_count = 1; |
| rc = icm42688_get_shift(SENSOR_CHAN_ACCEL_XYZ, header->accel_fs, header->gyro_fs, |
| &out->shift); |
| if (rc != 0) { |
| return -EINVAL; |
| } |
| |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_ACCEL_X, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_X)], |
| &out->readings[0].x); |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_ACCEL_Y, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Y)], |
| &out->readings[0].y); |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_ACCEL_Z, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_ACCEL_Z)], |
| &out->readings[0].z); |
| *fit = 1; |
| return 1; |
| } |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| case SENSOR_CHAN_GYRO_XYZ: { |
| channel_request = icm42688_encode_channel(SENSOR_CHAN_GYRO_XYZ); |
| if ((channel_request & edata->channels) != channel_request) { |
| return -ENODATA; |
| } |
| |
| struct sensor_three_axis_data *out = data_out; |
| |
| out->header.base_timestamp_ns = edata->header.timestamp; |
| out->header.reading_count = 1; |
| rc = icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, header->accel_fs, header->gyro_fs, |
| &out->shift); |
| if (rc != 0) { |
| return -EINVAL; |
| } |
| |
| out->readings[0].timestamp_delta = 0; |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_GYRO_X, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_X)], |
| &out->readings[0].x); |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_GYRO_Y, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_Y)], |
| &out->readings[0].y); |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_GYRO_Z, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_GYRO_Z)], |
| &out->readings[0].z); |
| *fit = 1; |
| return 1; |
| } |
| case SENSOR_CHAN_DIE_TEMP: { |
| channel_request = icm42688_encode_channel(SENSOR_CHAN_DIE_TEMP); |
| if ((channel_request & edata->channels) != channel_request) { |
| return -ENODATA; |
| } |
| |
| struct sensor_q31_data *out = data_out; |
| |
| out->header.base_timestamp_ns = edata->header.timestamp; |
| out->header.reading_count = 1; |
| |
| rc = icm42688_get_shift(SENSOR_CHAN_DIE_TEMP, header->accel_fs, header->gyro_fs, |
| &out->shift); |
| if (rc != 0) { |
| return -EINVAL; |
| } |
| out->readings[0].timestamp_delta = 0; |
| icm42688_convert_raw_to_q31( |
| &cfg, SENSOR_CHAN_DIE_TEMP, |
| edata->readings[icm42688_get_channel_position(SENSOR_CHAN_DIE_TEMP)], |
| &out->readings[0].temperature); |
| *fit = 1; |
| return 1; |
| } |
| default: |
| return -EINVAL; |
| } |
| } |
| |
| static int icm42688_decoder_decode(const uint8_t *buffer, enum sensor_channel channel, |
| size_t channel_idx, uint32_t *fit, |
| uint16_t max_count, void *data_out) |
| { |
| return icm42688_one_shot_decode(buffer, channel, channel_idx, fit, max_count, data_out); |
| } |
| |
| static int icm42688_decoder_get_frame_count(const uint8_t *buffer, enum sensor_channel channel, |
| size_t channel_idx, uint16_t *frame_count) |
| { |
| ARG_UNUSED(buffer); |
| if (channel_idx != 0) { |
| return -ENOTSUP; |
| } |
| switch (channel) { |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| case SENSOR_CHAN_ACCEL_XYZ: |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| case SENSOR_CHAN_GYRO_XYZ: |
| case SENSOR_CHAN_DIE_TEMP: |
| *frame_count = 1; |
| return 0; |
| default: |
| return -ENOTSUP; |
| } |
| } |
| |
| static int icm42688_decoder_get_size_info(enum sensor_channel channel, size_t *base_size, |
| size_t *frame_size) |
| { |
| switch (channel) { |
| case SENSOR_CHAN_ACCEL_X: |
| case SENSOR_CHAN_ACCEL_Y: |
| case SENSOR_CHAN_ACCEL_Z: |
| case SENSOR_CHAN_ACCEL_XYZ: |
| case SENSOR_CHAN_GYRO_X: |
| case SENSOR_CHAN_GYRO_Y: |
| case SENSOR_CHAN_GYRO_Z: |
| case SENSOR_CHAN_GYRO_XYZ: |
| *base_size = sizeof(struct sensor_three_axis_data); |
| *frame_size = sizeof(struct sensor_three_axis_sample_data); |
| return 0; |
| case SENSOR_CHAN_DIE_TEMP: |
| *base_size = sizeof(struct sensor_q31_data); |
| *frame_size = sizeof(struct sensor_q31_sample_data); |
| return 0; |
| default: |
| return -ENOTSUP; |
| } |
| } |
| |
| SENSOR_DECODER_API_DT_DEFINE() = { |
| .get_frame_count = icm42688_decoder_get_frame_count, |
| .get_size_info = icm42688_decoder_get_size_info, |
| .decode = icm42688_decoder_decode, |
| }; |
| |
| int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder) |
| { |
| ARG_UNUSED(dev); |
| *decoder = &SENSOR_DECODER_NAME(); |
| |
| return 0; |
| } |