From bd1d68306fc20f2341f1196be5aeae027e542001 Mon Sep 17 00:00:00 2001 From: Pedro Machado Date: Sat, 23 Sep 2023 16:01:59 +0100 Subject: [PATCH] drivers: sensors: bmi270: Add CRT feature module Fixes: #62514 Adds support for the bmi270 feature: Creates a specific kconfig for the feature. Adds a method to run the crt process. Adds a method to program the bmi270 nvm. Adds a method to enable/disable gain compensation. Signed-off-by: Pedro Machado --- drivers/sensor/bmi270/CMakeLists.txt | 1 + drivers/sensor/bmi270/Kconfig | 6 + drivers/sensor/bmi270/bmi270.c | 5 + drivers/sensor/bmi270/bmi270_crt.c | 743 +++++++++++++++++++++++++++ 4 files changed, 755 insertions(+) create mode 100644 drivers/sensor/bmi270/bmi270_crt.c diff --git a/drivers/sensor/bmi270/CMakeLists.txt b/drivers/sensor/bmi270/CMakeLists.txt index b6d64f0dc5ab06a..7b83659416d877a 100644 --- a/drivers/sensor/bmi270/CMakeLists.txt +++ b/drivers/sensor/bmi270/CMakeLists.txt @@ -11,3 +11,4 @@ zephyr_library_sources(bmi270.c) zephyr_library_sources_ifdef(CONFIG_BMI270_BUS_I2C bmi270_i2c.c) zephyr_library_sources_ifdef(CONFIG_BMI270_BUS_SPI bmi270_spi.c) zephyr_library_sources_ifdef(CONFIG_BMI270_TRIGGER bmi270_trigger.c) +zephyr_library_sources_ifdef(CONFIG_BMI270_CRT bmi270_crt.c) diff --git a/drivers/sensor/bmi270/Kconfig b/drivers/sensor/bmi270/Kconfig index 89ea6fd4503a175..25b3dbd223f1e3b 100644 --- a/drivers/sensor/bmi270/Kconfig +++ b/drivers/sensor/bmi270/Kconfig @@ -62,4 +62,10 @@ config BMI270_THREAD_STACK_SIZE help Stack size of thread used by the driver to handle interrupts. +config BMI270_CRT + bool "CRT feature support for bmi270" + default n + help + Gyro CRT calibration feature support for bmi270 + endif # BMI270 diff --git a/drivers/sensor/bmi270/bmi270.c b/drivers/sensor/bmi270/bmi270.c index d6ba0542765ad46..d80b42a51fa8e92 100644 --- a/drivers/sensor/bmi270/bmi270.c +++ b/drivers/sensor/bmi270/bmi270.c @@ -802,6 +802,11 @@ static const struct bmi270_feature_config bmi270_feature_base = { .config_file_len = sizeof(bmi270_config_file_base), .anymo_1 = &(struct bmi270_feature_reg){ .page = 1, .addr = 0x3C }, .anymo_2 = &(struct bmi270_feature_reg){ .page = 1, .addr = 0x3E }, +#if defined(CONFIG_BMI270_CRT) + .g_trig_1 = &(struct bmi270_feature_reg){.page = 1, .addr = 0x32}, + .gyr_gain_status = &(struct bmi270_feature_reg){.page = 0, .addr = 0x38}, + .gen_set_1 = &(struct bmi270_feature_reg){.page = 1, .addr = 0x34}, +#endif }; #define BMI270_FEATURE(inst) ( \ diff --git a/drivers/sensor/bmi270/bmi270_crt.c b/drivers/sensor/bmi270/bmi270_crt.c new file mode 100644 index 000000000000000..a973778b42912e9 --- /dev/null +++ b/drivers/sensor/bmi270/bmi270_crt.c @@ -0,0 +1,743 @@ +/* + * Copyright (c) 2023 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include "bmi270.h" +#include "bmi270_config_file.h" +LOG_MODULE_DECLARE(bmi270, CONFIG_SENSOR_LOG_LEVEL); + +#define BMI270_REG_FEATURES_0_END 0x3F +#define BMI270_INTER_WRITE_DELAY_US 1000 + +#define BMI270_CRT_STATUS_CHECK_RETRIES 15 +#define BMI270_NVM_STATUS_CHECK_RETRIES 100 +#define BMI270_STATUS_CHECK_POLL_PERIOD_US 10000 + +/** @name Enum to define status of gyroscope trigger G_TRIGGER */ +typedef enum { + SUCCESS = 0x00, /*Command is valid*/ + PRECON_ERR = 0x01, /*Command is aborted due to incomplete pre-conditions*/ + DL_ERR = 0x02, /*Command is aborted due to unsuccessful download*/ + ABORT_ERR = 0x03 /*Command is aborted by host or due to motion detection*/ +} GTriggerStatus; + +/** @name Structure to define gyroscope saturation status of user gain + * Describes the saturation status for the gyroscope gain + * update and G_TRIGGER command status + */ +struct bmi2_gyr_user_gain_status { + /* Status in x-axis + * This bit will be 1 if the updated gain + * results to saturated value based on the + * ratio provided for x axis, otherwise it will be 0 + */ + uint8_t sat_x; + + /* Status in y-axis + * This bit will be 1 if the updated gain + * results to saturated value based on the + * ratio provided for y axis, otherwise it will be 0 + */ + uint8_t sat_y; + + /* Status in z-axis + * This bit will be 1 if the updated gain + * results to saturated value based on the + * ratio provided for z axis, otherwise it will be 0 + */ + uint8_t sat_z; + + /* G trigger status + * Status of gyroscope trigger G_TRIGGER command. + * These bits are updated at the end of feature execution . + */ + GTriggerStatus g_trigger_status; +}; + +/** @name Structure to store the compensated user-gain data of gyroscope */ +struct bmi2_gyro_user_gain_data { + /** x-axis */ + int8_t x; + + /** y-axis */ + int8_t y; + + /** z-axis */ + int8_t z; +}; + +/* Global structures to store the CRT test results */ +struct bmi2_gyr_user_gain_status crt_result_sts = {0}; +struct bmi2_gyro_user_gain_data crt_gain = {0}; + +/** + * @brief This function verifies if advanced power saving mode is enabled + * If it enabled, it disables it. + * + * @param[in] status : status to set advanced power saving mode + * @param[in] dev : Structure instance of bmi270 device + * + * @return Result of disable aps execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int bmi270_set_aps(const uint8_t status, const struct device *dev) +{ + uint8_t adv_pwr_save; + int ret; + /* Get status of advance power save mode */ + ret = bmi270_reg_read(dev, BMI270_REG_PWR_CONF, &adv_pwr_save, 1); + if (ret != 0) { + LOG_ERR("Read power config register failed w/ error: %d", ret); + return ret; + } + + /* Check if apsm is already the one being set */ + if ((adv_pwr_save & BMI270_PWR_CONF_ADV_PWR_SAVE_MSK) != status) { + /* Change advance power save mode if needed */ + adv_pwr_save = + BMI270_SET_BITS_POS_0(adv_pwr_save, BMI270_PWR_CONF_ADV_PWR_SAVE, status); + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_PWR_CONF, &adv_pwr_save, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to disable advance power save, err: %d", ret); + return ret; + } + LOG_DBG("advance power save mode set to: %d", status); + } else { + LOG_DBG("advance power save mode already in the intended state"); + } + return 0; +} + +/** + * @brief Helper function to check if a feature address respects addressing rules + * + * @param[in] addr : feature addr, in the respective page + * + * @note Writes to a FEATURES register must be 16-bit word oriented, + * i.e. writes should start at an even address (2m) and the last + * byte written should be at an odd address (2n+1), where 0x30<=2m<=2n<0x3F. + * + * @return Result of feature rmw execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int bmi270_feature_addr_check(uint8_t addr) +{ + /* Check that the start address is even */ + if (addr % 2 != 0) { + LOG_ERR("Start address must be even: Address 0x%x", addr); + return -EINVAL; + } + + /* Check that the start address is greater than 0x30 */ + if (addr < (uint8_t)BMI270_REG_FEATURES_0) { + LOG_ERR("Feature start address must be greater than 0x30: Address 0x%02x", addr); + return -EINVAL; + } + + /* Check that end address is less than 0x3F */ + if (addr + 0x01 > (uint8_t)BMI270_REG_FEATURES_0_END) { + LOG_ERR("End address must be less than 0x3F: Address0x%02x", addr + 1); + return -EINVAL; + } + return 0; +} + +/** + * @brief This function is used to read-modify-write a feature + * + * @param[in] dev : Structure instance of bmi270 device + * @param[in] feature : Pointer to the feature + * @param[in] mask : feature mask + * @param[in] pos : feature starting position bit + * @param[in] value : new feature value to be written + * + * @return Result of feature rmw execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int bmi270_feature_reg_rmw(const struct device *dev, + const struct bmi270_feature_reg *feature, uint16_t mask, + uint8_t pos, uint8_t value) +{ + int ret; + uint8_t feat_page = feature->page; + uint16_t feature_value; + + /* Check if feature address respects addressing rules */ + ret = bmi270_feature_addr_check(feature->addr); + if (ret < 0) { + LOG_ERR("Feature addr check failed, addr:%d at page %d, err:%d", feature->addr, + feat_page, ret); + return ret; + } + + /* Disable advanced power save mode */ + ret = bmi270_set_aps(BMI270_PWR_CONF_ADV_PWR_SAVE_DIS, dev); + if (ret != 0) { + LOG_ERR("Failed bmi270_set_aps, err %d", ret); + return ret; + } + + /* Select feature page */ + ret = bmi270_reg_write(dev, BMI270_REG_FEAT_PAGE, &feat_page, 1); + if (ret < 0) { + LOG_ERR("bmi270_reg_write (0x%02x) failed: %d", BMI270_REG_FEAT_PAGE, ret); + return ret; + } + + /* Read feature, 16-bit word oriented */ + ret = bmi270_reg_read(dev, feature->addr, (uint8_t *)&feature_value, 2); + if (ret < 0) { + LOG_ERR("bmi270_reg_read (0x%02x) failed: %d", feature->addr, ret); + return ret; + } + LOG_DBG("Read feature reg[0x%02x]@%d = 0x%04x", feature->addr, feature->page, + feature_value); + + /* Modify feature Value */ + feature_value = ((feature_value & ~(mask)) | ((value << pos) & mask)); + + /* Write feature, 16-bit word oriented */ + ret = bmi270_reg_write(dev, feature->addr, (uint8_t *)&feature_value, 2); + if (ret < 0) { + LOG_ERR("bmi270_reg_write (0x%02x) failed: %d", feature->addr, ret); + return ret; + } + LOG_DBG("Wrote feature reg[0x%02x]@%d = 0x%04x", feature->addr, feature->page, + feature_value); + + return 0; +} + +/** + * @brief This function is used to disable block feature. + * + * @param[in] dev : Structure instance of bmi270 device + * + * @return Result of disable block feature execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static inline int bmi270_g_trig_1_block_unblock(const struct device *dev) +{ + const struct bmi270_config *cfg = dev->config; + int ret = bmi270_feature_reg_rmw(dev, cfg->feature->g_trig_1, BMI270_G_TRIG_1_BLOCK_MASK, + BMI270_G_TRIG_1_BLOCK_POS, BMI270_G_TRIG_1_BLOCK_UNBLOCK); + return ret; +} + +/** + * @brief This function is used to enable the CRT. + * + * @param[in] dev : Structure instance of bmi270 device + * + * @return Result of select crt enable execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static inline int bmi270_g_trig_1_select_crt(const struct device *dev) +{ + const struct bmi270_config *cfg = dev->config; + int ret = bmi270_feature_reg_rmw(dev, cfg->feature->g_trig_1, BMI270_G_TRIG_1_SELECT_MASK, + BMI270_G_TRIG_1_SELECT_POS, BMI270_G_TRIG_1_SELECT_CRT); + return ret; +} + +/** @brief Helper method to LOG CRT cmd status */ +static void crt_error_log(GTriggerStatus status) +{ + switch (status) { + case SUCCESS: + LOG_INF("CRT was successful!\n"); + break; + case PRECON_ERR: + LOG_ERR("LOG_ERR: Pre-condition error, command is aborted.\n"); + break; + case DL_ERR: + LOG_ERR("LOG_ERR: Download error, command is aborted.\n"); + break; + case ABORT_ERR: + LOG_ERR("LOG_ERR: Command aborted by host or due to motion detection.\n"); + break; + default: + LOG_ERR("LOG_ERR: Unknown error code.\n"); + break; + } +} + +/** + * @brief This internal function gets the saturation status for the gyroscope user + * gain update. + * + * @param[in] dev : Structure instance of bmi270 device + * @param[out] bmi2_gyr_user_gain_status : Structure that stores gyro gain + * CRT test results + * + * @return Result of select crt enable execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int bmi270_get_gyro_gain_update_status(const struct device *dev, + struct bmi2_gyr_user_gain_status *result_sts) +{ + int ret; + const struct bmi270_config *cfg = dev->config; + const uint8_t result_sts_page = cfg->feature->gyr_gain_status->page; + uint16_t status_value; + + /* Check address rules */ + ret = bmi270_feature_addr_check(cfg->feature->gyr_gain_status->addr); + if (ret < 0) { + LOG_ERR("gyr_gain_status addr check failed, err:%d", ret); + return ret; + } + + /* Disable advanced power save mode */ + ret = bmi270_set_aps(BMI270_PWR_CONF_ADV_PWR_SAVE_DIS, dev); + if (ret != 0) { + LOG_ERR("Failed bmi270_set_aps, err %d", ret); + return ret; + } + + /* Select result_sts_page */ + ret = bmi270_reg_write(dev, BMI270_REG_FEAT_PAGE, &result_sts_page, 1); + if (ret < 0) { + LOG_ERR("bmi270_reg_write (0x%02x) failed: %d", BMI270_REG_FEAT_PAGE, ret); + return ret; + } + + /* Read feature gyr_gain_status*/ + ret = bmi270_reg_read(dev, cfg->feature->gyr_gain_status->addr, (uint8_t *)&status_value, + 2); + if (ret < 0) { + LOG_ERR("bmi270_reg_read (0x%02x) failed: %d", cfg->feature->gyr_gain_status->addr, + ret); + return ret; + } + LOG_DBG("Read feature reg[0x%02x]@%d = 0x%04x", cfg->feature->gyr_gain_status->addr, + result_sts_page, status_value); + + /* Get the saturation status for x-axis */ + result_sts->sat_x = (status_value & BMI270_GYR_GAIN_STATUS_SAT_X_MASK) >> + BMI270_GYR_GAIN_STATUS_SAT_X_POS; + /* Get the saturation status for y-axis */ + result_sts->sat_y = (status_value & BMI270_GYR_GAIN_STATUS_SAT_Y_MASK) >> + BMI270_GYR_GAIN_STATUS_SAT_Y_POS; + /* Get the saturation status for z-axis */ + result_sts->sat_z = (status_value & BMI270_GYR_GAIN_STATUS_SAT_Z_MASK) >> + BMI270_GYR_GAIN_STATUS_SAT_Z_POS; + /* Get g trigger status */ + result_sts->g_trigger_status = + (GTriggerStatus)((status_value & BMI270_GYR_GAIN_STATUS_G_TRIG_MASK) >> + BMI270_GYR_GAIN_STATUS_G_TRIG_POS); + LOG_DBG("Status in x-axis: %d y-axis: %d z-axis %d, gtrigger: %d", result_sts->sat_x, + result_sts->sat_y, result_sts->sat_z, result_sts->g_trigger_status); + + return 0; +} + +/** + * @brief This internal function gets the compensated user-gain data of gyroscope + * gain update. + * + * @param[in] dev : Structure instance of bmi270 device + * @param[out] bmi2_gyr_user_gain_status : Structure that stores the compensated + * user-gain data of gyroscope + * + * @return Result of read compensated user-gain data execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int bmi270_read_gyro_user_gain(const struct device *dev, + struct bmi2_gyro_user_gain_data *gyr_usr_gain) +{ + int ret; + /* Variable to define register data */ + uint8_t reg_data[3] = {0}; + + /* Get the gyroscope compensated gain values */ + ret = bmi270_reg_read(dev, BMI270_GYR_USR_GAIN_0, reg_data, 3); + if (ret < 0) { + LOG_ERR("failed to get the gyroscope compensated gain values, err: %d", ret); + return ret; + } + + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)(reg_data[0] & BMI270_GYR_USR_GAIN_MASK); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)(reg_data[1] & BMI270_GYR_USR_GAIN_MASK); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)(reg_data[2] & BMI270_GYR_USR_GAIN_MASK); + + LOG_INF("Gyroscope user gain correction, X: %d Y: %d Z: %d", gyr_usr_gain->x, + gyr_usr_gain->y, gyr_usr_gain->z); + + return ret; +} + +/** + * @brief Method to prepare the setup for crt processing + * + * @param[in] dev : Structure instance of bmi270 device + * + * @return Result of prepare the setup for crt execution + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int crt_prepare_setup(const struct device *dev) +{ + int ret; + uint8_t pwr_ctrl; + uint8_t fifo_config1; + + /* Get Power mode control register */ + ret = bmi270_reg_read(dev, BMI270_REG_PWR_CTRL, &pwr_ctrl, 1); + if (ret != 0) { + LOG_ERR("Read power config register failed w/ error: %d", ret); + return ret; + } + /* Clears any bits above bit 3, unused */ + pwr_ctrl &= BMI270_PWR_CTRL_MSK; + + /* Disable gyroscope */ + pwr_ctrl &= ~BMI270_PWR_CTRL_GYR_EN; + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_PWR_CTRL, &pwr_ctrl, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to disable gyroscope, err: %d", ret); + return ret; + } + + /* Disable FIFO for all sensors */ + ret = bmi270_reg_read(dev, BMI270_REG_FIFO_CONFIG_1, &fifo_config1, 1); + if (ret != 0) { + LOG_ERR("Read BMI270_REG_FIFO_CONFIG_1 failed w/ error: %d", ret); + return ret; + } + fifo_config1 &= ~BMI270_FIFO_CONFIG_1_SENSORS_MSK; + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_FIFO_CONFIG_1, &fifo_config1, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to disable FIFO for all sensors, err: %d", ret); + return ret; + } + + /* Enable accelerometer */ + pwr_ctrl |= BMI270_PWR_CTRL_ACC_EN; + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_PWR_CTRL, &pwr_ctrl, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to enable accelerometer, Error code %d", ret); + return ret; + } + + /* Set G_TRIG_1.block=0 / Disable Abort */ + ret = bmi270_g_trig_1_block_unblock(dev); + if (ret != 0) { + LOG_ERR("Failed to unblock crt g_trig feature, err: %d", ret); + } + + return ret; +} + +/** + * @brief This function programs the non volatile memory(nvm) + * + * @param[in] dev: Structure instance of bmi270 device. + * + * @return Result of execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int bmi270_nvm_prog(const struct device *dev) +{ + const struct bmi270_config *cfg = dev->config; + const uint8_t nvm_prog_en = BMI270_NVM_PROG_EN; + const uint8_t nvm_prog_cmd = BMI270_CMD_NVM_PROG; + int ret; + uint8_t status; + uint8_t cmd_rdy; + uint8_t tries; + + /* Disable advanced power save mode */ + ret = bmi270_set_aps(BMI270_PWR_CONF_ADV_PWR_SAVE_DIS, dev); + if (ret != 0) { + LOG_ERR("Failed bmi270_set_aps, err %d", ret); + return ret; + } + + /* Read Sensor status flags */ + ret = bmi270_reg_read(dev, BMI270_REG_STATUS, &status, 1); + if (ret != 0) { + LOG_ERR("Failed to read BMI270_REG_STATUS, error: %d", ret); + return ret; + } + + /* cmd_rdy tells if a nvm prog is already in progress*/ + cmd_rdy = ((status & BMI270_CMD_RDY_MSK) >> BMI270_CMD_RDY_POS); + if (cmd_rdy == 0) { + LOG_ERR("NVM prog already running, canceling new request"); + return -ECANCELED; + } + + /* Prepare NVM write by setting GEN_SET_1.nvm_prog_prep =0b1 */ + ret = bmi270_feature_reg_rmw( + dev, cfg->feature->gen_set_1, BMI270_GEN_SET_1_NVM_PROG_PREP_MASK, + BMI270_GEN_SET_1_NVM_PROG_PREP_POS, BMI270_GEN_SET_1_NVM_PROG_PREP_EN); + if (ret != 0) { + LOG_ERR("Failed set_nvm_prep_prog, err: %d", ret); + return ret; + } + + /* Wait 40 ms */ + k_usleep(40000); + + /* Set bit 1 NVM_CONF.nvm_prog_en in order to unlock the NVM. */ + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_NVM_CONF, &nvm_prog_en, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to set the NVM_CONF.nvm_prog_en bit, err: %d", ret); + return ret; + } + + /* Write prog_nvm to the CMD register to trigger the write process */ + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_CMD, &nvm_prog_cmd, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to Send NVM prog command to command register, err: %d", ret); + return ret; + } + LOG_INF("Programming NVM ..."); + /* Wait till write operation is completed */ + for (tries = 0; tries <= BMI270_NVM_STATUS_CHECK_RETRIES; tries++) { + ret = bmi270_reg_read(dev, BMI270_REG_STATUS, &status, 1); + if (ret != 0) { + LOG_ERR("Failed to read BMI270_REG_STATUS, error: %d", ret); + return ret; + } + + cmd_rdy = ((status & BMI270_CMD_RDY_MSK) >> BMI270_CMD_RDY_POS); + /* Nvm is complete once cmd_rdy is 1, break if 1 */ + if (cmd_rdy) { + LOG_INF("NVM prog Completed!"); + break; + } + /* Wait till cmd_rdy becomes 1 indicating + * nvm process completes + */ + k_usleep(BMI270_STATUS_CHECK_POLL_PERIOD_US); + } + + /* Check if write operation timed-out */ + if (tries == BMI270_NVM_STATUS_CHECK_RETRIES) { + LOG_ERR("Failed in CRT status check: Reached max number of retries"); + return -ETIME; + } + + /* perform soft reset after nvm prog */ + ret = bmi270_soft_reset(dev); + if (ret != 0) { + LOG_ERR("Soft reset failed, err: %d", ret); + return ret; + } + + return 0; +} + +/** + * @brief This function enables/disables gain compensation + * with the gain defined in the gyr_usr_gain_[xyz] register + * to filtered and unfiltered Gyroscope data + * + * @param[in] dev : Structure instance of bmi270 device. + * @param[in] status : sensor_value.val1: 0x01-enable, 0x00-disable, + * + * @return Result of execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int bmi270_set_gyro_gain(const struct device *dev, const struct sensor_value *status) +{ + int ret; + uint8_t reg_data; + + /* Read gyr_gain_en register */ + ret = bmi270_reg_read(dev, BMI270_REG_OFFSET_6, ®_data, 1); + if (ret != 0) { + LOG_ERR("Failed to read BMI270_REG_STATUS, error: %d", ret); + return ret; + } + + /* Read gyr_gain_en register */ + reg_data = BMI270_SET_BITS(reg_data, BMI270_GYR_GAIN_EN, status->val1 & BMI270_GYR_GAIN_EN); + + /* Enable/Disable gyroscope gain */ + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_OFFSET_6, ®_data, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to set the BMI270_REG_OFFSET_6, err: %d", ret); + return ret; + } + LOG_DBG("Gyro usr gain compensation status:%d", status->val1 & BMI270_GYR_GAIN_EN); + return ret; +} + +/** + * @brief Method to run the crt process + * + * @param[in] dev : Structure instance of bmi270 device + * + * @note CRT may run in the full operating temperature range. + * datasheet recommends to run CRT at the operating + * temperature of the device. + * The sensitivity error is typically minimal at the + * temperature CRT was performed at. + * + * @param[in] dev : Structure instance of bmi270 device. + * + * @return Result of CRT execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int bmi270_gyro_crt(const struct device *dev) +{ + int ret; + uint8_t gyro_crt_conf; + uint8_t tries; + const uint8_t g_trigger_cmd = BMI270_CMD_G_TRIGGER; + struct bmi2_gyro_user_gain_data before_crt_gain = {0}; + + /* Initial user gain state */ + ret = bmi270_read_gyro_user_gain(dev, &before_crt_gain); + if (ret != 0) { + LOG_ERR("Failed in bmi270_read_gyro_user_gain: Error code %d", ret); + return ret; + } + + /* Disable advanced power save mode */ + ret = bmi270_set_aps(BMI270_PWR_CONF_ADV_PWR_SAVE_DIS, dev); + if (ret != 0) { + LOG_ERR("Failed bmi270_set_aps, err %d", ret); + return ret; + } + + /* Get crt running status */ + ret = bmi270_reg_read(dev, BMI270_REG_GYR_CRT_CONF, &gyro_crt_conf, 1); + if (ret != 0) { + LOG_ERR("Failed to read GYR_CRT_CONF, err: %d", ret); + return ret; + } + + if ((gyro_crt_conf & BMI270_GYR_CRT_CONF_RUNNING_MSK) >> BMI270_GYR_CRT_CONF_RUNNING_POS) { + LOG_ERR("CRT already running!"); + return -ECANCELED; + } + + /* Set GYR_CRT_CONF.crt_running=0b1 */ + gyro_crt_conf = BMI270_SET_BITS(gyro_crt_conf, BMI270_GYR_CRT_CONF_RUNNING, + BMI270_GYR_CRT_CONF_RUNNING_EN); + + ret = bmi270_reg_write_with_delay(dev, BMI270_REG_GYR_CRT_CONF, &gyro_crt_conf, 1, + BMI270_INTER_WRITE_DELAY_US); + if (ret != 0) { + LOG_ERR("Failed to enable CRT running, err: %d", ret); + return ret; + } + + /* CRT prepare setup */ + ret = crt_prepare_setup(dev); + if (ret != 0) { + LOG_ERR("CRT prepare setup failed, err: %d", ret); + return ret; + } + + /* Ensure that the device is at rest during CRT execution */ + LOG_WRN("Ensure that the device is at rest during CRT execution!"); + + /* Execute CRT / Set G_TRIG_1.select=1 */ + ret = bmi270_g_trig_1_select_crt(dev); + if (ret != 0) { + LOG_ERR("Failed to enable select crt in g_trig feature, err: %d", ret); + return ret; + } + + /* Send g_trigger command using the register CMD */ + ret = bmi270_reg_write(dev, BMI270_REG_CMD, &g_trigger_cmd, 1); + if (ret != 0) { + LOG_ERR("Failed to send g_trigger_cmd, err: %d", ret); + return ret; + } + + LOG_INF("CRT running..."); + /* CRT is complete, after the device sets GYR_CRT_CONF.crt_running=0b0 + * + * Timeout after @ref BMI270_CRT_STATUS_CHECK_RETRIES x + * @ref BMI270_CRT_STATUS_CHECK_POLL_PERIOD_US microseconds. + * If tries is equal to @ref BMI270_CRT_STATUS_CHECK_RETRIES + * by the end of the loop, report an error + */ + for (tries = 0; tries <= BMI270_CRT_STATUS_CHECK_RETRIES; tries++) { + ret = bmi270_reg_read(dev, BMI270_REG_GYR_CRT_CONF, &gyro_crt_conf, 1); + if (ret != 0) { + LOG_ERR("Failed to read Gyro CRT config, err: %d", ret); + return ret; + } + gyro_crt_conf &= BMI270_GYR_CRT_CONF_RUNNING_MSK; + if (gyro_crt_conf == BMI270_GYR_CRT_CONF_RUNNING_DIS) { + LOG_INF("CRT Completed!"); + break; + } + k_usleep(BMI270_STATUS_CHECK_POLL_PERIOD_US); + } + + /* Check timed-out */ + if (tries == BMI270_CRT_STATUS_CHECK_RETRIES) { + LOG_ERR("Failed in CRT status check: Reached max number of retries"); + return -ETIME; + } + + /* Get CRT results */ + ret = bmi270_get_gyro_gain_update_status(dev, &crt_result_sts); + if (ret != 0) { + LOG_ERR("Failed in get_bmi270_crt_results: Error code %d", ret); + return ret; + } + + /* Print CRT result status */ + crt_error_log(crt_result_sts.g_trigger_status); + + /* Wait for the gyro gain data to be updated */ + k_sleep(K_MSEC(350)); + + /* Get the gyroscope gain update data */ + ret = bmi270_read_gyro_user_gain(dev, &crt_gain); + if (ret != 0) { + LOG_ERR("Failed in bmi270_read_gyro_user_gain: Error code %d", ret); + return ret; + } + + /* Check if new gain values are different */ + if ((before_crt_gain.x == crt_gain.x) && (before_crt_gain.y == crt_gain.y) && + (before_crt_gain.z == crt_gain.z)) { + LOG_WRN("CRT new user-gyro gains remained the same"); + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + ret = bmi270_set_aps(BMI270_PWR_CONF_ADV_PWR_SAVE_EN, dev); + if (ret != 0) { + return ret; + } + + /* The new gain values are applied automatically at the next start of the gyroscope */ + return 0; +}