diff --git a/examples/Watchy_Accelerometer/Watchy_Accelerometer.ino b/examples/Watchy_Accelerometer/Watchy_Accelerometer.ino new file mode 100644 index 0000000..298551a --- /dev/null +++ b/examples/Watchy_Accelerometer/Watchy_Accelerometer.ino @@ -0,0 +1,33 @@ +/* + * Watchy - Accelerometer Example + * Prints out the BMA423 accelerometer data on Serial + */ + +#ifndef ESP32 +#error Please select ESP32 Wrover Module under Tools > Board +#endif + +#include + +BMA *bma = nullptr; +I2CBus *i2c = nullptr; + +void setup() +{ + byte data; + Serial.begin(115200); + i2c = new I2CBus(); + bma = new BMA(*i2c); + bma->begin(); + bma->enableAccel(); +} + +void loop(){ + Accel acc; + bool res = bma->getAccel(acc); + Serial.print(acc.x); + Serial.print(" , "); + Serial.print(acc.y); + Serial.print(" , "); + Serial.println(acc.z); +} \ No newline at end of file diff --git a/src/bma.cpp b/src/bma.cpp new file mode 100644 index 0000000..23c1e19 --- /dev/null +++ b/src/bma.cpp @@ -0,0 +1,273 @@ +#include "bma.h" +#include + +I2CBus *BMA::_bus = nullptr; + +BMA::BMA(I2CBus &bus) +{ + _bus = &bus; +} + +BMA::~BMA() +{ + +} + +uint16_t BMA::read(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len) +{ + return _bus->readBytes(addr, reg, data, len); +} + +uint16_t BMA::write(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len) +{ + return _bus->writeBytes(addr, reg, data, len); +} + +bool BMA::begin() +{ + _dev.dev_addr = BMA4_I2C_ADDR_PRIMARY; + _dev.interface = BMA4_I2C_INTERFACE; + _dev.bus_read = read; + _dev.bus_write = write; + _dev.delay = delay; + _dev.read_write_len = 8; + _dev.resolution = 12; + _dev.feature_len = BMA423_FEATURE_SIZE; + + reset(); + + delay(20); + + if (bma423_init(&_dev) != BMA4_OK) { + Serial.println("bma4 init fail"); + return false; + } + + config(); + + return true; +} + +void BMA::reset() +{ + uint8_t reg = 0xB6; + _bus->writeBytes(BMA4_I2C_ADDR_PRIMARY, 0x7E, ®, 1); +} + +uint16_t BMA::config() +{ + return bma423_write_config_file(&_dev); +} + +bool BMA::getAccel(Accel &acc) +{ + memset(&acc, 0, sizeof(acc)); + if (bma4_read_accel_xyz(&acc, &_dev) != BMA4_OK) { + return false; + } + return true; +} + +uint8_t BMA::direction() +{ + Accel acc; + if (bma4_read_accel_xyz(&acc, &_dev) != BMA4_OK) { + return 0; + } + uint16_t absX = abs(acc.x); + uint16_t absY = abs(acc.y); + uint16_t absZ = abs(acc.z); + + if ((absZ > absX) && (absZ > absY)) { + if (acc.z > 0) { + return DIRECTION_DISP_DOWN; + } else { + return DIRECTION_DISP_UP; + } + } else if ((absY > absX) && (absY > absZ)) { + if (acc.y > 0) { + return DIRECTION_BOTTOM_EDGE; + } else { + return DIRECTION_TOP_EDGE; + } + } else { + if (acc.x < 0) { + return DIRECTION_RIGHT_EDGE; + } else { + return DIRECTION_LEFT_EDGE; + } + } +} + +float BMA::temperature() +{ + int32_t data = 0; + bma4_get_temperature(&data, BMA4_DEG, &_dev); + float res = (float)data / (float)BMA4_SCALE_TEMP; + /* 0x80 - temp read from the register and 23 is the ambient temp added. + * If the temp read from register is 0x80, it means no valid + * information is available */ + if (((data - 23) / BMA4_SCALE_TEMP) == 0x80) { + res = 0; + } + return res; +} + + +void BMA::enableAccel() +{ + if (bma4_set_accel_enable(BMA4_ENABLE, &_dev)) { + return; + } + Acfg cfg; + cfg.odr = BMA4_OUTPUT_DATA_RATE_100HZ; + cfg.range = BMA4_ACCEL_RANGE_2G; + cfg.bandwidth = BMA4_ACCEL_NORMAL_AVG4; + cfg.perf_mode = BMA4_CONTINUOUS_MODE; + + if (bma4_set_accel_config(&cfg, &_dev)) { + Serial.println("[bma4] set accel config fail"); + return; + } +} + +void BMA::disalbeIrq() +{ + bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_STEP_CNTR_INT /* |BMA423_WAKEUP_INT*/, BMA4_DISABLE, &_dev); +} + +void BMA::enableIrq() +{ + bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_STEP_CNTR_INT /* |BMA423_WAKEUP_INT*/, BMA4_ENABLE, &_dev); +} + +//attachInterrupt bma423 int1 +void BMA::attachInterrupt() +{ + uint16_t rslt = BMA4_OK; + enableAccel(); + // rslt |= bma423_reset_step_counter(&_dev); + rslt |= bma423_step_detector_enable(BMA4_ENABLE, &_dev); + rslt |= bma423_feature_enable(BMA423_STEP_CNTR, BMA4_ENABLE, &_dev); + rslt |= bma423_feature_enable(BMA423_WAKEUP, BMA4_ENABLE, &_dev); + rslt |= bma423_feature_enable(BMA423_TILT, BMA4_ENABLE, &_dev); + rslt |= bma423_step_counter_set_watermark(100, &_dev); + + // rslt |= bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_STEP_CNTR_INT | BMA423_WAKEUP_INT, BMA4_ENABLE, &_dev); + + rslt |= bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_STEP_CNTR_INT, BMA4_ENABLE, &_dev); + rslt |= bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_TILT_INT, BMA4_ENABLE, &_dev); + + bma423_anymotion_enable_axis(BMA423_ALL_AXIS_DIS, &_dev); + + struct bma4_int_pin_config config ; + + config.edge_ctrl = BMA4_LEVEL_TRIGGER; + config.lvl = BMA4_ACTIVE_HIGH; + config.od = BMA4_PUSH_PULL; + config.output_en = BMA4_OUTPUT_ENABLE; + config.input_en = BMA4_INPUT_DISABLE; + rslt |= bma4_set_int_pin_config(&config, BMA4_INTR1_MAP, &_dev); + + // Serial.printf("[bma4] attachInterrupt %s\n", rslt != 0 ? "fail" : "pass"); + + + struct bma423_axes_remap remap_data; + remap_data.x_axis = 0; + remap_data.x_axis_sign = 1; + remap_data.y_axis = 1; + remap_data.y_axis_sign = 1; + remap_data.z_axis = 2; + remap_data.z_axis_sign = 0; + + bma423_set_remap_axes(&remap_data, &_dev); + +} + +bool BMA::readInterrupt() +{ + return bma423_read_int_status(&_irqStatus, &_dev) == BMA4_OK; +} + +uint8_t BMA::getIrqStatus() +{ + return _irqStatus; +} + +uint32_t BMA::getCounter() +{ + uint32_t stepCount; + if (bma423_step_counter_output(&stepCount, &_dev) == BMA4_OK) { + return stepCount; + } + return 0; +} + +bool BMA::isStepCounter() +{ + return (bool)(BMA423_STEP_CNTR_INT & _irqStatus); +} + +bool BMA::isDoubleClick() +{ + return (bool)(BMA423_WAKEUP_INT & _irqStatus); +} + + +bool BMA::isTilt() +{ + return (bool)(BMA423_TILT_INT & _irqStatus); +} + + +bool BMA::isActivity() +{ + return (bool)(BMA423_ACTIVITY_INT & _irqStatus); +} + +bool BMA::isAnyNoMotion() +{ + return (bool)(BMA423_ANY_NO_MOTION_INT & _irqStatus); +} + + +const char *BMA::getActivity() +{ + uint8_t activity; + bma423_activity_output(&activity, &_dev); + if (activity & BMA423_USER_STATIONARY) { + return "BMA423_USER_STATIONARY"; + } else if (activity & BMA423_USER_WALKING) { + return "BMA423_USER_WALKING"; + } else if (activity & BMA423_USER_RUNNING) { + return "BMA423_USER_RUNNING"; + } else if (activity & BMA423_STATE_INVALID) { + return "BMA423_STATE_INVALID"; + } + return "None"; +} + +bool BMA::enableStepCountInterrupt(bool en) +{ + return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_STEP_CNTR_INT, en, &_dev)); +} + +bool BMA::enableTiltInterrupt(bool en) +{ + return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_TILT_INT, en, &_dev)); +} + +bool BMA::enableWakeupInterrupt(bool en) +{ + return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_WAKEUP_INT, en, &_dev)); +} + +bool BMA::enableAnyNoMotionInterrupt(bool en) +{ + return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_ANY_NO_MOTION_INT, en, &_dev)); +} + +bool BMA::enableActivityInterrupt(bool en) +{ + return (BMA4_OK == bma423_map_interrupt(BMA4_INTR1_MAP, BMA423_ACTIVITY_INT, en, &_dev)); +} \ No newline at end of file diff --git a/src/bma.h b/src/bma.h new file mode 100644 index 0000000..4c07406 --- /dev/null +++ b/src/bma.h @@ -0,0 +1,63 @@ + +#pragma once + +#include "bma423.h" +#include "i2c_bus.h" + + +enum { + DIRECTION_TOP_EDGE = 0, + DIRECTION_BOTTOM_EDGE = 1, + DIRECTION_LEFT_EDGE = 2, + DIRECTION_RIGHT_EDGE = 3, + DIRECTION_DISP_UP = 4, + DIRECTION_DISP_DOWN = 5 +} ; +typedef struct bma4_dev Bma; +typedef struct bma4_accel Accel; +typedef struct bma4_accel_config Acfg; + +class BMA +{ +public: + BMA(I2CBus &bus); + ~BMA(); + bool begin(); + void reset(); + uint8_t direction(); + float temperature(); + void enableAccel(); + + void disalbeIrq(); + void enableIrq(); + void attachInterrupt(); + uint32_t getCounter(); + bool isStepCounter(); + bool isDoubleClick(); + bool readInterrupt(); + bool isTilt(); + bool isActivity(); + bool isAnyNoMotion(); + bool getAccel(Accel &acc); + uint8_t getIrqStatus(); + const char * getActivity(); + + bool enableStepCountInterrupt(bool en = true); + bool enableTiltInterrupt(bool en = true); + bool enableWakeupInterrupt(bool en = true); + bool enableAnyNoMotionInterrupt(bool en = true); + bool enableActivityInterrupt(bool en = true); + +private: + static uint16_t read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *read_data, uint16_t len); + static uint16_t write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *read_data, uint16_t len); + + uint16_t config(); + Bma _dev; + static bma4_com_fptr_t _read; + static bma4_com_fptr_t _write; + static I2CBus *_bus; + bool _irqRead = false; + uint16_t _irqStatus; + +}; \ No newline at end of file diff --git a/src/bma4.c b/src/bma4.c new file mode 100644 index 0000000..ac4e5eb --- /dev/null +++ b/src/bma4.c @@ -0,0 +1,4204 @@ +/* +**************************************************************************** +* Copyright (C) 2015 - 2016 Bosch Sensortec GmbH +* +* File :bma4.c +* +* Date: 12 Oct 2017 +* +* Revision: 2.1.9 $ +* +* Usage: Sensor Driver for BMA4 family of sensors +* +**************************************************************************** +* Disclaimer +* +* Common: +* Bosch Sensortec products are developed for the consumer goods industry. +* They may only be used within the parameters of the respective valid +* product data sheet. Bosch Sensortec products are provided with the +* express understanding that there is no warranty of fitness for a +* particular purpose.They are not fit for use in life-sustaining, +* safety or security sensitive systems or any system or device +* that may lead to bodily harm or property damage if the system +* or device malfunctions. In addition,Bosch Sensortec products are +* not fit for use in products which interact with motor vehicle systems. +* The resale and or use of products are at the purchasers own risk and +* his own responsibility. The examination of fitness for the intended use +* is the sole responsibility of the Purchaser. +* +* The purchaser shall indemnify Bosch Sensortec from all third party +* claims, including any claims for incidental, or consequential damages, +* arising from any product use not covered by the parameters of +* the respective valid product data sheet or not approved by +* Bosch Sensortec and reimburse Bosch Sensortec for all costs in +* connection with such claims. +* +* The purchaser must monitor the market for the purchased products, +* particularly with regard to product safety and inform Bosch Sensortec +* without delay of all security relevant incidents. +* +* Engineering Samples are marked with an asterisk (*) or (e). +* Samples may vary from the valid technical specifications of the product +* series. They are therefore not intended or fit for resale to third +* parties or for use in end products. Their sole purpose is internal +* client testing. The testing of an engineering sample may in no way +* replace the testing of a product series. Bosch Sensortec assumes +* no liability for the use of engineering samples. +* By accepting the engineering samples, the Purchaser agrees to indemnify +* Bosch Sensortec from all claims arising from the use of engineering +* samples. +* +* Special: +* This software module (hereinafter called "Software") and any information +* on application-sheets (hereinafter called "Information") is provided +* free of charge for the sole purpose to support your application work. +* The Software and Information is subject to the following +* terms and conditions: +* +* The Software is specifically designed for the exclusive use for +* Bosch Sensortec products by personnel who have special experience +* and training. Do not use this Software if you do not have the +* proper experience or training. +* +* This Software package is provided `` as is `` and without any expressed +* or implied warranties,including without limitation, the implied warranties +* of merchantability and fitness for a particular purpose. +* +* Bosch Sensortec and their representatives and agents deny any liability +* for the functional impairment +* of this Software in terms of fitness, performance and safety. +* Bosch Sensortec and their representatives and agents shall not be liable +* for any direct or indirect damages or injury, except as +* otherwise stipulated in mandatory applicable law. +* +* The Information provided is believed to be accurate and reliable. +* Bosch Sensortec assumes no responsibility for the consequences of use +* of such Information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of Bosch. Specifications mentioned in the Information are +* subject to change without notice. +**************************************************************************/ +/*! \file bma4.c + \brief Sensor Driver for BMA4 family of sensors */ +/***************************************************************************/ +/**\name Header files +****************************************************************************/ +#include "bma4.h" + +/***************************************************************************/ +/**\name Static Data Buffer +****************************************************************************/ +/* Local array to store the values read from the register + * using read_regs API */ +static uint8_t temp_buff[BMA4_MAX_BUFFER_SIZE] = {0}; + +/***************************************************************************/ +/**\name Local structures +****************************************************************************/ +/*! + * @brief Accel difference value of axis. + */ +struct data_with_sign { + /*! Difference value */ + int16_t val; + /*! Indicates negative value if set */ + uint8_t is_negative; +}; + +/*! + * @brief Accel data deviation from ideal value + */ +struct offset_delta { + /*! Accel x axis */ + struct data_with_sign x; + /*! Accel y axis */ + struct data_with_sign y; + /*! Accel z axis */ + struct data_with_sign z; +}; + +/*! + * @brief Accel offset xyz structure + */ +struct accel_offset { + /*! Accel offset X data */ + uint8_t x; + /*! Accel offset Y data */ + uint8_t y; + /*! Accel offset Z data */ + uint8_t z; +}; + +/*! + * @brief Accel self test diff xyz data structure + */ +struct selftest_delta_limit { + /*! Accel X data */ + int32_t x; + /*! Accel Y data */ + int32_t y; + /*! Accel Z data */ + int32_t z; +}; + +/*! + * @brief Structure to store temp data values + */ +struct accel_temp { + /*! Accel X temp data */ + int32_t x; + /*! Accel Y temp data */ + int32_t y; + /*! Accel Z temp data */ + int32_t z; +}; + +/***************************************************************************/ +/*! Static Function Declarations +****************************************************************************/ + +/*! + * @brief This API validates the bandwidth and perfmode + * value set by the user. + * + * param bandwidth[in] : bandwidth value set by the user. + * param perf_mode[in] : perf_mode value set by the user. + */ +static uint16_t validate_bandwidth_perfmode(uint8_t bandwidth, uint8_t perf_mode); + +/*! + * @brief @brief This API validates the ODR value set by the user. + * + * param bandwidth[in] : odr for accelerometer + */ +static uint16_t validate_odr(uint8_t odr); + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + * + * @param dev[in,out] : Structure instance of bma4_dev + * + */ +static void reset_fifo_data_structure(const struct bma4_dev *dev); + +/*! + * @brief This API computes the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + * + * @param[out] start_idx : The start index for parsing data + * @param[out] len : Number of bytes to be parsed + * @param[in] acc_count : Number of accelerometer frames to be read + * @param[in] dev : Structure instance of bma4_dev. + * + */ +static void get_accel_len_to_parse(uint16_t *start_idx, uint16_t *len, const uint16_t *acc_count, + const struct bma4_dev *dev); + +/*! + * @brief This API checks the fifo read data as empty frame, if it + * is empty frame then moves the index to last byte. + * + * @param[in,out] data_index : The index of the current data to + * be parsed from fifo data + * @param[in] dev : Structure instance of bma4_dev. + */ +static void check_empty_fifo(uint16_t *data_index, const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + * + * @param[in,out] accel_data : Structure instance of bma4_accel where + * the accelerometer data in FIFO is stored. + * @param[in,out] accel_length : Number of accelerometer frames + * (x,y,z axes data) + * @param[in,out] dev : Structure instance of bma4_dev. + * + */ +static void extract_accel_header_mode(struct bma4_accel *accel_data, uint16_t *accel_length, + const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It update the idx value which is used to store the index of + * the current data byte which is parsed. + * + * @param[in,out] acc : Structure instance of bma4_accel. + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] acc_idx : Index value of accelerometer data + * (x,y,z axes) frame to be parsed + * @param[in] frm : It consists of either fifo_data_enable + * parameter (Accel and/or mag data enabled in FIFO) + * in header-less mode or frame header data + * in header mode + * @param[in] dev : Structure instance of bma4_dev. + * + */ +static void unpack_acc_frm(struct bma4_accel *acc, uint16_t *idx, uint16_t *acc_idx, uint8_t frm, + const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bma4_accel. + * + * @param[out] accel_data : Structure instance of bma4_accel where + * the parsed accel data bytes are stored. + * @param[in] data_start_index : Index value of the accel data bytes + * which is to be parsed from the fifo data. + * @param[in] dev : Structure instance of bma4_dev. + * + */ +static void unpack_accel_data(struct bma4_accel *accel_data, uint16_t data_start_index, const struct bma4_dev *dev); +/*! + * @brief This API computes the number of bytes of Mag FIFO data which is + * to be parsed in header-less mode + * + * @param[out] start_idx : The start index for parsing data + * @param[out] len : Number of bytes to be parsed + * @param[in] mag_count : Number of magnetometer frames to be read + * @param[in] dev : Structure instance of bma4_dev. + * + */ +static void get_mag_len_to_parse(uint16_t *start_idx, uint16_t *len, const uint16_t *mag_count, + const struct bma4_dev *dev); +/*! + * @brief This API is used to parse the magnetometer data from the + * FIFO data in header mode. + * + * @param[in,out] data : Structure instance of bma4_mag_xyzr where + * the magnetometer data in FIFO is extracted + * and stored. + * @param[in,out] len : Number of magnetometer frames + * (x,y,z,r data) + * @param[in,out] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t extract_mag_header_mode(struct bma4_mag *data, uint16_t *len, const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse the magnetometer data from the + * FIFO data in both header mode and header-less mode and update the + * idx value which is used to store the index of the current + * data byte which is parsed. + * + * @param data : Structure instance of bma4_mag_xyzr. + * @param idx : Index value of number of bytes parsed + * @param mag_idx : Index value magnetometer data frame (x,y,z,r) + * to be parsed + * @param frm : It consists of either the fifo_data_enable parameter + * (Accel and/or mag data enabled in FIFO) in + * header-less mode and frame header data in header mode + * @param dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t unpack_mag_frm(struct bma4_mag *data, uint16_t *idx, uint16_t *mag_idx, uint8_t frm, + const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse the auxiliary magnetometer data from + * the FIFO data and store it in the instance of the structure mag_data. + * + * @param mag_data : Structure instance of bma4_mag_xyzr where the + * parsed magnetometer data bytes are stored. + * @param start_idx : Index value of the magnetometer data bytes + * which is to be parsed from the FIFO data + * @param dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t unpack_mag_data(struct bma4_mag *mag_data, uint16_t start_idx, const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the sensor time. + * @param[in,out] dev : Structure instance of bma4_dev. + * + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the skipped frame count. + * @param[in,out] dev : Structure instance of bma4_dev. + * + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bma4_dev *dev); + +/*! + * @brief This API is used to parse and store the dropped_frame_count from + * the FIFO data in the structure instance dev. + * + * @param[in,out] data_index : Index of the FIFO data which + * has the dropped frame data. + * @param[in,out] dev : Structure instance of bma4_dev. + * + */ +static void unpack_dropped_frame(uint16_t *data_index, const struct bma4_dev *dev); + +/*! + * @brief This API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which + * is to be moved ahead of the + * current_frame_length + * @param[in] current_frame_length : Number of bytes in a particular frame + * @param[in] dev : Structure instance of bma4_dev. + * + */ +static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bma4_dev *dev); + +/*! +* @brief This API writes the config stream data in memory using burst mode +* +* @param[in] stream_data : Pointer to store data of 32 bytes +* @param[in] index : Represents value in multiple of 32 bytes +* @param[in] dev : Structure instance of bma4_dev. +* +* @return Result of API execution status +* @retval 0 -> Success +* @retval Any non zero value -> Fail +*/ +static uint16_t stream_transfer_write(const uint8_t *stream_data, uint16_t index, struct bma4_dev *dev); + +/*! + * @brief This API enables or disables the Accel Self test feature in the + * sensor. + * + * @param[in] accel_selftest_enable : Variable used to enable or disable + * the Accel self test feature + * Value | Description + * --------|--------------- + * 0x00 | BMA4_DISABLE + * 0x01 | BMA4_ENABLE + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t set_accel_selftest_enable(uint8_t accel_selftest_axis, struct bma4_dev *dev); + +/*! + * @brief This API selects the sign of Accel self-test excitation + * + * @param[in] accel_selftest_sign: Variable used to select the Accel + * self test sign + * Value | Description + * --------|-------------------------- + * 0x00 | BMA4_DISABLE (negative) + * 0x01 | BMA4_ENABLE (positive) + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t set_accel_selftest_sign(uint8_t accel_selftest_sign, struct bma4_dev *dev); + +/*! + * @brief This API sets the Accel self test amplitude in the sensor. + * + * @param[in] accel_selftest_amp : Variable used to specify the Accel self + * test amplitude + * Value | Description + * --------|------------------------------------ + * 0x00 | BMA4_SELFTEST_AMP_LOW + * 0x01 | BMA4_SELFTEST_AMP_HIGH + * + * @param[in] dev : structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t set_accel_selftest_amp(uint8_t accel_selftest_amp, struct bma4_dev *dev); + +/*! + * @brief This function enables and configures the Accel which is needed + * for Self test operation. + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return results of self test + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t set_accel_selftest_config(struct bma4_dev *dev); + +/*! + * @brief This API validates the Accel g value provided as input by the + * user for Accel offset compensation. + * + * @param[in] gvalue : Pointer variable used to specify the g value + * set by the user for Accel offset compensation. + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + * + * @return results of the status of user input validation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t validate_user_input(const int32_t *gvalue); +/*! + * @brief This API converts the range value into corresponding integer + * value. + * + * @param[in] range_in : input range value. + * @param[out] range_out : pointer to store the integer value of range. + * + * @return results of the status of user input validation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static void map_range(uint8_t range_in, uint8_t *range_out); + +/*! + * @brief This API normalise the data with offset. + * + * @param[out] compensated_data : pointer to store the compensated data. + * @param[in] offset_data : pointer of offset. + * + * @return results of the status of user input validation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static void normalise_offset(const struct offset_delta *compensated_data, struct accel_offset *offset_data); + +/*! + * @brief This API normalise the data with offset. + * + * @param[in] res : resolution of the sensor. + * @param[in] range : G-range of the accel. + * @param[in] delta : pointer of offset_delta. + * @param[out] data : pointer to store accel_offset data. + * + * @return results of the status of user input validation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static void scale_offset(uint8_t res, uint8_t range, const struct offset_delta *delta, struct accel_offset *data); + +/*! + * @brief This API compensate the accel data against gravity. + * + * @param[in] lsb_per_g : lsb value pre 1g. + * @param[in] g_val : G reference value of all axis. + * @param[in] data : pointer of accel_offset data. + * @param[out] comp_data : pointer to store compensated data. + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + * + * @return results of the status of user input validation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static void comp_for_grvty(uint16_t lsb_per_g, const int32_t g_val[3], const struct bma4_accel *data, + struct offset_delta *comp_data); +/*! + * @brief This function validates the Accel Self test data and decides the + * result of Self test operation. + * + * @param[in] accel_data_diff : Pointer to structure variable which holds + * the Accel data difference of Self test operation + * @param[in] dev : Structure instance of bma4_dev + * + * @return results of self test operation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t validate_selftest(const struct selftest_delta_limit *accel_data_diff, + const struct bma4_dev *dev); + +/*! + * @brief This function configure the Accel for FOC. + * + * @param[in] acc_conf : accel config structure instance + * @param[in] acc_en : enables the accel + * @param[in] pwr_mode : set the power mode + * @param[in] dev : Structure instance of bma4_dev + * + * @return results of self test operation + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +static uint16_t foc_config(struct bma4_accel_config *acc_conf, uint8_t *acc_en, uint8_t *pwr_mode, + struct bma4_dev *dev); + +/*! + * @brief This API converts lsb value of axes to mg for self-test + * + * @param[in] accel_data_diff : Pointer variable used to pass accel difference + * values in g + * @param[out] accel_data_diff_mg : Pointer variable used to store accel + * difference values in mg + * @param[out] dev : Structure instance of bma4_dev + * + * @return None * + */ +static void convert_lsb_g(const struct selftest_delta_limit *accel_data_diff, + struct selftest_delta_limit *accel_data_diff_mg, + const struct bma4_dev *dev); + +/*! + * @brief This API sets the feature config. data start address in the sensor. + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +static uint16_t set_feature_config_start_addr(struct bma4_dev *dev); + +/*! + * @brief This API increments the feature config. data address according to the user + * provided read/write length in the dev structure. + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +static uint16_t increment_feature_config_addr(const struct bma4_dev *dev); + +/*! + * @brief This API reads the 8-bit data from the given register + * in the sensor. + */ +static uint16_t read_regs(uint8_t addr, uint8_t *data, uint8_t len, const struct bma4_dev *dev); + +/*! + * @brief This API writes the 8-bit data to the given register + * in the sensor. + */ +static uint16_t write_regs(uint8_t addr, uint8_t *data, uint8_t len, const struct bma4_dev *dev); + +/*! + * @brief This API sets the feature config. data start address in the sensor. + */ +static uint16_t get_feature_config_start_addr(struct bma4_dev *dev); + +/*! + * @brief This API is used to calculate the power of given + * base value. + * + * @param[in] base : value of base + * @param[in] resolution : resolution of the sensor + * + * @return : return the value of base^resolution + */ + +static int32_t power(int16_t base, uint8_t resolution); + +/*! + * @brief This API performs roundoff on given value + * + * @param[in] value : Value which is need to be rounded off + * + * @return : None + */ +static int8_t roundoff(int32_t value); + +/*! + * @brief This API finds the bit position of 3.9mg according to given range + * and resolution. + * + * @param[in] range : range of the accel. + * @param[in] res : resolution of the accel. + * + * @return : bit position of 3.9mg + */ +static int8_t get_bit_pos_3_9mg(uint8_t range, uint8_t res); + +/*! + * @brief This API finds the the null error of the device pointer structure + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Null error + */ +static uint16_t bma4_null_pointer_check(const struct bma4_dev *dev); + +/*! + * @brief This internal API brings up the secondary interface to access + * auxiliary sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * + * @retval 0 if success, else fail + */ +static uint16_t bma4_set_aux_interface_config(struct bma4_dev *dev); + +/*! + * @brief This internal API reads the data from the auxiliary sensor + * depending on burst length configured + * + * @param[in] dev : Structure instance of bma4_dev. + * @param[out] aux_data : Pointer variable to store data read + * @param[in] aux_reg_addr : Variable to pass address from where + * data is to be read + * + * @return Result of API execution status + * + * @retval 0 if success, else fail + */ +static uint16_t bma4_extract_aux_data(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev); + +/*! + * @brief This internal API maps the actual burst read length with user length set. + * + * @param[in] dev : Structure instance of bma4_dev. + * @param[out] len : Pointer variable to store mapped length + * + * @return Result of API execution status + * + * @retval 0 if success, else fail + */ +static uint16_t bma4_map_read_len(uint8_t *len, const struct bma4_dev *dev); + +/***************************************************************************/ +/**\name Extern Declarations +****************************************************************************/ + +/***************************************************************************/ +/**\name Globals +****************************************************************************/ + + +/***************************************************************************/ +/**\name Function definitions +****************************************************************************/ +/*! + * @brief This API is the entry point. + * Call this API before using all other APIs. + * This API reads the chip-id of the sensor which is the first step to + * verify the sensor and also it configures the read mechanism of SPI and + * I2C interface. + */ +uint16_t bma4_init(struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + /* Check the bma4 structure as NULL */ + if ((dev == NULL) || (dev->bus_read == NULL) || (dev->bus_write == NULL)) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (dev->interface == BMA4_SPI_INTERFACE) + dev->dummy_byte = 1; + else + dev->dummy_byte = 0; + + rslt |= bma4_read_regs(BMA4_CHIP_ID_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + /* Assign Chip Id */ + dev->chip_id = data; + } + } + + return rslt; +} + +/*! + * @brief This API is used to write the binary configuration in the sensor + */ +uint16_t bma4_write_config_file(struct bma4_dev *dev) +{ + uint16_t rslt; + /* Config loading disable*/ + uint8_t config_load = 0; + uint16_t index = 0; + uint8_t config_stream_status = 0; + + /* Disable advanced power save */ + rslt = bma4_set_advance_power_save(BMA4_DISABLE, dev); + /* Wait for sensor time synchronization. Refer the data-sheet for + more information*/ + dev->delay(1); + + if (rslt == BMA4_OK) { + /* Disable config loading*/ + rslt |= bma4_write_regs(BMA4_INIT_CTRL_ADDR, &config_load, 1, dev); + /* Write the config stream */ + for (index = 0; index < BMA4_CONFIG_STREAM_SIZE; index += dev->read_write_len) + rslt |= stream_transfer_write((dev->config_file_ptr + index), index, dev); + + /* Enable config loading and FIFO mode */ + config_load = 0x01; + rslt |= bma4_write_regs(BMA4_INIT_CTRL_ADDR, &config_load, 1, dev); + /* Wait till ASIC is initialized. Refer the data-sheet for + more information*/ + dev->delay(150); + /* Read the status of config stream operation */ + rslt |= bma4_read_regs(BMA4_INTERNAL_STAT, &config_stream_status, 1, dev); + + if (config_stream_status != BMA4_ASIC_INITIALIZED) { + rslt |= BMA4_E_CONFIG_STREAM_ERROR; + } else { + /* Enable advanced power save */ + rslt |= bma4_set_advance_power_save(BMA4_ENABLE, dev); + rslt |= get_feature_config_start_addr(dev); + } + } + return rslt; +} + +/*! + * @brief This API checks whether the write operation requested is for feature + * config or register write and accordingly writes the data in the sensor. + */ +uint16_t bma4_write_regs(uint8_t addr, uint8_t *data, uint8_t len, struct bma4_dev *dev) +{ + uint8_t i; + uint8_t loop_count; + uint8_t overflow; + uint8_t index; + uint16_t rslt = BMA4_OK; + uint8_t adv_pwr_save = 0; + + /* Check the dev structure as NULL*/ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (addr == BMA4_FEATURE_CONFIG_ADDR) { + /* Disable APS if enabled before writing the feature config register */ + rslt = bma4_get_advance_power_save(&adv_pwr_save, dev); + if (adv_pwr_save == BMA4_ENABLE) { + rslt |= bma4_set_advance_power_save(BMA4_DISABLE, dev); + /* Wait for sensor time synchronization. Refer the data-sheet for + more information*/ + dev->delay(1); + } + + if (((len % 2) == 0) && (len <= dev->feature_len) && (rslt == BMA4_OK)) { + if (dev->read_write_len < len) { + /* Calculate the no of writes to be performed according + to the read/write length */ + loop_count = len / dev->read_write_len; + overflow = len % dev->read_write_len; + index = 0; + rslt = set_feature_config_start_addr(dev); + for (i = 0; i < loop_count; i++) { + rslt |= write_regs(BMA4_FEATURE_CONFIG_ADDR, data + index, + dev->read_write_len, dev); + rslt |= increment_feature_config_addr(dev); + index = index + dev->read_write_len; + } + if (overflow) + rslt |= write_regs(BMA4_FEATURE_CONFIG_ADDR, data + index, + overflow, dev); + rslt |= set_feature_config_start_addr(dev); + } else { + rslt = write_regs(BMA4_FEATURE_CONFIG_ADDR, data, len, dev); + } + } else { + rslt = BMA4_E_RD_WR_LENGTH_INVALID; + } + + if (rslt == BMA4_OK) { + /* Enable APS once write feature config register is done */ + rslt = bma4_get_advance_power_save(&adv_pwr_save, dev); + if (adv_pwr_save == BMA4_DISABLE) { + rslt |= bma4_set_advance_power_save(BMA4_ENABLE, dev); + /* Wait for sensor time synchronization. Refer the data-sheet for + more information*/ + dev->delay(1); + } + } + + } else { + rslt = write_regs(addr, data, len, dev); + } + } + + return rslt; +} + +/*! + * @brief This API writes the 8-bit data to the given register + * in the sensor. + */ +static uint16_t write_regs(uint8_t addr, uint8_t *data, uint8_t len, const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (dev->interface == BMA4_SPI_INTERFACE) + addr = addr & BMA4_SPI_WR_MASK; + /* write data in the register*/ + rslt |= dev->bus_write(dev->dev_addr, addr, data, len); + } + return rslt; +} + +/*! + * @brief This API sets the feature config. data start address in the sensor. + */ +static uint16_t get_feature_config_start_addr(struct bma4_dev *dev) +{ + uint16_t rslt; + uint8_t asic_lsb; + uint8_t asic_msb; + + rslt = read_regs(BMA4_RESERVED_REG_5B_ADDR, &asic_lsb, 1, dev); + rslt |= read_regs(BMA4_RESERVED_REG_5C_ADDR, &asic_msb, 1, dev); + + /* Store asic info in dev structure */ + dev->asic_data.asic_lsb = asic_lsb & 0x0F; + dev->asic_data.asic_msb = asic_msb; + + return rslt; +} + +/*! + * @brief This API sets the feature config. data start address in the sensor. + */ +static uint16_t set_feature_config_start_addr(struct bma4_dev *dev) +{ + uint16_t rslt; + + rslt = write_regs(BMA4_RESERVED_REG_5B_ADDR, &dev->asic_data.asic_lsb, 1, dev); + rslt |= write_regs(BMA4_RESERVED_REG_5C_ADDR, &dev->asic_data.asic_msb, 1, dev); + + return rslt; +} + +/*! + * @brief This API increments the feature config. data address according to the user + * provided read/write length in the dev structure. + */ +static uint16_t increment_feature_config_addr(const struct bma4_dev *dev) +{ + uint16_t rslt; + uint16_t asic_addr; + uint8_t asic_lsb; + uint8_t asic_msb; + + /* Read the asic address from the sensor */ + rslt = read_regs(BMA4_RESERVED_REG_5B_ADDR, &asic_lsb, 1, dev); + rslt |= read_regs(BMA4_RESERVED_REG_5C_ADDR, &asic_msb, 1, dev); + /* Get the asic address */ + asic_addr = (asic_msb << 4) | (asic_lsb & 0x0F); + /* Sum the asic address with read/write length after converting from + byte to word */ + asic_addr = asic_addr + (dev->read_write_len / 2); + /* Split the asic address */ + asic_lsb = asic_addr & 0x0F; + asic_msb = (uint8_t)(asic_addr >> 4); + /* Write the asic address in the sensor */ + rslt |= write_regs(BMA4_RESERVED_REG_5B_ADDR, &asic_lsb, 1, dev); + rslt |= write_regs(BMA4_RESERVED_REG_5C_ADDR, &asic_msb, 1, dev); + + return rslt; +} + +/*! + * @brief This API checks whether the read operation requested is for feature + * or register read and accordingly reads the data from the sensor. + */ +uint16_t bma4_read_regs(uint8_t addr, uint8_t *data, uint8_t len, struct bma4_dev *dev) +{ + uint8_t i; + uint8_t loop_count; + uint8_t overflow; + uint8_t index; + uint16_t rslt = BMA4_OK; + uint8_t adv_pwr_save = 0; + + /* Check the dev structure as NULL*/ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (addr == BMA4_FEATURE_CONFIG_ADDR) { + /* Disable APS if enabled before reading the feature config register */ + rslt = bma4_get_advance_power_save(&adv_pwr_save, dev); + if (adv_pwr_save == BMA4_ENABLE) { + rslt |= bma4_set_advance_power_save(BMA4_DISABLE, dev); + /* Wait for sensor time synchronization. Refer the data-sheet for + more information*/ + dev->delay(1); + } + + if (((len % 2) == 0) && (len <= dev->feature_len) && (rslt == BMA4_OK)) { + if (dev->read_write_len < len) { + /* Calculate the no of writes to be performed according + to the read/write length */ + loop_count = len / dev->read_write_len; + overflow = len % dev->read_write_len; + index = 0; + rslt = set_feature_config_start_addr(dev); + for (i = 0; i < loop_count; i++) { + rslt |= read_regs(BMA4_FEATURE_CONFIG_ADDR, data + index, + dev->read_write_len, dev); + rslt |= increment_feature_config_addr(dev); + index = index + dev->read_write_len; + } + if (overflow) + rslt |= read_regs(BMA4_FEATURE_CONFIG_ADDR, data + index, + overflow, dev); + rslt |= set_feature_config_start_addr(dev); + } else { + rslt = read_regs(BMA4_FEATURE_CONFIG_ADDR, data, len, dev); + } + } else { + rslt = BMA4_E_RD_WR_LENGTH_INVALID; + } + if (rslt == BMA4_OK) { + /* Enable APS once read feature config register is done */ + rslt = bma4_get_advance_power_save(&adv_pwr_save, dev); + if (adv_pwr_save == BMA4_DISABLE) { + rslt |= bma4_set_advance_power_save(BMA4_ENABLE, dev); + /* Wait for sensor time synchronization. Refer the data-sheet for + more information*/ + dev->delay(1); + } + } + } else { + rslt = read_regs(addr, data, len, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the 8-bit data from the given register + * in the sensor. + */ +static uint16_t read_regs(uint8_t addr, uint8_t *data, uint8_t len, const struct bma4_dev *dev) +{ + /* variable used to return the status of communication result*/ + uint16_t rslt = 0; + uint16_t temp_len = len + dev->dummy_byte; + uint16_t i; + + if (dev->interface == BMA4_SPI_INTERFACE) { + /* SPI mask added */ + addr = addr | BMA4_SPI_RD_MASK; + } + + if (temp_len > BMA4_MAX_BUFFER_SIZE) { + /* Buffer size is not sufficient */ + rslt |= BMA4_E_OUT_OF_RANGE; + } + + if (rslt == BMA4_OK) { + /* Read the data from the register */ + rslt |= dev->bus_read(dev->dev_addr, addr, temp_buff, temp_len); + + for (i = 0; i < len; i++) { + /* Parsing and storing the valid data */ + data[i] = temp_buff[i + dev->dummy_byte]; + } + } + + return rslt; +} + +/*! + * @brief This API reads the error status from the sensor. + */ +uint16_t bma4_get_error_status(struct bma4_err_reg *err_reg, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the error codes*/ + rslt |= bma4_read_regs(BMA4_ERROR_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + /* Fatal error*/ + err_reg->fatal_err = BMA4_GET_BITS_POS_0(data, BMA4_FATAL_ERR); + /* Cmd error*/ + err_reg->cmd_err = BMA4_GET_BITSLICE(data, BMA4_CMD_ERR); + /* User error*/ + err_reg->err_code = BMA4_GET_BITSLICE(data, BMA4_ERR_CODE); + /* FIFO error*/ + err_reg->fifo_err = BMA4_GET_BITSLICE(data, BMA4_FIFO_ERR); + /* Mag data ready error*/ + err_reg->aux_err = BMA4_GET_BITSLICE(data, BMA4_AUX_ERR); + } + } + + return rslt; +} + +/*! + * @brief This API reads the sensor status from the sensor. + */ +uint16_t bma4_get_status(uint8_t *status, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the error codes*/ + rslt |= bma4_read_regs(BMA4_STATUS_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *status = data; + } + + return rslt; +} + +/*! + * @brief This API reads the Accel data for x,y and z axis from the sensor. + * The data units is in LSB format. + */ +uint16_t bma4_read_accel_xyz(struct bma4_accel *accel, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint16_t lsb = 0; + uint16_t msb = 0; + uint8_t data[BMA4_ACCEL_DATA_LENGTH] = {0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_DATA_8_ADDR, data, BMA4_ACCEL_DATA_LENGTH, dev); + + if (rslt == BMA4_OK) { + msb = data[1]; + lsb = data[0]; + /* Accel data x axis */ + accel->x = (int16_t)((msb << 8) | lsb); + + msb = data[3]; + lsb = data[2]; + /* Accel data y axis */ + accel->y = (int16_t)((msb << 8) | lsb); + + msb = data[5]; + lsb = data[4]; + /* Accel data z axis */ + accel->z = (int16_t)((msb << 8) | lsb); + + if (dev->resolution == BMA4_12_BIT_RESOLUTION) { + accel->x = (accel->x / 0x10); + accel->y = (accel->y / 0x10); + accel->z = (accel->z / 0x10); + } else if (dev->resolution == BMA4_14_BIT_RESOLUTION) { + accel->x = (accel->x / 0x04); + accel->y = (accel->y / 0x04); + accel->z = (accel->z / 0x04); + } + } + } + + return rslt; +} + +/*! + * @brief This API reads the sensor time of Sensor time gets updated + * with every update of data register or FIFO. + */ +uint16_t bma4_get_sensor_time(uint32_t *sensor_time, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[BMA4_SENSOR_TIME_LENGTH] = {0}; + uint8_t msb = 0; + uint8_t xlsb = 0; + uint8_t lsb = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_SENSORTIME_0_ADDR, data, BMA4_SENSOR_TIME_LENGTH, dev); + + if (rslt == BMA4_OK) { + msb = data[BMA4_SENSOR_TIME_MSB_BYTE]; + xlsb = data[BMA4_SENSOR_TIME_XLSB_BYTE]; + lsb = data[BMA4_SENSOR_TIME_LSB_BYTE]; + *sensor_time = (uint32_t)((msb << 16) | (xlsb << 8) | lsb); + } + } + + return rslt; +} + +/*! + * @brief This API reads the chip temperature of sensor. + * + * @note Using a scaling factor of 1000, to obtain integer values, which + * at the user end, are used to get accurate temperature value . + * BMA4_FAHREN_SCALED = 1.8 * 1000, BMA4_KELVIN_SCALED = 273.15 * 1000 + */ +uint16_t bma4_get_temperature(int32_t *temp, uint8_t temp_unit, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[BMA4_TEMP_DATA_SIZE] = {0}; + int32_t temp_raw_scaled = 0; + + /* Check for Null pointer error */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read temperature value from the register */ + rslt |= bma4_read_regs(BMA4_TEMPERATURE_ADDR, data, BMA4_TEMP_DATA_SIZE, dev); + + if (rslt == BMA4_OK) + temp_raw_scaled = (int32_t)data[BMA4_TEMP_BYTE] * BMA4_SCALE_TEMP; + + /* '0' value read from the register corresponds to 23 degree C */ + (*temp) = temp_raw_scaled + (BMA4_OFFSET_TEMP * BMA4_SCALE_TEMP); + + switch (temp_unit) { + case BMA4_DEG: + break; + + case BMA4_FAHREN: + /* Temperature in degree Fahrenheit */ + (*temp) = (((*temp) / BMA4_SCALE_TEMP) * BMA4_FAHREN_SCALED) + (32 * BMA4_SCALE_TEMP); + break; + + case BMA4_KELVIN: + /* Temperature in degree Kelvin */ + (*temp) = (*temp) + BMA4_KELVIN_SCALED; + break; + + default: + break; + } + } + + return rslt; +} + +/*! + * @brief This API reads the Output data rate, Bandwidth, perf_mode + * and Range of accel. + */ +uint16_t bma4_get_accel_config(struct bma4_accel_config *accel, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_ACCEL_CONFIG_ADDR, data, BMA4_ACCEL_CONFIG_LENGTH, dev); + + if (rslt == BMA4_OK) { + /* To get the ODR */ + accel->odr = BMA4_GET_BITS_POS_0(data[0], BMA4_ACCEL_ODR); + /* To get the bandwidth */ + accel->bandwidth = BMA4_GET_BITSLICE(data[0], BMA4_ACCEL_BW); + /* To get the under sampling mode */ + accel->perf_mode = BMA4_GET_BITSLICE(data[0], BMA4_ACCEL_PERFMODE); + /* Read the Accel range */ + accel->range = BMA4_GET_BITS_POS_0(data[1], BMA4_ACCEL_RANGE); + } + } + + return rslt; +} + +/*! + * @brief This API sets the output_data_rate, bandwidth, perf_mode + * and range of Accel. + */ +uint16_t bma4_set_accel_config(const struct bma4_accel_config *accel, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t accel_config_data[2] = {0, 0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* check whether the bandwidth and perfmode + settings are valid */ + rslt = validate_bandwidth_perfmode(accel->bandwidth, accel->perf_mode); + + if (rslt == BMA4_OK) { + /* check ODR is valid */ + rslt |= validate_odr(accel->odr); + + if (rslt == BMA4_OK) { + accel_config_data[0] = accel->odr & BMA4_ACCEL_ODR_MSK; + accel_config_data[0] |= (uint8_t)(accel->bandwidth << BMA4_ACCEL_BW_POS); + accel_config_data[0] |= (uint8_t)(accel->perf_mode << BMA4_ACCEL_PERFMODE_POS); + accel_config_data[1] = accel->range & BMA4_ACCEL_RANGE_MSK; + + /* Burst write is not possible in + suspend mode hence individual write is + used with delay of 1 ms */ + rslt |= bma4_write_regs(BMA4_ACCEL_CONFIG_ADDR, &accel_config_data[0], 1, dev); + dev->delay(BMA4_GEN_READ_WRITE_DELAY); + rslt |= bma4_write_regs((BMA4_ACCEL_CONFIG_ADDR + 1), &accel_config_data[1], + 1, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API validates the bandwidth and perf_mode + * value set by the user. + */ +static uint16_t validate_bandwidth_perfmode(uint8_t bandwidth, uint8_t perf_mode) +{ + uint16_t rslt = BMA4_OK; + + if (perf_mode == BMA4_CONTINUOUS_MODE) { + if (bandwidth > BMA4_ACCEL_NORMAL_AVG4) { + /* Invalid bandwidth error for continuous mode */ + rslt = BMA4_E_OUT_OF_RANGE; + } + } else if (perf_mode == BMA4_CIC_AVG_MODE) { + if (bandwidth > BMA4_ACCEL_RES_AVG128) { + /* Invalid bandwidth error for CIC avg. mode */ + rslt = BMA4_E_OUT_OF_RANGE; + } + } else { + rslt = BMA4_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API validates the ODR value set by the user. + */ +static uint16_t validate_odr(uint8_t odr) +{ + uint16_t rslt = BMA4_OK; + + if ((odr < BMA4_OUTPUT_DATA_RATE_0_78HZ) || (odr > BMA4_OUTPUT_DATA_RATE_1600HZ)) { + /* If odr is not valid return error */ + rslt = BMA4_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API sets the advance power save mode in the sensor. + */ +uint16_t bma4_set_advance_power_save(uint8_t adv_pwr_save, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + if (rslt == BMA4_OK) { + data = BMA4_SET_BITS_POS_0(data, BMA4_ADVANCE_POWER_SAVE, adv_pwr_save); + rslt |= bma4_write_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the status of advance power save mode + * from the sensor. + */ +uint16_t bma4_get_advance_power_save(uint8_t *adv_pwr_save, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *adv_pwr_save = BMA4_GET_BITS_POS_0(data, BMA4_ADVANCE_POWER_SAVE); + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO self wake up functionality in the sensor. + */ +uint16_t bma4_set_fifo_self_wakeup(uint8_t fifo_self_wakeup, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_FIFO_SELF_WAKE_UP, fifo_self_wakeup); + rslt |= bma4_write_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of FIFO self wake up functionality from + * the sensor. + */ +uint16_t bma4_get_fifo_self_wakeup(uint8_t *fifo_self_wake_up, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *fifo_self_wake_up = BMA4_GET_BITSLICE(data, BMA4_FIFO_SELF_WAKE_UP); + } + + return rslt; +} + +/*! + * @brief This API enables or disables the Accel in the sensor. + */ +uint16_t bma4_set_accel_enable(uint8_t accel_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_ACCEL_ENABLE, accel_en); + rslt |= bma4_write_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API checks whether Accel is enabled or not in the sensor. + */ +uint16_t bma4_get_accel_enable(uint8_t *accel_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *accel_en = BMA4_GET_BITSLICE(data, BMA4_ACCEL_ENABLE); + } + + return rslt; +} + +/*! + * @brief This API is used to enable or disable auxiliary Mag + * in the sensor. + */ +uint16_t bma4_set_mag_enable(uint8_t mag_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITS_POS_0(data, BMA4_MAG_ENABLE, mag_en); + rslt |= bma4_write_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to check whether the auxiliary Mag is enabled + * or not in the sensor. + */ +uint16_t bma4_get_mag_enable(uint8_t *mag_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_POWER_CTRL_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_en = BMA4_GET_BITS_POS_0(data, BMA4_MAG_ENABLE); + } + + return rslt; +} + +/*! + * @brief This API reads the SPI interface mode which is set for primary + * interface. + */ +uint16_t bma4_get_spi_interface(uint8_t *spi, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read SPI mode */ + rslt |= bma4_read_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *spi = BMA4_GET_BITS_POS_0(data, BMA4_CONFIG_SPI3); + } + + return rslt; +} + +/*! + * @brief This API configures the SPI interface Mode for primary interface + */ +uint16_t bma4_set_spi_interface(uint8_t spi, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (spi <= BMA4_MAX_VALUE_SPI3) { + /* Write SPI mode */ + rslt |= bma4_read_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITS_POS_0(data, BMA4_CONFIG_SPI3, spi); + rslt |= bma4_write_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + } + } else { + rslt |= BMA4_E_OUT_OF_RANGE; + } + } + + return rslt; +} + +/*! +* @brief This API writes the available sensor specific commands +* to the sensor. +*/ +uint16_t bma4_set_command_register(uint8_t command_reg, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write command register */ + rslt |= bma4_write_regs(BMA4_CMD_ADDR, &command_reg, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API sets the I2C device address of auxiliary sensor + */ +uint16_t bma4_set_i2c_device_addr(struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0, dev_id = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write the auxiliary I2C device address */ + rslt |= bma4_read_regs(BMA4_AUX_DEV_ID_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + dev_id = BMA4_SET_BITSLICE(data, BMA4_I2C_DEVICE_ADDR, dev->aux_config.aux_dev_addr); + rslt |= bma4_write_regs(BMA4_AUX_DEV_ID_ADDR, &dev_id, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API sets the register access on MAG_IF[2], MAG_IF[3], + * MAG_IF[4] in the sensor. This implies that the DATA registers are + * not updated with Mag values automatically. + */ +uint16_t bma4_set_mag_manual_enable(uint8_t mag_manual, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write the Mag manual*/ + rslt |= bma4_read_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + dev->delay(BMA4_GEN_READ_WRITE_DELAY); + + if (rslt == BMA4_OK) { + /* Set the bit of Mag manual enable */ + data = BMA4_SET_BITSLICE(data, BMA4_MAG_MANUAL_ENABLE, mag_manual); + rslt |= bma4_write_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + dev->aux_config.manual_enable = (uint8_t)mag_manual; + } else { + /*dev->mag_manual_enable = 0;*/ + dev->aux_config.manual_enable = 0; + } + } + + return rslt; +} + +/*! + * @brief This API checks whether the Mag access is done manually or + * automatically in the sensor. + * If the Mag access is done through manual mode then Mag data registers + * in sensor are not updated automatically. + */ +uint16_t bma4_get_mag_manual_enable(uint8_t *mag_manual, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read Mag manual */ + rslt |= bma4_read_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_manual = BMA4_GET_BITSLICE(data, BMA4_MAG_MANUAL_ENABLE); + } + + return rslt; +} + +/*! + * @brief This API sets the I2C interface configuration(if) mode + * for auxiliary Mag. + */ +uint16_t bma4_set_aux_if_mode(uint8_t if_mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_IF_CONFIG_IF_MODE, if_mode); + rslt |= bma4_write_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the address of the register of Aux Mag sensor + * where the data to be read. + */ +uint16_t bma4_get_mag_read_addr(uint8_t *mag_read_addr, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_AUX_RD_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_read_addr = BMA4_GET_BITS_POS_0(data, BMA4_READ_ADDR); + } + + return rslt; +} + +/*! + * @brief This API sets the address of the register of Aux Mag sensor + * where the data to be read. + */ +uint16_t bma4_set_mag_read_addr(uint8_t mag_read_addr, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write the Mag read address*/ + rslt |= bma4_write_regs(BMA4_AUX_RD_ADDR, &mag_read_addr, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API gets the Aux Mag write address from the sensor. + * Mag write address is where the Mag data will be written. + */ +uint16_t bma4_get_mag_write_addr(uint8_t *mag_write_addr, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_AUX_WR_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_write_addr = BMA4_GET_BITS_POS_0(data, BMA4_WRITE_ADDR); + } + + return rslt; +} + +/*! + * @brief This API sets the Aux Mag write address in the sensor. + * Mag write address is where the Mag data will be written. + */ +uint16_t bma4_set_mag_write_addr(uint8_t mag_write_addr, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) + rslt |= BMA4_E_NULL_PTR; + else + rslt |= bma4_write_regs(BMA4_AUX_WR_ADDR, &mag_write_addr, 1, dev); + + return rslt; +} + +/*! + * @brief This API reads the data from the sensor which is written to the + * Mag. + */ +uint16_t bma4_get_mag_write_data(uint8_t *mag_write_data, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_AUX_WR_DATA_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_write_data = BMA4_GET_BITS_POS_0(data, BMA4_WRITE_DATA); + } + + return rslt; +} + +/*! + * @brief This API sets the data in the sensor which in turn will + * be written to Mag. + */ +uint16_t bma4_set_mag_write_data(uint8_t mag_write_data, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) + rslt |= BMA4_E_NULL_PTR; + else + rslt |= bma4_write_regs(BMA4_AUX_WR_DATA_ADDR, &mag_write_data, 1, dev); + + return rslt; +} + +/*! + * @brief This API reads the x,y,z and r axis data from the auxiliary + * Mag BMM150/AKM9916 sensor. + */ +uint16_t bma4_read_mag_xyzr(struct bma4_mag_xyzr *mag, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint16_t msb = 0; + uint16_t lsb = 0; + uint8_t data[BMA4_MAG_XYZR_DATA_LENGTH] = {0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_DATA_0_ADDR, data, BMA4_MAG_XYZR_DATA_LENGTH, dev); + + if (rslt == BMA4_OK) { + /* Data X */ + /* X-axis LSB value shifting */ + lsb = BMA4_GET_BITSLICE(data[BMA4_MAG_X_LSB_BYTE], BMA4_DATA_MAG_X_LSB); + msb = data[BMA4_MAG_X_MSB_BYTE]; + mag->x = (int16_t)((msb << 8) | lsb); + mag->x = (mag->x / 0x08); + + /* Data Y */ + /* Y-axis LSB value shifting */ + lsb = BMA4_GET_BITSLICE(data[BMA4_MAG_Y_LSB_BYTE], BMA4_DATA_MAG_Y_LSB); + msb = data[BMA4_MAG_Y_MSB_BYTE]; + mag->y = (int16_t)((msb << 8) | lsb); + mag->y = (mag->y / 0x08); + + /* Data Z */ + /* Z-axis LSB value shifting */ + lsb = BMA4_GET_BITSLICE(data[BMA4_MAG_Z_LSB_BYTE], BMA4_DATA_MAG_Z_LSB); + msb = data[BMA4_MAG_Z_MSB_BYTE]; + mag->z = (int16_t)((msb << 8) | lsb); + mag->z = (mag->z / 0x02); + + /* RHall */ + /* R-axis LSB value shifting */ + lsb = BMA4_GET_BITSLICE(data[BMA4_MAG_R_LSB_BYTE], BMA4_DATA_MAG_R_LSB); + msb = data[BMA4_MAG_R_MSB_BYTE]; + mag->r = (int16_t)((msb << 8) | lsb); + mag->r = (mag->r / 0x04); + } + } + + return rslt; +} + +/*! + * @brief This API sets the burst data length (1,2,6,8 byte) of auxiliary + * Mag sensor. + */ +uint16_t bma4_set_mag_burst(uint8_t mag_burst, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write auxiliary burst mode length*/ + rslt |= bma4_read_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITS_POS_0(data, BMA4_MAG_BURST, mag_burst); + rslt |= bma4_write_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the burst data length of Mag set in the sensor. + */ +uint16_t bma4_get_mag_burst(uint8_t *mag_burst, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write Mag burst mode length*/ + rslt |= bma4_read_regs(BMA4_AUX_IF_CONF_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *mag_burst = BMA4_GET_BITS_POS_0(data, BMA4_MAG_BURST); + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO data of Accel and/or Mag sensor + */ +uint16_t bma4_read_fifo_data(struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + uint8_t addr = BMA4_FIFO_DATA_ADDR; + /* check the bma4 structure as NULL*/ + if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + reset_fifo_data_structure(dev); + /* read FIFO data*/ + if (dev->interface == BMA4_SPI_INTERFACE) + addr = addr | BMA4_SPI_RD_MASK; + + rslt |= dev->bus_read(dev->dev_addr, addr, dev->fifo->data, dev->fifo->length); + /* read fifo frame content configuration*/ + rslt |= bma4_read_regs(BMA4_FIFO_CONFIG_1_ADDR, &data, 1, dev); + /* filter fifo header enabled status */ + dev->fifo->fifo_header_enable = data & BMA4_FIFO_HEADER; + /* filter accel/mag data enabled status */ + dev->fifo->fifo_data_enable = data & BMA4_FIFO_M_A_ENABLE; + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the accelerometer frames from + * FIFO data read by the "bma4_read_fifo_data" API and stores it in the + * "accel_data" structure instance. + */ +uint16_t bma4_extract_accel(struct bma4_accel *accel_data, uint16_t *accel_length, const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint16_t data_index = 0; + uint16_t accel_index = 0; + uint16_t data_read_length = 0; + + if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) { + get_accel_len_to_parse(&data_index, &data_read_length, accel_length, dev); + + for (; data_index < data_read_length;) { + unpack_acc_frm(accel_data, &data_index, &accel_index, dev->fifo->fifo_data_enable, dev); + /*Check for the availability of next + two bytes of FIFO data */ + check_empty_fifo(&data_index, dev); + } + /* update number of accel data read*/ + *accel_length = accel_index; + /*update the accel byte index*/ + dev->fifo->accel_byte_start_idx = data_index; + } else { + /* Parsing the FIFO data in header mode */ + extract_accel_header_mode(accel_data, accel_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the magnetometer frames from + * FIFO data read by the "bma4_read_fifo_data" API and stores it in the + * "mag_data" structure instance parameter of this API + */ +uint16_t bma4_extract_mag(struct bma4_mag *mag_data, uint16_t *mag_length, const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint16_t data_index = 0; + uint16_t mag_index = 0; + uint16_t data_read_length = 0; + + if (dev == NULL || dev->fifo == NULL || dev->fifo->data == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Parsing the FIFO data in header-less mode */ + if (dev->fifo->fifo_header_enable == 0) { + get_mag_len_to_parse(&data_index, &data_read_length, mag_length, dev); + for (; data_index < data_read_length;) { + rslt |= unpack_mag_frm(mag_data, &data_index, &mag_index, + dev->fifo->fifo_data_enable, dev); + /*Check for the availability of next + two bytes of FIFO data */ + check_empty_fifo(&data_index, dev); + } + /* update number of Aux. sensor data read*/ + *mag_length = mag_index; + /*update the Aux. sensor frame index*/ + dev->fifo->mag_byte_start_idx = data_index; + } else { + /* Parsing the FIFO data in header mode */ + rslt |= extract_mag_header_mode(mag_data, mag_length, dev); + } + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO water mark level which is set + * in the sensor. + */ +uint16_t bma4_get_fifo_wm(uint16_t *fifo_wm, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0, 0}; + + /* Check the bma4 structure as NULL*/ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the FIFO water mark level*/ + rslt |= bma4_read_regs(BMA4_FIFO_WTM_0_ADDR, data, BMA4_FIFO_WM_LENGTH, dev); + + if (BMA4_OK == rslt) + *fifo_wm = (data[1] << 8) | (data[0]); + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO watermark level in the sensor. + */ +uint16_t bma4_set_fifo_wm(uint16_t fifo_wm, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0, 0}; + + /* Check the bma4 structure as NULL*/ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + data[0] = BMA4_GET_LSB(fifo_wm); + data[1] = BMA4_GET_MSB(fifo_wm); + /* consecutive write is not possible in suspend mode hence + separate write is used with delay of 1 ms*/ + /* Write the fifo watermark level*/ + rslt |= bma4_write_regs(BMA4_FIFO_WTM_0_ADDR, &data[0], 1, dev); + dev->delay(BMA4_GEN_READ_WRITE_DELAY); + rslt |= bma4_write_regs((BMA4_FIFO_WTM_0_ADDR + 1), &data[1], 1, dev); + } + + return rslt; +} + +/*! + * @brief This API checks whether the Accel FIFO data is set for filtered + * or unfiltered mode. + */ +uint16_t bma4_get_accel_fifo_filter_data(uint8_t *accel_fifo_filter, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the Accel FIFO filter data */ + rslt |= bma4_read_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *accel_fifo_filter = BMA4_GET_BITSLICE(data, BMA4_FIFO_FILTER_ACCEL); + } + + return rslt; +} + +/*! + * @brief This API sets the condition of Accel FIFO data either to + * filtered or unfiltered mode. + */ +uint16_t bma4_set_accel_fifo_filter_data(uint8_t accel_fifo_filter, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (accel_fifo_filter <= BMA4_MAX_VALUE_FIFO_FILTER) { + rslt |= bma4_read_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + /* Write Accel FIFO filter data */ + data = BMA4_SET_BITSLICE(data, BMA4_FIFO_FILTER_ACCEL, accel_fifo_filter); + rslt |= bma4_write_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + } + } else { + rslt |= BMA4_E_OUT_OF_RANGE; + } + } + return rslt; +} + +/*! + * @brief This API reads the down sampling rates which is configured + * for Accel FIFO data. + */ +uint16_t bma4_get_fifo_down_accel(uint8_t *fifo_down, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the Accel FIFO down data */ + rslt |= bma4_read_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *fifo_down = BMA4_GET_BITSLICE(data, BMA4_FIFO_DOWN_ACCEL); + } + + return rslt; +} + +/*! + * @brief This API sets the down-sampling rates for Accel FIFO. + */ +uint16_t bma4_set_fifo_down_accel(uint8_t fifo_down, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Write the Accel FIFO down data */ + rslt |= bma4_read_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_FIFO_DOWN_ACCEL, fifo_down); + rslt |= bma4_write_regs(BMA4_FIFO_DOWN_ADDR, &data, 1, dev); + } + } + return rslt; +} + +/*! + * @brief This API reads the length of FIFO data available in the sensor + * in the units of bytes. + */ +uint16_t bma4_get_fifo_length(uint16_t *fifo_length, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t index = 0; + uint8_t data[BMA4_FIFO_DATA_LENGTH] = {0, 0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read FIFO length*/ + rslt |= bma4_read_regs(BMA4_FIFO_LENGTH_0_ADDR, data, BMA4_FIFO_DATA_LENGTH, dev); + + if (rslt == BMA4_OK) { + index = BMA4_FIFO_LENGTH_MSB_BYTE; + data[index] = BMA4_GET_BITS_POS_0(data[index], BMA4_FIFO_BYTE_COUNTER_MSB); + *fifo_length = ((data[index] << 8) | data[index - 1]); + } + } + + return rslt; +} + +/*! + * @brief This API aligns and compensates the Mag data of BMM150/AKM9916 + * sensor. + */ +uint16_t bma4_second_if_mag_compensate_xyz(struct bma4_mag_fifo_data mag_fifo_data, + uint8_t mag_second_if, struct bma4_mag *compensated_mag_data) +{ + uint16_t rslt = 0; +#ifdef BMM150 + int16_t mag_x = 0; + int16_t mag_y = 0; + int16_t mag_z = 0; + uint16_t mag_r = 0; +#endif + + switch (mag_second_if) { +#ifdef BMM150 + case BMA4_SEC_IF_BMM150: + /* X data*/ + mag_x = (int16_t)((mag_fifo_data.mag_x_msb << 8) | (mag_fifo_data.mag_x_lsb)); + mag_x = (int16_t) (mag_x / 0x08); + + /* Y data*/ + mag_y = (int16_t)((mag_fifo_data.mag_y_msb << 8) | (mag_fifo_data.mag_y_lsb)); + mag_y = (int16_t) (mag_y / 0x08); + + /* Z data*/ + mag_z = (int16_t)((mag_fifo_data.mag_z_msb << 8) | (mag_fifo_data.mag_z_lsb)); + mag_z = (int16_t) (mag_z / 0x02); + + /* R data*/ + mag_r = (uint16_t)((mag_fifo_data.mag_r_y2_msb << 8) | (mag_fifo_data.mag_r_y2_lsb)); + mag_r = (uint16_t) (mag_r >> 2); + + /* Compensated Mag x data */ + compensated_mag_data->x = bma4_bmm150_mag_compensate_X(mag_x, mag_r); + + /* Compensated Mag y data */ + compensated_mag_data->y = bma4_bmm150_mag_compensate_Y(mag_y, mag_r); + + /* Compensated Mag z data */ + compensated_mag_data->z = bma4_bmm150_mag_compensate_Z(mag_z, mag_r); + break; +#endif + +#ifdef AKM9916 + + case BMA4_SEC_IF_AKM09916: + /* Compensated X data */ + compensated_mag_data->x = (int16_t)((mag_fifo_data.mag_x_msb << 8) | (mag_fifo_data.mag_x_lsb)); + /* Compensated Y data*/ + compensated_mag_data->y = (int16_t)((mag_fifo_data.mag_y_msb << 8) | (mag_fifo_data.mag_y_lsb)); + /* Compensated Z data*/ + compensated_mag_data->z = (int16_t)((mag_fifo_data.mag_z_msb << 8) | (mag_fifo_data.mag_z_lsb)); + break; + +#endif + default: + rslt |= BMA4_E_OUT_OF_RANGE; + break; + } + return rslt; +} + +/*! + * @brief This API reads Mag. x,y and z axis data from either BMM150 or + * AKM9916 sensor + */ +uint16_t bma4_read_mag_xyz(struct bma4_mag *mag, uint8_t sensor_select, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + +#if defined(AKM9916) || defined(BMM150) + uint8_t index; + uint16_t msb = 0; + uint16_t lsb = 0; + uint8_t data[BMA4_MAG_XYZ_DATA_LENGTH] = {0}; +#endif + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + switch (sensor_select) { + +#if defined(BMM150) + + case BMA4_SEC_IF_BMM150: + rslt |= bma4_read_regs(BMA4_DATA_0_ADDR, data, BMA4_MAG_XYZ_DATA_LENGTH, dev); + + if (rslt == BMA4_OK) { + index = BMA4_MAG_X_LSB_BYTE; + /*X-axis LSB value shifting*/ + data[index] = BMA4_GET_BITSLICE(data[index], BMA4_DATA_MAG_X_LSB); + /* Data X */ + msb = data[index + 1]; + lsb = data[index]; + mag->x = (int16_t)((msb << 8) | lsb); + mag->x = (mag->x / 0x08); + + /* Data Y */ + /*Y-axis LSB value shifting*/ + data[index + 2] = BMA4_GET_BITSLICE(data[index + 2], BMA4_DATA_MAG_Y_LSB); + msb = data[index + 3]; + lsb = data[index + 2]; + mag->y = (int16_t)((msb << 8) | lsb); + mag->y = (mag->y / 0x08); + + /* Data Z */ + /*Z-axis LSB value shifting*/ + data[index + 4] = BMA4_GET_BITSLICE(data[index + 4], BMA4_DATA_MAG_Z_LSB); + msb = data[index + 5]; + lsb = data[index + 4]; + mag->z = (int16_t)((msb << 8) | lsb); + mag->z = (mag->z / 0x02); + } + break; +#endif + +#if defined(AKM9916) + case BMA4_SEC_IF_AKM09916: + + if (AKM9916_SENSOR == dev->aux_sensor) { + rslt |= bma4_read_regs(BMA4_DATA_0_ADDR, data, BMA4_MAG_XYZ_DATA_LENGTH, dev); + + if (rslt == BMA4_OK) { + index = BMA4_MAG_X_LSB_BYTE; + /* Data X */ + msb = data[index + 1]; + lsb = data[index]; + mag->x = (int16_t)((msb << 8) | lsb); + + /* Data Y */ + msb = data[index + 3]; + lsb = data[index + 2]; + mag->y = (int32_t)((msb << 8) | lsb); + + /* Data Z */ + msb = data[index + 5]; + lsb = data[index + 4]; + mag->z = (int16_t)((msb << 8) | lsb); + } + } + break; +#endif + + default: + rslt |= BMA4_E_OUT_OF_RANGE; + break; + } + } + + return rslt; +} + +/*! + * @brief This API reads the auxiliary I2C interface configuration which + * is set in the sensor. + */ +uint16_t bma4_get_if_mode(uint8_t *if_mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read auxiliary interface configuration */ + rslt |= bma4_read_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *if_mode = BMA4_GET_BITSLICE(data, BMA4_IF_CONFIG_IF_MODE); + } + + return rslt; +} + +/*! + * @brief This API sets the auxiliary interface configuration in the + * sensor. + */ +uint16_t bma4_set_if_mode(uint8_t if_mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (if_mode <= BMA4_MAX_IF_MODE) { + /* Write the interface configuration mode */ + rslt |= bma4_read_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_IF_CONFIG_IF_MODE, if_mode); + rslt |= bma4_write_regs(BMA4_IF_CONFIG_ADDR, &data, 1, dev); + } + } else { + rslt |= BMA4_E_OUT_OF_RANGE; + } + } + + return rslt; +} + +/*! + * @brief This API reads the data ready status of Accel from the sensor. + */ +uint16_t bma4_get_accel_data_rdy(uint8_t *data_rdy, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /*Reads the status of Accel data ready*/ + rslt |= bma4_read_regs(BMA4_STATUS_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *data_rdy = BMA4_GET_BITSLICE(data, BMA4_STAT_DATA_RDY_ACCEL); + } + + return rslt; +} + +/*! + * @brief This API reads the data ready status of Mag from the sensor. + * The status get reset when Mag data register is read. + */ +uint16_t bma4_get_mag_data_rdy(uint8_t *data_rdy, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /*Reads the status of Accel data ready*/ + rslt |= bma4_read_regs(BMA4_STATUS_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) + *data_rdy = BMA4_GET_BITSLICE(data, BMA4_STAT_DATA_RDY_MAG); + } + + return rslt; +} + +/*! + * @brief This API reads the ASIC status from the sensor. + * The status information is mentioned in the below table. + */ +uint16_t bma4_get_asic_status(struct bma4_asic_status *asic_status, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the Mag I2C device address*/ + rslt |= bma4_read_regs(BMA4_INTERNAL_ERROR, &data, 1, dev); + + if (rslt == BMA4_OK) { + asic_status->sleep = (data & 0x01); + asic_status->irq_ovrn = ((data & 0x02) >> 0x01); + asic_status->wc_event = ((data & 0x04) >> 0x02); + asic_status->stream_transfer_active = ((data & 0x08) >> 0x03); + } + } + + return rslt; +} + +/*! + * @brief This API enables the offset compensation for filtered and + * unfiltered Accel data. + */ +uint16_t bma4_set_offset_comp(uint8_t offset_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_NV_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + /* Write Accel FIFO filter data */ + data = BMA4_SET_BITSLICE(data, BMA4_NV_ACCEL_OFFSET, offset_en); + rslt |= bma4_write_regs(BMA4_NV_CONFIG_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of Accel offset compensation + */ +uint16_t bma4_get_offset_comp(uint8_t *offset_en, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_NV_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + /* Write Accel FIFO filter data */ + *offset_en = BMA4_GET_BITSLICE(data, BMA4_NV_ACCEL_OFFSET); + } + } + + return rslt; +} + +/*! + * @brief This API performs Fast Offset Compensation for Accel. + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + */ +uint16_t bma4_perform_accel_foc(const int32_t accel_g_value[3], struct bma4_dev *dev) +{ + uint16_t rslt = 0; + struct bma4_accel accel_value[10] = { {0} }; + struct accel_offset offset = {0}; + struct offset_delta delta = { {0} }; + struct bma4_accel_config acc_conf = {0}; + uint8_t accel_en = 0; + uint8_t adv_pwr_save = 0; + uint8_t range = 0; + uint16_t lsb_per_g = 0; + struct accel_temp temp = {0}; + struct bma4_accel avg = {0}; + struct bma4_accel accel_data = {0}; + uint8_t i = 0; + + /* used to validate user input */ + rslt |= validate_user_input(accel_g_value); + + if (BMA4_OK == rslt) { + /* Configure accel config, accel enable and + advance power save for FOC */ + rslt |= foc_config(&acc_conf, &accel_en, &adv_pwr_save, dev); + + /*TO DO: Check for data ready status before + reading accel values*/ + + if (BMA4_OK == rslt) { + /* Giving a delay of 20ms before reading accel data + since odr is configured as 50Hz */ + for (i = 0; i < 10; i++) { + dev->delay(20); + rslt |= bma4_read_accel_xyz(&accel_value[i], dev); + temp.x = temp.x + (int32_t)accel_value[i].x; + temp.y = temp.y + (int32_t)accel_value[i].y; + temp.z = temp.z + (int32_t)accel_value[i].z; + } + + /* Take average of x, y and z data for lesser noise */ + avg.x = (int16_t)(temp.x / 10); + avg.y = (int16_t)(temp.y / 10); + avg.z = (int16_t)(temp.z / 10); + + /* Copy average value in another structure */ + accel_data = avg; + + if (BMA4_OK == rslt) { + /* Get the exact range value */ + map_range(acc_conf.range, &range); + /* Get LSB per bit given the range and resolution */ + lsb_per_g = (uint16_t)(power(2, dev->resolution) / (2 * range)); + /* Compensate accel data against gravity */ + comp_for_grvty(lsb_per_g, accel_g_value, &accel_data, &delta); + /* scale according to offset register resolution*/ + scale_offset(dev->resolution, range, &delta, &offset); + /* normalise the data with offset*/ + normalise_offset(&delta, &offset); + + /* offset values are written in the offset register */ + rslt |= bma4_write_regs(BMA4_OFFSET_0_ADDR, (uint8_t *)&offset.x, 1, dev); + rslt |= bma4_write_regs(BMA4_OFFSET_1_ADDR, (uint8_t *)&offset.y, 1, dev); + rslt |= bma4_write_regs(BMA4_OFFSET_2_ADDR, (uint8_t *)&offset.z, 1, dev); + + /* Enable offset compensation */ + rslt |= bma4_set_offset_comp(BMA4_ENABLE, dev); + + /* Set accel config, accel enable and advance power save */ + rslt |= bma4_set_accel_config(&acc_conf, dev); + rslt |= bma4_set_accel_enable(accel_en, dev); + rslt |= bma4_set_advance_power_save(adv_pwr_save, dev); + } else { + rslt |= BMA4_E_FOC_FAIL; + } + } + } + return rslt; +} + +/*! + * @brief This API checks whether the self test functionality of the sensor + * is working or not. + * The following parameter of struct bma4_dev, should have the valid value before + * performing the Self test, + * 1. Variant and 2. Resolution + */ +uint16_t bma4_perform_accel_selftest(uint8_t *result, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + struct bma4_accel positive = {0}; + struct bma4_accel negative = {0}; + /*! Structure for difference of accel values in g*/ + struct selftest_delta_limit accel_data_diff = {0}; + /*! Structure for difference of accel values in mg*/ + struct selftest_delta_limit accel_data_diff_mg = {0}; + + *result = BMA4_SELFTEST_FAIL; + + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + rslt = set_accel_selftest_config(dev); + dev->delay(20); + rslt |= bma4_selftest_config(BMA4_ENABLE, dev); + + if (rslt == BMA4_OK) { + dev->delay(100); + rslt = bma4_read_accel_xyz(&positive, dev); + rslt |= bma4_selftest_config(BMA4_DISABLE, dev); + + if (rslt == BMA4_OK) { + dev->delay(100); + rslt = bma4_read_accel_xyz(&negative, dev); + + accel_data_diff.x = ABS(positive.x) + ABS(negative.x); + accel_data_diff.y = ABS(positive.y) + ABS(negative.y); + accel_data_diff.z = ABS(positive.z) + ABS(negative.z); + + /*! Converting LSB of the differences of accel values to mg*/ + convert_lsb_g(&accel_data_diff, &accel_data_diff_mg, dev); + /*! Validating self test for accel values in mg*/ + rslt |= validate_selftest(&accel_data_diff_mg, dev); + + if (rslt == BMA4_OK) + *result = BMA4_SELFTEST_PASS; + + /* Triggers a soft reset */ + rslt |= bma4_set_command_register(0xB6, dev); + dev->delay(200); + } + } + } + + return rslt; +} + +/*! + * @brief This API performs the steps needed for Self test operation + * before reading the Accel Self test data. + */ +uint16_t bma4_selftest_config(uint8_t sign, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + rslt |= set_accel_selftest_enable(BMA4_ENABLE, dev); + rslt |= set_accel_selftest_sign(sign, dev); + + /* Set self test amplitude based on variant */ + switch (dev->variant) { + case BMA42X_VARIANT: + /* Set self test amplitude to high for BMA42x */ + rslt |= set_accel_selftest_amp(BMA4_ENABLE, dev); + break; + + case BMA45X_VARIANT: + /* Set self test amplitude to low for BMA45x */ + rslt |= set_accel_selftest_amp(BMA4_DISABLE, dev); + break; + + default: + rslt = BMA4_E_INVALID_SENSOR; + break; + } + + return rslt; +} + +/*! + * @brief API sets the interrupt to either interrupt1 or + * interrupt2 pin in the sensor. + */ +uint16_t bma4_map_interrupt(uint8_t int_line, uint16_t int_map, uint8_t enable, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[3] = {0, 0, 0}; + uint8_t index[2] = {BMA4_INT_MAP_1_ADDR, BMA4_INT_MAP_2_ADDR}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + rslt |= bma4_read_regs(BMA4_INT_MAP_1_ADDR, data, 3, dev); + + if (enable == TRUE) { + /* Feature interrupt mapping */ + data[int_line] |= (uint8_t)(int_map & (0x00FF)); + /* Hardware interrupt mapping */ + if (int_line == BMA4_INTR2_MAP) + data[2] |= (uint8_t)((int_map & (0xFF00)) >> 4); + else + data[2] |= (uint8_t)((int_map & (0xFF00)) >> 8); + + rslt |= bma4_write_regs(index[int_line], &data[int_line], 1, dev); + rslt |= bma4_write_regs(BMA4_INT_MAP_DATA_ADDR, &data[2], 1, dev); + + } else { + /* Feature interrupt un-mapping */ + data[int_line] &= (~(uint8_t)(int_map & (0x00FF))); + /* Hardware interrupt un-mapping */ + if (int_line == BMA4_INTR2_MAP) + data[2] &= (~(uint8_t)((int_map & (0xFF00)) >> 4)); + else + data[2] &= (~(uint8_t)((int_map & (0xFF00)) >> 8)); + + rslt |= bma4_write_regs(index[int_line], &data[int_line], 1, dev); + rslt |= bma4_write_regs(BMA4_INT_MAP_DATA_ADDR, &data[2], 1, dev); + + } + } + + return rslt; +} + +/*! + * @brief This API sets the interrupt mode in the sensor. + */ +uint16_t bma4_set_interrupt_mode(uint8_t mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + if (mode == BMA4_NON_LATCH_MODE || mode == BMA4_LATCH_MODE) + rslt |= bma4_write_regs(BMA4_INTR_LATCH_ADDR, &mode, 1, dev); + else + rslt |= BMA4_E_OUT_OF_RANGE; + } + + return rslt; +} + +/*! + * @brief This API gets the interrupt mode which is set in the sensor. + */ +uint16_t bma4_get_interrupt_mode(uint8_t *mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_INTR_LATCH_ADDR, &data, 1, dev); + *mode = data; + } + + return rslt; + +} + +/*! + * @brief This API sets the auxiliary Mag(BMM150 or AKM9916) output data + * rate and offset. + */ +uint16_t bma4_set_aux_mag_config(const struct bma4_aux_mag_config *aux_mag, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if ((aux_mag->odr >= BMA4_OUTPUT_DATA_RATE_0_78HZ) && + (aux_mag->odr <= BMA4_OUTPUT_DATA_RATE_1600HZ) + && ((aux_mag->offset & BMA4_MAG_CONFIG_OFFSET_MSK) == 0x00)) { + data = (uint8_t)(aux_mag->odr | + ((aux_mag->offset << + BMA4_MAG_CONFIG_OFFSET_POS))); + rslt |= bma4_write_regs(BMA4_AUX_CONFIG_ADDR, &data, 1, dev); + } else { + rslt |= BMA4_E_OUT_OF_RANGE; + } + } + + return rslt; +} + +/*! + * @brief This API reads the auxiliary Mag(BMM150 or AKM9916) output data + * rate and offset. + */ +uint16_t bma4_get_aux_mag_config(struct bma4_aux_mag_config *aux_mag, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_AUX_CONFIG_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + aux_mag->odr = (data & 0x0F); + aux_mag->offset = (data & BMA4_MAG_CONFIG_OFFSET_MSK) >> 4; + } + } + + return rslt; +} + +/*! @brief This API sets the FIFO configuration in the sensor. + */ +uint16_t bma4_set_fifo_config(uint8_t config, uint8_t enable, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0, 0}; + uint8_t fifo_config_0 = config & BMA4_FIFO_CONFIG_0_MASK; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_FIFO_CONFIG_0_ADDR, data, BMA4_FIFO_CONFIG_LENGTH, dev); + + if (rslt == BMA4_OK) { + + if (fifo_config_0 > 0) { + + if (enable == TRUE) + data[0] = data[0] | fifo_config_0; + else + data[0] = data[0] & (~fifo_config_0); + } + + if (enable == TRUE) + data[1] = data[1] | (config & BMA4_FIFO_CONFIG_1_MASK); + else + data[1] = data[1] & (~(config & BMA4_FIFO_CONFIG_1_MASK)); + + /* Burst write is not possible in suspend mode hence + separate write is used with delay of 1 ms*/ + rslt |= bma4_write_regs(BMA4_FIFO_CONFIG_0_ADDR, &data[0], 1, dev); + dev->delay(BMA4_GEN_READ_WRITE_DELAY); + rslt |= bma4_write_regs((BMA4_FIFO_CONFIG_0_ADDR + 1), &data[1], 1, dev); + } + } + + return rslt; +} + +/*! @brief This API reads the FIFO configuration from the sensor. + */ +uint16_t bma4_get_fifo_config(uint8_t *fifo_config, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0, 0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_FIFO_CONFIG_0_ADDR, data, BMA4_FIFO_CONFIG_LENGTH, dev); + + if (rslt == BMA4_OK) + *fifo_config = ((uint8_t)((data[0] & BMA4_FIFO_CONFIG_0_MASK) | (data[1]))); + + } + + return rslt; +} + +/*! @brief This function sets the electrical behaviour of interrupt pin1 or + * pin2 in the sensor. + */ +uint16_t bma4_set_int_pin_config(const struct bma4_int_pin_config *int_pin_config, uint8_t int_line, + struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t interrupt_address_array[2] = {BMA4_INT1_IO_CTRL_ADDR, BMA4_INT2_IO_CTRL_ADDR}; + uint8_t data = 0; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (int_line <= 1) { + data = ((uint8_t)((int_pin_config->edge_ctrl & BMA4_INT_EDGE_CTRL_MASK) | + ((int_pin_config->lvl << 1) & BMA4_INT_LEVEL_MASK) | + ((int_pin_config->od << 2) & BMA4_INT_OPEN_DRAIN_MASK) | + ((int_pin_config->output_en << 3) & BMA4_INT_OUTPUT_EN_MASK) | + ((int_pin_config->input_en << 4) & BMA4_INT_INPUT_EN_MASK))); + + rslt |= bma4_write_regs(interrupt_address_array[int_line], &data, 1, dev); + } else { + rslt |= BMA4_E_INT_LINE_INVALID; + } + } + + return rslt; +} + +/*! @brief This API reads the electrical behavior of interrupt pin1 or pin2 + * from the sensor. + */ +uint16_t bma4_get_int_pin_config(struct bma4_int_pin_config *int_pin_config, uint8_t int_line, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t interrupt_address_array[2] = {BMA4_INT1_IO_CTRL_ADDR, BMA4_INT2_IO_CTRL_ADDR}; + uint8_t data = 0; + + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + if (int_line <= 1) { + rslt |= bma4_read_regs(interrupt_address_array[int_line], &data, 1, dev); + /* Assign interrupt configurations to the + structure members*/ + if (rslt == BMA4_OK) { + int_pin_config->edge_ctrl = data & BMA4_INT_EDGE_CTRL_MASK; + int_pin_config->lvl = ((data & BMA4_INT_LEVEL_MASK) >> BMA4_INT_LEVEL_POS); + int_pin_config->od = ((data & BMA4_INT_OPEN_DRAIN_MASK) >> BMA4_INT_OPEN_DRAIN_POS); + int_pin_config->output_en = ((data & BMA4_INT_OUTPUT_EN_MASK) + >> BMA4_INT_OUTPUT_EN_POS); + int_pin_config->input_en = ((data & BMA4_INT_INPUT_EN_MASK) >> BMA4_INT_INPUT_EN_POS); + } + } else { + rslt |= BMA4_E_INT_LINE_INVALID; + } + } + + return rslt; +} + +/*! + * @brief This API reads the Feature and Hardware interrupt status from the sensor. + */ +uint16_t bma4_read_int_status(uint16_t *int_status, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data[2] = {0}; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + rslt |= bma4_read_regs(BMA4_INT_STAT_0_ADDR, data, 2, dev); + + if (rslt == BMA4_OK) { + *int_status = data[0]; + *((uint8_t *)int_status + 1) = data[1]; + } + } + + return rslt; +} + +/*! + * @brief This API reads the Feature interrupt status from the sensor. + */ +uint16_t bma4_read_int_status_0(uint8_t *int_status_0, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + /* Null pointer check */ + rslt = BMA4_E_NULL_PTR; + } else { + rslt = bma4_read_regs(BMA4_INT_STAT_0_ADDR, int_status_0, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API reads the Hardware interrupt status from the sensor. + */ +uint16_t bma4_read_int_status_1(uint8_t *int_status_1, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + /* Null pointer check */ + rslt = BMA4_E_NULL_PTR; + } else { + rslt = bma4_read_regs(BMA4_INT_STAT_1_ADDR, int_status_1, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API initializes the auxiliary interface to access + * auxiliary sensor + */ +uint16_t bma4_aux_interface_init(struct bma4_dev *dev) +{ + /* Variable to return error codes */ + uint16_t rslt = BMA4_OK; + + /* Check for Null pointer error */ + rslt |= bma4_null_pointer_check(dev); + if (rslt == BMA4_OK) { + /* Set the auxiliary sensor configuration */ + rslt = bma4_set_aux_interface_config(dev); + if (rslt != BMA4_OK) + rslt = BMA4_E_AUX_CONFIG_FAIL; + } + + return rslt; +} + +/*! + * @brief This API reads the data from the auxiliary sensor +*/ +uint16_t bma4_aux_read(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev) +{ + /* Variable to return error codes */ + uint16_t rslt = BMA4_OK; + + /* Check for Null pointer error */ + rslt |= bma4_null_pointer_check(dev); + if (rslt == BMA4_OK) { + /* Read the data from the data register in terms of + user defined length */ + rslt = bma4_extract_aux_data(aux_reg_addr, aux_data, len, dev); + } + + return rslt; +} + +/*! + * @brief This API writes the data into the auxiliary sensor +*/ +uint16_t bma4_aux_write(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev) +{ + + uint16_t rslt = BMA4_OK; + + /* Check for Null pointer error */ + rslt |= bma4_null_pointer_check(dev); + if (rslt == BMA4_OK) { + /* Write data in terms of user defined length */ + if (len > 0) { + while (len--) { + /* First set data to write */ + rslt = bma4_write_regs(BMA4_AUX_WR_DATA_ADDR, aux_data, 1, dev); + dev->delay(BMA4_AUX_COM_DELAY); + if (rslt == BMA4_OK) { + /* Then set address to write */ + rslt = bma4_write_regs(BMA4_AUX_WR_ADDR, &aux_reg_addr, 1, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Increment data array and register address until + * user-defined length is greater than 0 */ + if ((rslt == BMA4_OK) && (len > 0)) { + aux_data++; + aux_reg_addr++; + } + } else { + rslt = BMA4_E_FAIL; + } + } + } else { + rslt = BMA4_E_RD_WR_LENGTH_INVALID; + } + } + + return rslt; +} + +/*****************************************************************************/ +/* Static function definition */ +/*! + * @brief This API converts lsb value of axes to mg for self-test * + */ +static void convert_lsb_g(const struct selftest_delta_limit *accel_data_diff, + struct selftest_delta_limit *accel_data_diff_mg, + const struct bma4_dev *dev) +{ + uint32_t lsb_per_g; + /*! Range considered for self-test is 8g */ + uint8_t range = 8; + + /*! lsb_per_g for the respective resolution and 8g range*/ + lsb_per_g = (uint32_t)(power(2, dev->resolution) / (2 * range)); + /*! accel x value in mg */ + accel_data_diff_mg->x = (accel_data_diff->x / (int32_t)lsb_per_g) * 1000; + /*! accel y value in mg */ + accel_data_diff_mg->y = (accel_data_diff->y / (int32_t)lsb_per_g) * 1000; + /*! accel z value in mg */ + accel_data_diff_mg->z = (accel_data_diff->z / (int32_t)lsb_per_g) * 1000; +} + +/*! + * @brief This API writes the config stream data in memory using burst mode + * @note index value should be even number. + */ +static uint16_t stream_transfer_write(const uint8_t *stream_data, uint16_t index, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t asic_msb = (uint8_t)((index / 2) >> 4); + uint8_t asic_lsb = ((index / 2) & 0x0F); + + rslt |= bma4_write_regs(BMA4_RESERVED_REG_5B_ADDR, &asic_lsb, 1, dev); + + if (rslt == BMA4_OK) { + rslt |= bma4_write_regs(BMA4_RESERVED_REG_5C_ADDR, &asic_msb, 1, dev); + + if (rslt == BMA4_OK) + rslt |= write_regs(BMA4_FEATURE_CONFIG_ADDR, (uint8_t *)stream_data, dev->read_write_len, dev); + } + + return rslt; +} + +/*! + * @brief This API enables or disables the Accel Self test feature in the + * sensor. + */ +static uint16_t set_accel_selftest_enable(uint8_t accel_selftest_enable, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + /* Read the self test register */ + rslt |= bma4_read_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITS_POS_0(data, BMA4_ACCEL_SELFTEST_ENABLE, accel_selftest_enable); + rslt |= bma4_write_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API selects the sign of Accel self-test excitation. + */ +static uint16_t set_accel_selftest_sign(uint8_t accel_selftest_sign, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + if (accel_selftest_sign <= BMA4_MAX_VALUE_SELFTEST_SIGN) { + /* Read the Accel self test sign*/ + rslt |= bma4_read_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_ACCEL_SELFTEST_SIGN, accel_selftest_sign); + rslt |= bma4_write_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + } else { + rslt = BMA4_E_OUT_OF_RANGE; + } + } + + return rslt; +} + +/*! + * @brief This API sets the Accel self test amplitude in the sensor. + */ +static uint16_t set_accel_selftest_amp(uint8_t accel_selftest_amp, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t data = 0; + /* Check the bma4 structure as NULL */ + if (dev == NULL) { + rslt |= BMA4_E_NULL_PTR; + } else { + + if (accel_selftest_amp <= BMA4_MAX_VALUE_SELFTEST_AMP) { + /* Write self test amplitude*/ + rslt |= bma4_read_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + + if (rslt == BMA4_OK) { + data = BMA4_SET_BITSLICE(data, BMA4_SELFTEST_AMP, accel_selftest_amp); + + rslt |= bma4_write_regs(BMA4_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + } else { + rslt |= BMA4_E_OUT_OF_RANGE; + } + } + + return rslt; +} + +/*! + * @brief This function enables and configures the Accel which is needed + * for Self test operation. + */ +static uint16_t set_accel_selftest_config(struct bma4_dev *dev) +{ + uint16_t rslt = 0; + struct bma4_accel_config accel = {0}; + + accel.odr = BMA4_OUTPUT_DATA_RATE_1600HZ; + accel.bandwidth = BMA4_ACCEL_NORMAL_AVG4; + accel.perf_mode = BMA4_ENABLE; + accel.range = BMA4_ACCEL_RANGE_8G; + + rslt |= bma4_set_accel_enable(BMA4_ENABLE, dev); + dev->delay(1); + rslt |= bma4_set_accel_config(&accel, dev); + + return rslt; +} + +/*! + * @brief This API validates the Accel g value provided as input by the + * user for Accel offset compensation. + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + */ +static uint16_t validate_user_input(const int32_t *gvalue) + +{ + uint8_t index = 0; + int32_t min_gval = (int32_t)(-1.0 * BMA4XY_MULTIPLIER); + int32_t max_gval = (int32_t)(1.0 * BMA4XY_MULTIPLIER); + + while (index < 3) { + if (gvalue[index] >= min_gval && gvalue[index] <= max_gval) + index++; + else + return BMA4_E_OUT_OF_RANGE; + } + + return BMA4_OK; +} + +/*! + * @brief This API normalise the data with offset + */ +static void normalise_offset(const struct offset_delta *compensated_data, struct accel_offset *offset_data) +{ + /* for handling negative offset */ + /* employing twos's Complement method*/ + if (compensated_data->x.is_negative == TRUE) { + offset_data->x = ~offset_data->x; + offset_data->x += 1; + } + + if (compensated_data->y.is_negative == TRUE) { + offset_data->y = ~offset_data->y; + offset_data->y += 1; + } + + if (compensated_data->z.is_negative == TRUE) { + offset_data->z = ~offset_data->z; + offset_data->z += 1; + } + + offset_data->x = (uint8_t)((offset_data->x) * (-1)); + offset_data->y = (uint8_t)((offset_data->y) * (-1)); + offset_data->z = (uint8_t)((offset_data->z) * (-1)); +} + +/*! + * @brief This API normalize the data with offset. + */ +static void scale_offset(uint8_t res, uint8_t range, const struct offset_delta *delta, struct accel_offset *data) +{ + int8_t bit_pos_3_9mg, bit_pos_3_9mg_nextbit; + uint8_t round_off = 0; + + /* Find the bit position of 3.9mg */ + bit_pos_3_9mg = get_bit_pos_3_9mg(range, res); + + /* Data register resolution less than or equal to 3.9 mg */ + if (bit_pos_3_9mg > 0) { + /* Round off, consider if the next bit is high */ + bit_pos_3_9mg_nextbit = bit_pos_3_9mg - 1; + round_off = (uint8_t)(1 * power(2, ((uint8_t) bit_pos_3_9mg_nextbit))); + /* scale according to offset register resolution*/ + data->x = (uint8_t)((delta->x.val + round_off) / power(2, ((uint8_t)bit_pos_3_9mg))); + data->y = (uint8_t)((delta->y.val + round_off) / power(2, ((uint8_t)bit_pos_3_9mg))); + data->z = (uint8_t)((delta->z.val + round_off) / power(2, ((uint8_t)bit_pos_3_9mg))); + } else if (bit_pos_3_9mg < 0) { + bit_pos_3_9mg = (int8_t)(bit_pos_3_9mg * -1); + data->x = (uint8_t)(delta->x.val * power(2, ((uint8_t)bit_pos_3_9mg))); + data->y = (uint8_t)(delta->y.val * power(2, ((uint8_t)bit_pos_3_9mg))); + data->z = (uint8_t)(delta->z.val * power(2, ((uint8_t)bit_pos_3_9mg))); + } else { + /* Scale according to offset register resolution */ + data->x = (uint8_t)(delta->x.val); + data->y = (uint8_t)(delta->y.val); + data->z = (uint8_t)(delta->z.val); + } +} + +/*! + * @brief This API compensate the accel data against gravity. + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + */ +static void comp_for_grvty(uint16_t lsb_per_g, const int32_t g_val[3], const struct bma4_accel *data, + struct offset_delta *comp_data) +{ + int64_t accel_value_lsb[3] = {0}; + uint8_t index; + + for (index = 0; index < 3; index++) { + /* convert g to lsb */ + accel_value_lsb[index] = ((int64_t)(lsb_per_g) * (int64_t)(g_val[index])); + } + + /* Dividing the accel value in LSB by 1000000 to get + compensated data back in g-value */ + comp_data->x.val = (int16_t)(data->x - (int16_t)((accel_value_lsb[BMA4_X_AXIS] / (int64_t)BMA4XY_MULTIPLIER))); + comp_data->y.val = (int16_t)(data->y - (int16_t)((accel_value_lsb[BMA4_Y_AXIS] / (int64_t)BMA4XY_MULTIPLIER))); + comp_data->z.val = (int16_t)(data->z - (int16_t)((accel_value_lsb[BMA4_Z_AXIS] / (int64_t)BMA4XY_MULTIPLIER))); + + if (comp_data->x.val < 0) { + comp_data->x.val = ABS(comp_data->x.val); + comp_data->x.is_negative = 1; + } + + if (comp_data->y.val < 0) { + comp_data->y.val = ABS(comp_data->y.val); + comp_data->y.is_negative = 1; + } + + if (comp_data->z.val < 0) { + comp_data->z.val = ABS(comp_data->z.val); + comp_data->z.is_negative = 1; + } +} + +/*! + * @brief This API converts the range value into corresponding + * integer value. + */ +static void map_range(uint8_t range_in, uint8_t *range_out) +{ + switch (range_in) { + case BMA4_ACCEL_RANGE_2G: + *range_out = 2; + break; + case BMA4_ACCEL_RANGE_4G: + *range_out = 4; + break; + case BMA4_ACCEL_RANGE_8G: + *range_out = 8; + break; + case BMA4_ACCEL_RANGE_16G: + *range_out = 16; + break; + default: + break; + } +} + +/*! + * @brief This API is used to reset the FIFO related configurations + * in the fifo_frame structure. + * + */ +static void reset_fifo_data_structure(const struct bma4_dev *dev) +{ + /*Prepare for next FIFO read by resetting FIFO's + internal data structures*/ + dev->fifo->accel_byte_start_idx = 0; + dev->fifo->mag_byte_start_idx = 0; + dev->fifo->sc_frame_byte_start_idx = 0; + dev->fifo->sensor_time = 0; + dev->fifo->skipped_frame_count = 0; + dev->fifo->accel_dropped_frame_count = 0; + dev->fifo->mag_dropped_frame_count = 0; +} + +/*! + * @brief This API computes the number of bytes of accel FIFO data + * which is to be parsed in header-less mode + */ +static void get_accel_len_to_parse(uint16_t *start_idx, uint16_t *len, const uint16_t *acc_count, + const struct bma4_dev *dev) +{ + uint8_t dummy_byte_spi = 0; + + /*Check if this is the first iteration of data unpacking + if yes, then consider dummy byte on SPI*/ + if (dev->fifo->accel_byte_start_idx == 0) + dummy_byte_spi = dev->dummy_byte; + + /*Data start index*/ + *start_idx = dev->fifo->accel_byte_start_idx + dummy_byte_spi; + + if (dev->fifo->fifo_data_enable == BMA4_FIFO_A_ENABLE) { + /*Len has the number of bytes to loop for */ + *len = (uint16_t)(((*acc_count) * BMA4_FIFO_A_LENGTH) + dummy_byte_spi); + } else if (dev->fifo->fifo_data_enable == BMA4_FIFO_M_A_ENABLE) { + /*Len has the number of bytes to loop for */ + *len = (uint16_t)(((*acc_count) * BMA4_FIFO_MA_LENGTH) + dummy_byte_spi); + } else { + /*Only aux. sensor or no sensor is enabled in FIFO, + so there will be no accel data. + Update the data index as complete*/ + *start_idx = dev->fifo->length; + } + if ((*len) > dev->fifo->length) { + /*Handling the case where more data is requested + than available*/ + *len = dev->fifo->length; + } +} + +/*! + * @brief This API checks the fifo read data as empty frame, if it + * is empty frame then moves the index to last byte. + */ +static void check_empty_fifo(uint16_t *data_index, const struct bma4_dev *dev) +{ + if ((*data_index + 2) < dev->fifo->length) { + /* Check if FIFO is empty */ + if ((dev->fifo->data[*data_index] == FIFO_MSB_CONFIG_CHECK) + && (dev->fifo->data[*data_index + 1] == FIFO_LSB_CONFIG_CHECK)) { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in header mode. + * + */ +static void extract_accel_header_mode(struct bma4_accel *accel_data, uint16_t *accel_length, const struct bma4_dev *dev) +{ + uint8_t frame_header = 0; + uint16_t data_index; + uint16_t accel_index = 0; + uint16_t frame_to_read = *accel_length; + /*Check if this is the first iteration of data unpacking + if yes, then consider dummy byte on SPI*/ + if (dev->fifo->accel_byte_start_idx == 0) + dev->fifo->accel_byte_start_idx = dev->dummy_byte; + + for (data_index = dev->fifo->accel_byte_start_idx; data_index < dev->fifo->length;) { + /*Header byte is stored in the variable frame_header*/ + frame_header = dev->fifo->data[data_index]; + /*Get the frame details from header*/ + frame_header = frame_header & BMA4_FIFO_TAG_INTR_MASK; + /*Index is moved to next byte where the data is starting*/ + data_index++; + + switch (frame_header) { + /* Accel frame */ + case FIFO_HEAD_A: + case FIFO_HEAD_M_A: + unpack_acc_frm(accel_data, &data_index, &accel_index, frame_header, dev); + break; + /* Aux. sensor frame */ + case FIFO_HEAD_M: + move_next_frame(&data_index, BMA4_FIFO_M_LENGTH, dev); + break; + /* Sensor time frame */ + case FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + /* Skip frame */ + case FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + /* Input config frame */ + case FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + /* Sample drop frame */ + case FIFO_HEAD_SAMPLE_DROP: + unpack_dropped_frame(&data_index, dev); + break; + /* Over read FIFO data */ + case FIFO_HEAD_OVER_READ_MSB: + /* Update the data index as complete*/ + data_index = dev->fifo->length; + break; + default: + break; + } + if (frame_to_read == accel_index) { + /*Number of frames to read completed*/ + break; + } + } + + /*Update number of accel data read*/ + *accel_length = accel_index; + /*Update the accel frame index*/ + dev->fifo->accel_byte_start_idx = data_index; +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data in both header mode and header-less mode. + * It update the idx value which is used to store the index of + * the current data byte which is parsed. + */ +static void unpack_acc_frm(struct bma4_accel *acc, uint16_t *idx, uint16_t *acc_idx, uint8_t frm, + const struct bma4_dev *dev) +{ + switch (frm) { + case FIFO_HEAD_A: + case BMA4_FIFO_A_ENABLE: + /*Partial read, then skip the data*/ + if ((*idx + BMA4_FIFO_A_LENGTH) > dev->fifo->length) { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + /*Unpack the data array into the structure instance "acc" */ + unpack_accel_data(&acc[*acc_idx], *idx, dev); + /*Move the data index*/ + *idx = *idx + BMA4_FIFO_A_LENGTH; + (*acc_idx)++; + break; + case FIFO_HEAD_M_A: + case BMA4_FIFO_M_A_ENABLE: + /*Partial read, then skip the data*/ + if ((*idx + BMA4_FIFO_MA_LENGTH) > dev->fifo->length) { + /*Update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + /*Unpack the data array into structure instance "acc"*/ + unpack_accel_data(&acc[*acc_idx], *idx + BMA4_MA_FIFO_A_X_LSB, dev); + /*Move the data index*/ + *idx = *idx + BMA4_FIFO_MA_LENGTH; + (*acc_idx)++; + break; + /* Aux. sensor frame */ + case FIFO_HEAD_M: + case BMA4_FIFO_M_ENABLE: + (*idx) = (*idx) + BMA4_FIFO_M_LENGTH; + break; + default: + break; + } +} + +/*! + * @brief This API is used to parse the accelerometer data from the + * FIFO data and store it in the instance of the structure bma4_accel. + */ +static void unpack_accel_data(struct bma4_accel *accel_data, uint16_t data_start_index, const struct bma4_dev *dev) +{ + uint16_t data_lsb; + uint16_t data_msb; + + /* Accel raw x data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw y data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Accel raw z data */ + data_lsb = dev->fifo->data[data_start_index++]; + data_msb = dev->fifo->data[data_start_index++]; + accel_data->z = (int16_t)((data_msb << 8) | data_lsb); + + if (dev->resolution == BMA4_12_BIT_RESOLUTION) { + accel_data->x = (accel_data->x / 0x10); + accel_data->y = (accel_data->y / 0x10); + accel_data->z = (accel_data->z / 0x10); + } else if (dev->resolution == BMA4_14_BIT_RESOLUTION) { + accel_data->x = (accel_data->x / 0x04); + accel_data->y = (accel_data->y / 0x04); + accel_data->z = (accel_data->z / 0x04); + } +} + +/*! + * @brief This API computes the number of bytes of Mag FIFO data which is + * to be parsed in header-less mode + * + */ +static void get_mag_len_to_parse(uint16_t *start_idx, uint16_t *len, const uint16_t *mag_count, + const struct bma4_dev *dev) +{ + uint8_t dummy_byte_spi = 0; + + /*Check if this is the first iteration of data unpacking + if yes, then consider dummy byte on SPI*/ + if (dev->fifo->mag_byte_start_idx == 0) + dummy_byte_spi = dev->dummy_byte; + + /*Data start index*/ + *start_idx = dev->fifo->mag_byte_start_idx + dummy_byte_spi; + + if (dev->fifo->fifo_data_enable == BMA4_FIFO_M_ENABLE) { + /*Len has the number of bytes to loop for */ + *len = (uint16_t)(((*mag_count) * BMA4_FIFO_M_LENGTH) + dummy_byte_spi); + } else if (dev->fifo->fifo_data_enable == BMA4_FIFO_M_A_ENABLE) { + /*Len has the number of bytes to loop for */ + *len = (uint16_t)(((*mag_count) * BMA4_FIFO_MA_LENGTH) + dummy_byte_spi); + } else { + /*Only accel sensor or no sensor is enabled in FIFO, + so there will be no mag data. + Update the data index as complete*/ + *start_idx = dev->fifo->length; + } + + /*Handling the case where more data is requested than available*/ + if ((*len) > dev->fifo->length) { + /*Len is equal to the FIFO length*/ + *len = dev->fifo->length; + } +} + +/*! + * @brief This API is used to parse the magnetometer data from the + * FIFO data in header mode. + * + */ +static uint16_t extract_mag_header_mode(struct bma4_mag *data, uint16_t *len, const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t frame_header = 0; + uint16_t data_index; + uint16_t mag_index = 0; + uint16_t frame_to_read = *len; + + /*Check if this is the first iteration of data unpacking + if yes, then consider dummy byte on SPI*/ + if (dev->fifo->mag_byte_start_idx == 0) + dev->fifo->mag_byte_start_idx = dev->dummy_byte; + + for (data_index = dev->fifo->mag_byte_start_idx; data_index < dev->fifo->length;) { + /*Header byte is stored in the variable frame_header*/ + frame_header = dev->fifo->data[data_index]; + /*Get the frame details from header*/ + frame_header = frame_header & BMA4_FIFO_TAG_INTR_MASK; + /*Index is moved to next byte where the data is starting*/ + data_index++; + + switch (frame_header) { + /* Aux. sensor frame */ + case FIFO_HEAD_M: + case FIFO_HEAD_M_A: + rslt |= unpack_mag_frm(data, &data_index, &mag_index, frame_header, dev); + break; + /* Aux. sensor frame */ + case FIFO_HEAD_A: + move_next_frame(&data_index, BMA4_FIFO_A_LENGTH, dev); + break; + /* Sensor time frame */ + case FIFO_HEAD_SENSOR_TIME: + unpack_sensortime_frame(&data_index, dev); + break; + /* Skip frame */ + case FIFO_HEAD_SKIP_FRAME: + unpack_skipped_frame(&data_index, dev); + break; + /* Input config frame */ + case FIFO_HEAD_INPUT_CONFIG: + move_next_frame(&data_index, 1, dev); + break; + /* Sample drop frame */ + case FIFO_HEAD_SAMPLE_DROP: + unpack_dropped_frame(&data_index, dev); + break; + case FIFO_HEAD_OVER_READ_MSB: + /*Update the data index as complete*/ + data_index = dev->fifo->length; + break; + default: + break; + } + if (frame_to_read == mag_index) { + /*Number of frames to read completed*/ + break; + } + } + /*update number of Aux. sensor data read*/ + *len = mag_index; + /*update the Aux. sensor frame index*/ + dev->fifo->mag_byte_start_idx = data_index; + return rslt; +} + +/*! + * @brief This API is used to parse the magnetometer data from the + * FIFO data in both header mode and header-less mode and update the + * data_index value which is used to store the index of the current + * data byte which is parsed. + * + */ +static uint16_t unpack_mag_frm(struct bma4_mag *data, uint16_t *idx, uint16_t *mag_idx, uint8_t frm, + const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + switch (frm) { + case FIFO_HEAD_M: + case BMA4_FIFO_M_ENABLE: + /*partial read, then skip the data*/ + if ((*idx + BMA4_FIFO_M_LENGTH) > dev->fifo->length) { + /*update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*unpack the data array into Aux. sensor data structure*/ + rslt |= unpack_mag_data(&data[*mag_idx], *idx, dev); + /*move the data index*/ + *idx = *idx + BMA4_FIFO_M_LENGTH; + (*mag_idx)++; + break; + case FIFO_HEAD_M_A: + case BMA4_FIFO_M_A_ENABLE: + /*partial read, then skip the data*/ + if ((*idx + BMA4_FIFO_MA_LENGTH) > dev->fifo->length) { + /*update the data index as complete*/ + *idx = dev->fifo->length; + break; + } + + /*unpack the data array into Aux. sensor data structure*/ + rslt |= unpack_mag_data(&data[*mag_idx], *idx, dev); + /*move the data index to next frame*/ + *idx = *idx + BMA4_FIFO_MA_LENGTH; + (*mag_idx)++; + break; + /* aux. sensor frame */ + case FIFO_HEAD_A: + case BMA4_FIFO_A_ENABLE: + (*idx) = (*idx) + BMA4_FIFO_A_LENGTH; + break; + default: + break; + } + + return rslt; +} + +/*! + * @brief This API is used to parse the auxiliary magnetometer data from + * the FIFO data and store it in the instance of the structure mag_data. + * + */ +static uint16_t unpack_mag_data(struct bma4_mag *mag_data, uint16_t start_idx, const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + struct bma4_mag_fifo_data mag_fifo_data; + + /* Aux. mag sensor raw x data */ + mag_fifo_data.mag_x_lsb = dev->fifo->data[start_idx++]; + mag_fifo_data.mag_x_msb = dev->fifo->data[start_idx++]; + + /* Aux. mag sensor raw y data */ + mag_fifo_data.mag_y_lsb = dev->fifo->data[start_idx++]; + mag_fifo_data.mag_y_msb = dev->fifo->data[start_idx++]; + + /* Aux. mag sensor raw z data */ + mag_fifo_data.mag_z_lsb = dev->fifo->data[start_idx++]; + mag_fifo_data.mag_z_msb = dev->fifo->data[start_idx++]; + + /* Aux. mag sensor raw r data */ + mag_fifo_data.mag_r_y2_lsb = dev->fifo->data[start_idx++]; + mag_fifo_data.mag_r_y2_msb = dev->fifo->data[start_idx++]; + + /*Compensated FIFO data output*/ + rslt |= bma4_second_if_mag_compensate_xyz(mag_fifo_data, dev->aux_sensor, mag_data); + + return rslt; +} + +/*! + * @brief This API is used to parse and store the sensor time from the + * FIFO data in the structure instance dev. + * + */ +static void unpack_sensortime_frame(uint16_t *data_index, const struct bma4_dev *dev) +{ + uint32_t sensor_time_byte3 = 0; + uint16_t sensor_time_byte2 = 0; + uint8_t sensor_time_byte1 = 0; + + /*Partial read, then move the data index to last data*/ + if ((*data_index + BMA4_SENSOR_TIME_LENGTH) > dev->fifo->length) { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } else { + sensor_time_byte3 = dev->fifo->data[(*data_index) + BMA4_SENSOR_TIME_MSB_BYTE] << 16; + sensor_time_byte2 = dev->fifo->data[(*data_index) + BMA4_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = dev->fifo->data[(*data_index)]; + /* Sensor time */ + dev->fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + *data_index = (*data_index) + BMA4_SENSOR_TIME_LENGTH; + } +} + +/*! + * @brief This API is used to parse and store the skipped_frame_count from + * the FIFO data in the structure instance dev. + */ +static void unpack_skipped_frame(uint16_t *data_index, const struct bma4_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if (*data_index >= dev->fifo->length) { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } else { + dev->fifo->skipped_frame_count = dev->fifo->data[*data_index]; + /*Move the data index*/ + *data_index = (*data_index) + 1; + } +} + +/*! + * @brief This API is used to parse and store the dropped_frame_count from + * the FIFO data in the structure instance dev. + */ +static void unpack_dropped_frame(uint16_t *data_index, const struct bma4_dev *dev) +{ + uint8_t dropped_frame = 0; + /*Partial read, then move the data index to last data*/ + if (*data_index >= dev->fifo->length) { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } else { + /*Extract accel and mag dropped frame count*/ + dropped_frame = dev->fifo->data[*data_index] & ACCEL_AUX_FIFO_DROP; + /*Move the data index and update the dropped frame count*/ + switch (dropped_frame) { + case ACCEL_FIFO_DROP: + *data_index = (*data_index) + BMA4_FIFO_A_LENGTH; + dev->fifo->accel_dropped_frame_count = dev->fifo->accel_dropped_frame_count + 1; + break; + case AUX_FIFO_DROP: + *data_index = (*data_index) + BMA4_FIFO_M_LENGTH; + dev->fifo->mag_dropped_frame_count = dev->fifo->mag_dropped_frame_count + 1; + break; + case ACCEL_AUX_FIFO_DROP: + *data_index = (*data_index) + BMA4_FIFO_MA_LENGTH; + dev->fifo->accel_dropped_frame_count = dev->fifo->accel_dropped_frame_count + 1; + dev->fifo->mag_dropped_frame_count = dev->fifo->mag_dropped_frame_count + 1; + break; + default: + break; + } + } +} + +/*! + * @brief This API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static void move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bma4_dev *dev) +{ + /*Partial read, then move the data index to last data*/ + if ((*data_index + current_frame_length) > dev->fifo->length) { + /*Update the data index as complete*/ + *data_index = dev->fifo->length; + } else { + /*Move the data index to next frame*/ + *data_index = *data_index + current_frame_length; + } +} + +/*! + * @brief This function validates the Accel Self test data and decides the + * result of Self test operation. + */ +static uint16_t validate_selftest(const struct selftest_delta_limit *accel_data_diff, + const struct bma4_dev *dev) +{ + uint16_t rslt = 0; + + /* Set self test amplitude based on variant */ + switch (dev->variant) { + case BMA42X_VARIANT: + /* Validating accel data by comparing with minimum value of the axes in mg */ + /* For BMA42x - > x axis limit 400mg, y axis limit 800mg and z axis limit 400mg */ + if ((accel_data_diff->x > BMA42X_ST_ACC_X_AXIS_SIGNAL_DIFF) && + (accel_data_diff->y > BMA42X_ST_ACC_Y_AXIS_SIGNAL_DIFF) && + (accel_data_diff->z > BMA42X_ST_ACC_Z_AXIS_SIGNAL_DIFF)) { + rslt = BMA4_OK; + } else { + rslt = BMA4_E_SELF_TEST_FAIL; + } + break; + + case BMA45X_VARIANT: + /* Validating accel data by comparing with minimum value of the axes in mg */ + /* For BMA45x - > x axis limit 1800mg, y axis limit 1800mg and z axis limit 1800mg */ + if ((accel_data_diff->x > BMA45X_ST_ACC_X_AXIS_SIGNAL_DIFF) && + (accel_data_diff->y > BMA45X_ST_ACC_Y_AXIS_SIGNAL_DIFF) && + (accel_data_diff->z > BMA45X_ST_ACC_Z_AXIS_SIGNAL_DIFF)) { + rslt = BMA4_OK; + } else { + rslt = BMA4_E_SELF_TEST_FAIL; + } + break; + + default: + rslt = BMA4_E_INVALID_SENSOR; + break; + } + + return rslt; +} + +/*! + * @brief This function configure the Accel for FOC. + */ +static uint16_t foc_config(struct bma4_accel_config *acc_conf, uint8_t *acc_en, uint8_t *pwr_mode, struct bma4_dev *dev) +{ + uint16_t rslt = 0; + uint8_t accel_cnf = BMA4_ACCEL_CONFIG_FOC; + + /* for saving Accel configuration, + Accel enable status, advance power save*/ + rslt |= bma4_get_accel_config(acc_conf, dev); + rslt |= bma4_get_accel_enable(acc_en, dev); + rslt |= bma4_get_advance_power_save(pwr_mode, dev); + + /* Disabling offset compensation that is in place*/ + rslt |= bma4_set_offset_comp(BMA4_DISABLE, dev); + + if (rslt == BMA4_OK) { + /* Set Accel config to 50Hz, continuous filter mode, + no under sampling */ + rslt |= bma4_write_regs(BMA4_ACCEL_CONFIG_ADDR, &accel_cnf, 1, dev); + + if (rslt == BMA4_OK) { + /* Switch Accel to normal mode and + advance power save to zero*/ + rslt |= bma4_set_accel_enable(BMA4_ENABLE, dev); + rslt |= bma4_set_advance_power_save(BMA4_DISABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This API is used to calculate the power of 2 + */ +static int32_t power(int16_t base, uint8_t resolution) +{ + uint8_t i = 1; + /* Initialize variable to store the power of 2 value */ + int32_t value = 1; + + for (; i <= resolution; i++) + value = (int32_t)(value * base); + + return value; +} + +/*! + * @brief This API performs the roundoff on given value + */ +static int8_t roundoff(int32_t value) +{ + /* Variable to return the round off value */ + int8_t ret = 0; + + /* Since the value passed is scaled in multiples of 100, + the return value is divided by 100 to get the round off value */ + if (value < 0) + ret = (int8_t)(((value) - 50) / 100); + else + ret = (int8_t)(((value) + 50) / 100); + + + return ret; +} + +/*! + * @brief This API finds the bit position of 3.9mg according to given range + * and resolution. + */ +static int8_t get_bit_pos_3_9mg(uint8_t range, uint8_t res) +{ + /* Variable to store the bit position of 3.9mg */ + int8_t bit_pos_3_9mg = 0; + /* Variable to store the value to be rounded off */ + int32_t value = 0; + /* Variable to store the LSB per bit in micros */ + int32_t ug_per_bit; + /* Variable to store the rounded off value */ + int8_t round_off_int; + /* Variable to store the bit count */ + uint8_t bit_count = 0; + /* Variable to store the signed range value */ + int32_t range_value; + + /* Scaling range with a multiplier to get integer value of ug_per_bit */ + range_value = (int32_t)(2 * range * BMA4XY_MULTIPLIER); + + /* Get the G-per bit resolution*/ + ug_per_bit = (int32_t)(range_value / power(2, res)); + + /* Compare for -ve & +ve bit position w.r.t 3900micro-g or as reference + * Note: Value scaled in 100s to get accurate integer value */ + if (ug_per_bit > 3900) + value = (int32_t)(ug_per_bit * 100 / 3900); + else + value = (int32_t)(3900 * 100 / ug_per_bit); + + /* Round off the value */ + round_off_int = (int8_t)(roundoff(value)); + + /* Find the bit position of 3.9mg*/ + while (round_off_int != 1) { + round_off_int = (round_off_int / 0x02); + bit_count++; + } + + /* Check for +ve and -ve bit position of 3.9 mg */ + if (ug_per_bit > 3900) + bit_pos_3_9mg = (int8_t)(bit_count * (-1)); + else + bit_pos_3_9mg = (int8_t)bit_count; + + return bit_pos_3_9mg; +} +/*! + * @brief This internal API brings up the secondary interface to access + * auxiliary sensor * + */ +static uint16_t bma4_set_aux_interface_config(struct bma4_dev *dev) +{ + /* Variable to return error codes */ + uint16_t rslt = BMA4_OK; + + /* Check for null pointer error */ + rslt |= bma4_null_pointer_check(dev); + + if (rslt == BMA4_OK) { + /* Enable the auxiliary sensor */ + rslt |= bma4_set_mag_enable(0x01, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Disable advance power save */ + rslt |= bma4_set_advance_power_save(0x00, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Set the I2C device address of auxiliary device */ + rslt |= bma4_set_i2c_device_addr(dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Set auxiliary interface to manual mode */ + rslt |= bma4_set_mag_manual_enable(dev->aux_config.manual_enable, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Set the number of bytes for burst read */ + rslt |= bma4_set_mag_burst(dev->aux_config.burst_read_length, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + /* Switch on the the auxiliary interface mode */ + rslt |= bma4_set_if_mode(dev->aux_config.if_mode, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + } + + return rslt; +} + +/*! +* @brief This internal API reads the data from the auxiliary sensor +* depending on burst length configured +*/ +static uint16_t bma4_extract_aux_data(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev) +{ + /* Variable to return error codes */ + uint16_t rslt = BMA4_OK; + /* Pointer variable to read data from the register */ + uint8_t data[15] = {0}; + /* Variable to define length counts */ + uint8_t len_count = 0; + /* Variable to define burst read length */ + uint8_t burst_len = 0; + /* Variable to define read length */ + uint8_t read_length = 0; + /* Variable to define the number of burst reads */ + uint8_t burst_count; + /* Variable to define address of the data register*/ + uint8_t aux_read_addr = BMA4_DATA_0_ADDR; + + /* Extract burst read length in a variable */ + rslt |= bma4_map_read_len(&burst_len, dev); + + for (burst_count = 0; burst_count < len; burst_count += burst_len) { + /* Set the address whose data is to be read */ + rslt |= bma4_set_mag_read_addr(aux_reg_addr, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + if (rslt == BMA4_OK) { + /* If user defined length is valid */ + if (len > 0) { + /* Read the data from the data register */ + rslt |= bma4_read_regs(aux_read_addr, data, (uint8_t)burst_len, dev); + dev->delay(BMA4_AUX_COM_DELAY); + + if (rslt == BMA4_OK) { + /* If defined user length or remaining length after a burst + read is less than burst length */ + if ((len - burst_count) < burst_len) { + /* Read length is equal to burst_length or + remaining length*/ + read_length = (uint8_t)(len - burst_count); + } else { + /* Read length is equal to burst_length */ + read_length = burst_len; + } + + /* Copy the read data in terms of given read length */ + for (len_count = 0; len_count < read_length; len_count++) + aux_data[burst_count + len_count] = data[len_count]; + + /* Increment the register address by burst read length */ + aux_reg_addr += burst_len; + } else { + rslt = BMA4_E_RD_WR_LENGTH_INVALID; + } + } else { + rslt = BMA4_E_FAIL; + } + } else { + rslt = BMA4_E_FAIL; + } + } + + return rslt; +} + +/*! + * @brief This internal API maps the actual burst read length with user + length set. + */ +static uint16_t bma4_map_read_len(uint8_t *len, const struct bma4_dev *dev) +{ + /* Variable to return error codes */ + uint16_t rslt = BMA4_OK; + + switch (dev->aux_config.burst_read_length) { + + case BMA4_AUX_READ_LEN_0: + *len = 1; + break; + case BMA4_AUX_READ_LEN_1: + *len = 2; + break; + case BMA4_AUX_READ_LEN_2: + *len = 6; + break; + case BMA4_AUX_READ_LEN_3: + *len = 8; + break; + default: + rslt = BMA4_E_OUT_OF_RANGE; + break; + } + + return rslt; +} + +/*! +* @brief This internal API checks null pointer error +*/ +static uint16_t bma4_null_pointer_check(const struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + if ((dev == NULL) || (dev->bus_read == NULL) || (dev->bus_write == NULL)) + rslt |= BMA4_E_NULL_PTR; + else + rslt = BMA4_OK; + + return rslt; +} \ No newline at end of file diff --git a/src/bma4.h b/src/bma4.h new file mode 100644 index 0000000..499a3f2 --- /dev/null +++ b/src/bma4.h @@ -0,0 +1,1574 @@ +/* +* +**************************************************************************** +* Copyright (C) 2015 - 2016 Bosch Sensortec GmbH +* +* File : bma4.h +* +* Date: 12 Oct 2017 +* +* Revision: 2.1.9 $ +* +* Usage: Sensor Driver for BMA4 family of sensors +* +**************************************************************************** +* +* Disclaimer +* +* Common: +* Bosch Sensortec products are developed for the consumer goods industry. +* They may only be used within the parameters of the respective valid +* product data sheet. Bosch Sensortec products are provided with the +* express understanding that there is no warranty of fitness for a +* particular purpose.They are not fit for use in life-sustaining, +* safety or security sensitive systems or any system or device +* that may lead to bodily harm or property damage if the system +* or device malfunctions. In addition,Bosch Sensortec products are +* not fit for use in products which interact with motor vehicle systems. +* The resale and or use of products are at the purchasers own risk and +* his own responsibility. The examination of fitness for the intended use +* is the sole responsibility of the Purchaser. +* +* The purchaser shall indemnify Bosch Sensortec from all third party +* claims, including any claims for incidental, or consequential damages, +* arising from any product use not covered by the parameters of +* the respective valid product data sheet or not approved by +* Bosch Sensortec and reimburse Bosch Sensortec for all costs in +* connection with such claims. +* +* The purchaser must monitor the market for the purchased products, +* particularly with regard to product safety and inform Bosch Sensortec +* without delay of all security relevant incidents. +* +* Engineering Samples are marked with an asterisk (*) or (e). +* Samples may vary from the valid technical specifications of the product +* series. They are therefore not intended or fit for resale to third +* parties or for use in end products. Their sole purpose is internal +* client testing. The testing of an engineering sample may in no way +* replace the testing of a product series. Bosch Sensortec assumes +* no liability for the use of engineering samples. +* By accepting the engineering samples, the Purchaser agrees to indemnify +* Bosch Sensortec from all claims arising from the use of engineering +* samples. +* +* Special: +* This software module (hereinafter called "Software") and any information +* on application-sheets (hereinafter called "Information") is provided +* free of charge for the sole purpose to support your application work. +* The Software and Information is subject to the following +* terms and conditions: +* +* The Software is specifically designed for the exclusive use for +* Bosch Sensortec products by personnel who have special experience +* and training. Do not use this Software if you do not have the +* proper experience or training. +* +* This Software package is provided `` as is `` and without any expressed +* or implied warranties,including without limitation, the implied warranties +* of merchantability and fitness for a particular purpose. +* +* Bosch Sensortec and their representatives and agents deny any liability +* for the functional impairment +* of this Software in terms of fitness, performance and safety. +* Bosch Sensortec and their representatives and agents shall not be liable +* for any direct or indirect damages or injury, except as +* otherwise stipulated in mandatory applicable law. +* +* The Information provided is believed to be accurate and reliable. +* Bosch Sensortec assumes no responsibility for the consequences of use +* of such Information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of Bosch. Specifications mentioned in the Information are +* subject to change without notice. +**************************************************************************/ +/*! \file bma4.h + \brief Sensor Driver for BMA4 family of sensors */ +#ifndef BMA4_H__ +#define BMA4_H__ + +/*********************************************************************/ +/* header files */ + +#include "bma4_defs.h" +#ifdef AKM9916 +#include "aux_akm9916.h" +#endif + +#ifdef BMM150 +#include "aux_bmm150.h" +#endif +/*********************************************************************/ +/* (extern) variable declarations */ + +/*********************************************************************/ +/* function prototype declarations */ +/*! + * @brief This API is the entry point. + * Call this API before using all other APIs. + * This API reads the chip-id of the sensor which is the first step to + * verify the sensor and also it configures the read mechanism of SPI and + * I2C interface. + * + * @param[in,out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + * @note + * While changing the parameter of the bma4 + * consider the following point: + * Changing the reference value of the parameter + * will changes the local copy or local reference + * make sure your changes will not + * affect the reference value of the parameter + * (Better case don't change the reference value of the parameter) + */ +uint16_t bma4_init(struct bma4_dev *dev); + +/*! + * @brief This API is used to write the binary configuration in the sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_write_config_file(struct bma4_dev *dev); + +/*! + * @brief This API checks whether the write operation requested is for + * feature config or register write and accordingly writes the data in the + * sensor. + * + * @note user has to disable the advance power save mode in the sensor when + * using this API in burst write mode. + * bma4_set_advance_power_save(BMA4_DISABLE, dev); + * + * @param[in] addr : Register address. + * @param[in] data : Write data buffer + * @param[in] len : No of bytes to write + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma4_write_regs(uint8_t addr, uint8_t *data, uint8_t len, struct bma4_dev *dev); + +/*! + * @brief This API checks whether the read operation requested is for + * feature or register read and accordingly reads the data from the sensor. + * + * @param[in] addr : Register address. + * @param[in] data : Read data buffer. + * @param[in] len : No of bytes to read. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma4_read_regs(uint8_t addr, uint8_t *data, uint8_t len, struct bma4_dev *dev); + +/*! + * @brief This API reads the error status from the sensor. + * + * Below table mention the types of error which can occur in the sensor + *@verbatim + ************************************************************************* + * Error | Description + *************************|*********************************************** + * | Fatal Error, chip is not in operational + * fatal | state (Boot-, power-system). + * | This flag will be reset only by + * | power-on-reset or soft reset. + *************************|*********************************************** + * cmd | Command execution failed. + *************************|*********************************************** + * | Value Name Description + * error_code | 000 no_error no error + * | 001 acc_err error in + * | ACC_CONF + *************************|*********************************************** + * | Error in FIFO detected: Input data was + * fifo | discarded in stream mode. This flag + * | will be reset when read. + *************************|*********************************************** + * mag | Error in I2C-Master detected. + * | This flag will be reset when read. + ************************************************************************* + *@endverbatim + * + * @param[in,out] err_reg : Pointer to structure variable which stores the + * error status read from the sensor. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma4_get_error_status(struct bma4_err_reg *err_reg, struct bma4_dev *dev); + +/*! + * @brief This API reads the sensor status from the dev sensor. + * + * Below table lists the sensor status flags + * Status | Description + * ----------------------------|---------------------------------------- + * BMA4_MAG_MAN_OP_ONGOING | Manual Mag. interface operation ongoing + * BMA4_CMD_RDY | Command decoder is ready. + * BMA4_MAG_DATA_RDY | Data ready for Mag. + * BMA4_ACC_DATA_RDY | Data ready for Accel. + * + * @param[in] status : Variable used to store the sensor status flags + * which is read from the sensor. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_status(uint8_t *status, struct bma4_dev *dev); + +/*! + * @brief This API reads the Accel data for x,y and z axis from the sensor. + * The data units is in LSB format. + * + * @param[in] accel : Variable used to store the Accel data which is read + * from the sensor. + * @param[in] dev : Structure instance of bma4_dev. + * + * @note For setting the Accel configuration use the below function + * bma4_set_accel_config + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_accel_xyz(struct bma4_accel *accel, struct bma4_dev *dev); + +/*! + * @brief This API reads the sensor time of Sensor time gets updated + * with every update of data register or FIFO. + * + * @param[in] sensor_time : Pointer variable which stores sensor time + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_sensor_time(uint32_t *sensor_time, struct bma4_dev *dev); + +/*! + * @brief This API reads the chip temperature of sensor. + * @note If Accel and Mag are disabled, the temperature value will be set + * to invalid. + * + * @param[out] temp : Pointer variable which stores the temperature value. + * @param[in] temp_unit : indicates the unit of temperature + * temp_unit | description + * ------------|------------------- + * BMA4_DEG | degrees Celsius + * BMA4_FAHREN | degrees fahrenheit + * BMA4_KELVIN | degrees kelvin + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @note Using a scaling factor of 1000, to obtain integer values, which + * at the user end, are used to get accurate temperature value. + * BMA4_SCALE_FARHAN = 1.8 * 1000, BMA4_SCALE_KELVIN = 273.15 * 1000 + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_temperature(int32_t *temp, uint8_t temp_unit, struct bma4_dev *dev); + +/*! + * @brief This API reads the Output data rate, Bandwidth, perf_mode + * and Range of accel. + * + * @param[in,out] accel : Address of user passed structure which is used + * to store the Accel configurations read from the sensor. + * + * @note Enums and corresponding values for structure parameters like + * Odr, Bandwidth and Range are mentioned in the below tables. + * + * Value | Odr + * -----------|------------------------------------ + * 1 | BMA4_OUTPUT_DATA_RATE_0_78HZ + * 2 | BMA4_OUTPUT_DATA_RATE_1_56HZ + * 3 | BMA4_OUTPUT_DATA_RATE_3_12HZ + * 4 | BMA4_OUTPUT_DATA_RATE_6_25HZ + * 5 | BMA4_OUTPUT_DATA_RATE_12_5HZ + * 6 | BMA4_OUTPUT_DATA_RATE_25HZ + * 7 | BMA4_OUTPUT_DATA_RATE_50HZ + * 8 | BMA4_OUTPUT_DATA_RATE_100HZ + * 9 | BMA4_OUTPUT_DATA_RATE_200HZ + * 10 | BMA4_OUTPUT_DATA_RATE_400HZ + * 11 | BMA4_OUTPUT_DATA_RATE_800HZ + * 12 | BMA4_OUTPUT_DATA_RATE_1600HZ + * + * Value | accel_bw + * ------|-------------------------- + * 0 | BMA4_ACCEL_OSR4_AVG1 + * 1 | BMA4_ACCEL_OSR2_AVG2 + * 2 | BMA4_ACCEL_NORMAL_AVG4 + * 3 | BMA4_ACCEL_CIC_AVG8 + * 4 | BMA4_ACCEL_RES_AVG16 + * 5 | BMA4_ACCEL_RES_AVG32 + * 6 | BMA4_ACCEL_RES_AVG64 + * 7 | BMA4_ACCEL_RES_AVG128 + * + * Value | g_range + * --------|--------------------- + * 0x00 | BMA4_ACCEL_RANGE_2G + * 0x01 | BMA4_ACCEL_RANGE_4G + * 0x02 | BMA4_ACCEL_RANGE_8G + * 0x03 | BMA4_ACCEL_RANGE_16G + * + * @param[in] dev : Structure instance of bma4_dev + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_accel_config(struct bma4_accel_config *accel, struct bma4_dev *dev); + +/*! + * @brief This API sets the output_data_rate, bandwidth, perf_mode + * and range of Accel. + * + * @param[in] accel : Pointer to structure variable which specifies the + * Accel configurations. + * + * @note Enums and corresponding values for structure parameters like + * Odr, Bandwidth and Range are mentioned in the below tables. + * Value | ODR + * --------|----------------------------------------- + * 1 | BMA4_OUTPUT_DATA_RATE_0_78HZ + * 2 | BMA4_OUTPUT_DATA_RATE_1_56HZ + * 3 | BMA4_OUTPUT_DATA_RATE_3_12HZ + * 4 | BMA4_OUTPUT_DATA_RATE_6_25HZ + * 5 | BMA4_OUTPUT_DATA_RATE_12_5HZ + * 6 | BMA4_OUTPUT_DATA_RATE_25HZ + * 7 | BMA4_OUTPUT_DATA_RATE_50HZ + * 8 | BMA4_OUTPUT_DATA_RATE_100HZ + * 9 | BMA4_OUTPUT_DATA_RATE_200HZ + * 10 | BMA4_OUTPUT_DATA_RATE_400HZ + * 11 | BMA4_OUTPUT_DATA_RATE_800HZ + * 12 | BMA4_OUTPUT_DATA_RATE_1600HZ + * + * Value | accel_bw + * ------|-------------------------- + * 0 | BMA4_ACCEL_OSR4_AVG1 + * 1 | BMA4_ACCEL_OSR2_AVG2 + * 2 | BMA4_ACCEL_NORMAL_AVG4 + * 3 | BMA4_ACCEL_CIC_AVG8 + * 4 | BMA4_ACCEL_RES_AVG16 + * 5 | BMA4_ACCEL_RES_AVG32 + * 6 | BMA4_ACCEL_RES_AVG64 + * 7 | BMA4_ACCEL_RES_AVG128 + * + * Value | g_range + * --------|--------------------- + * 0x00 | BMA4_ACCEL_RANGE_2G + * 0x01 | BMA4_ACCEL_RANGE_4G + * 0x02 | BMA4_ACCEL_RANGE_8G + * 0x03 | BMA4_ACCEL_RANGE_16G + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_accel_config(const struct bma4_accel_config *accel, struct bma4_dev *dev); + +/*! + * @brief This API sets the advance power save mode in the sensor. + * + * @note If advanced power save is enabled and the Accel and/or + * magnetometer operate in duty cycling mode, the length of the unlatched + * DRDY interrupt pulse is longer than 1/3.2 kHz (312.5 us). + * + * @param[in] adv_pwr_save : The value of advance power save mode + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_advance_power_save(uint8_t adv_pwr_save, + struct bma4_dev *dev); + +/*! + * @brief This API reads the status of advance power save mode + * from the sensor. + * + * @note If the advanced power save is enabled and the Accel and/or + * magnetometer operate in duty cycling mode, the length of the unlatched + * DRDY interrupt pulse is longer than 1/3.2 kHz (312.5 us). + * + * @param[out] adv_pwr_save : The value of advance power save mode + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_advance_power_save(uint8_t *adv_pwr_save, struct bma4_dev *dev); + +/*! + * @brief This API sets the FIFO self wake up functionality in the sensor. + * + * @note Functionality related to FIFO self wake up depends upon the + * advance power save mode. for more info. refer data sheet. + * + * @param[in] fifo_self_wakeup : Variable used to enable or disable + * FIFO self wake up functionality. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_fifo_self_wakeup(uint8_t fifo_self_wakeup, struct bma4_dev *dev); + +/*! + * @brief This API gets the status of FIFO self wake up functionality from + * the sensor. + * + * @note Functionality related to FIFO self wake up depends upon the + * advance power save mode. for more info. refer data sheet. + * + * @param[out] fifo_self_wake_up : Pointer variable used to store the + * fifo self wake up status. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_fifo_self_wakeup(uint8_t *fifo_self_wake_up, struct bma4_dev *dev); + +/*! + * @brief This API enables or disables the Accel in the sensor. + * + * @note Before reading Accel data, user should call this API. + * + * @param[in] accel_en : Variable used to enable or disable the Accel. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_accel_enable(uint8_t accel_en, struct bma4_dev *dev); + +/*! + * @brief This API checks whether Accel is enabled or not in the sensor. + * + * @param[out] accel_en : Pointer variable used to store the Accel enable + * status + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_accel_enable(uint8_t *accel_en, struct bma4_dev *dev); + +/*! + * @brief This API is used to enable or disable auxiliary Mag + * in the sensor. + * + * @note Before reading Mag data, user should call this API. + * + * @param[in] mag_en : Variable used to enable or disable the Mag. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_enable(uint8_t mag_en, struct bma4_dev *dev); + +/*! + * @brief This API is used to check whether the auxiliary Mag is enabled + * or not in the sensor. + * + * @param[out] mag_en : Pointer variable used to store the enable status of + * Mag in the sensor. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_enable(uint8_t *mag_en, struct bma4_dev *dev); + +/*! + * @brief This API reads the SPI interface mode which is set for primary + * interface. + * + * @param[out] spi : Pointer variable which stores the SPI mode selection + * Value | Description + * --------|------------------ + * 0 | SPI 4-wire mode + * 1 | SPI 3-wire mode + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_spi_interface(uint8_t *spi, struct bma4_dev *dev); + +/*! + * @brief This API configures the SPI interface Mode for primary interface + * + * @param[in] spi : The value of SPI mode selection + * Value | Description + * --------|------------------ + * 0 | SPI 4-wire mode + * 1 | SPI 3-wire mode + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_spi_interface(uint8_t spi, struct bma4_dev *dev); + + /*! + * @brief This API writes the available sensor specific commands + * to the sensor. + * + * @param[in] command_reg : The command to write to the command register. + *@verbatim + * value | Description + * --------|------------------------------------------------------ + * 0xB6 | Triggers a soft reset + * 0xB0 | Clears all data in the FIFO, does not change + * | FIFO_CONFIG and FIFO_DOWNS registers + * 0xF0 | Reset acceleration data path + *@endverbatim + * @param[in] dev : Structure instance of bma4_dev. + * + * @note Register will always read as 0x00 + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_command_register(uint8_t command_reg, struct bma4_dev *dev); + +/*! + * @brief This API sets the I2C device address of auxiliary sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_i2c_device_addr(struct bma4_dev *dev); + +/*! + * @brief This API sets the register access on MAG_IF[2], MAG_IF[3], + * MAG_IF[4] in the sensor. This implies that the DATA registers are + * not updated with Mag values automatically. + * + * @param[in] mag_manual : Variable used to specify the Mag manual + * enable status. + * value | mag manual + * ---------|-------------------- + * 0x01 | BMA4_ENABLE + * 0x00 | BMA4_DISABLE + * + * @param[out] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_manual_enable(uint8_t mag_manual, struct bma4_dev *dev); + +/*! + * @brief This API checks whether the Mag access is done manually or + * automatically in the sensor. + * If the Mag access is done through manual mode then Mag data registers + * in sensor are not updated automatically. + * + * @param[out] mag_manual : Mag manual enable value + * value | mag_manual + * --------|------------------- + * 0x01 | BMA4_ENABLE + * 0x00 | BMA4_DISABLE + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_manual_enable(uint8_t *mag_manual, struct bma4_dev *dev); + +/*! + * @brief This API sets the I2C interface configuration(if) mode + * for auxiliary Mag. + * + * @param[in] if_mode : The value of interface configuration mode + * Value | Description + * ------------|------------------------------------------- + * 0 | p_auto_s_off Auxiliary interface:off + * 1 | p_auto_s_mag Auxiliary interface:on + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_aux_if_mode(uint8_t if_mode, struct bma4_dev *dev); + +/*! + * @brief This API gets the address of the register of Aux Mag sensor + * where the data to be read. + * + * @param[out] mag_read_addr : Pointer variable used to store the + * mag read address. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_read_addr(uint8_t *mag_read_addr, struct bma4_dev *dev); + +/*! + * @brief This API sets the address of the register of Aux Mag sensor + * where the data to be read. + * + * @param[in] mag_read_addr: Value of Mag. read address in order to read + * the data from the auxiliary Mag. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_read_addr(uint8_t mag_read_addr, struct bma4_dev *dev); + +/*! + * @brief This API gets the Aux Mag write address from the sensor. + * Mag write address is where the Mag data will be written. + * + * @param[out] mag_write_addr: Pointer used to store the Mag write address + * which is read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_write_addr(uint8_t *mag_write_addr, struct bma4_dev *dev); + +/*! + * @brief This API sets the Aux Mag write address in the sensor. + * Mag write address is where the Mag data will be written. + * + * @param[in] mag_write_addr: Write address of Mag where the data will + * be written. + * @param[out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_write_addr(uint8_t mag_write_addr, struct bma4_dev *dev); + +/*! + * @brief This API reads the data from the sensor which is written to the + * Mag. + * + * @param[out] mag_write_data: Pointer variable which stores the + * data which is written in Mag through sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_write_data(uint8_t *mag_write_data, struct bma4_dev *dev); + +/*! + * @brief This API sets the data in the sensor which in turn will + * be written to Mag. + * + * @param[in] mag_write_data: variable which specify the data which is to + * be written in Mag. + * @param[out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_write_data(uint8_t mag_write_data, struct bma4_dev *dev); + +/*! + * @brief This API reads the x,y,z and r axis data from the auxiliary + * Mag BMM150/AKM9916 sensor. + * + * @param[out] mag : Pointer variable to store the auxiliary Mag x,y,z + * and r axis data read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_mag_xyzr(struct bma4_mag_xyzr *mag, struct bma4_dev *dev); + +/*! + * @brief This API sets the burst data length (1,2,6,8 byte) of auxiliary + * Mag sensor. + * + * @param[in] mag_burst : Variable used to specify the Mag burst read length + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_mag_burst(uint8_t mag_burst, struct bma4_dev *dev); + +/*! + * @brief This API reads the burst data length of Mag set in the sensor. + * + * @param[out] mag_burst : Pointer variable used to store the burst length + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_burst(uint8_t *mag_burst, struct bma4_dev *dev); + +/*! + * @brief This API reads the FIFO data of Accel and/or Mag sensor + * + * @param dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_fifo_data(struct bma4_dev *dev); + +/*! + * @brief This API reads the FIFO water mark level which is set + * in the sensor. + * + * @note The FIFO watermark is issued when the FIFO fill level is + * equal or above the watermark level. + * + * @param[out] fifo_wm : Pointer variable to store FIFO water mark level + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_fifo_wm(uint16_t *fifo_wm, struct bma4_dev *dev); + +/*! + * @brief This API sets the FIFO watermark level in the sensor. + * + * @note The FIFO watermark is issued when the FIFO fill level is + * equal or above the watermark level. + * + * @param[in] fifo_wm : Variable used to set the FIFO water mark level + * @param[out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_fifo_wm(uint16_t fifo_wm, struct bma4_dev *dev); + +/*! + * @brief This API checks whether the Accel FIFO data is set for filtered + * or unfiltered mode. + * + * @param[out] accel_fifo_filter : Variable used to check whether the Accel + * data is filtered or unfiltered. + * Value | accel_fifo_filter + * ---------|------------------------- + * 0x00 | Unfiltered data + * 0x01 | Filtered data + * @param[in] dev : structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_accel_fifo_filter_data(uint8_t *accel_fifo_filter, struct bma4_dev *dev); + +/*! + * @brief This API sets the condition of Accel FIFO data either to + * filtered or unfiltered mode. + * + * @param[in] accel_fifo_filter : Variable used to set the filtered or + * unfiltered condition of Accel FIFO data. + * value | accel_fifo_filter_data + * -----------|------------------------- + * 0x00 | Unfiltered data + * 0x01 | Filtered data + * @param[out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_accel_fifo_filter_data(uint8_t accel_fifo_filter, struct bma4_dev *dev); + +/*! + * @brief This API reads the down sampling rates which is configured + * for Accel FIFO data. + * + * @param[out] fifo_down : Variable used to specify the Accel FIFO + * down-sampling rates + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_fifo_down_accel(uint8_t *fifo_down, struct bma4_dev *dev); + +/*! + * @brief This API sets the down-sampling rates for Accel FIFO. + * + * @param[in] fifo_down : Variable used to specify the Accel FIFO + * down-sampling rates. + * @param[in] dev : structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_fifo_down_accel(uint8_t fifo_down, struct bma4_dev *dev); + +/*! + * @brief This API reads the length of FIFO data available in the sensor + * in the units of bytes. + * + * @note This byte counter is updated each time a complete frame was read + * or written + * + * @param[in] fifo_length : Pointer variable used to store the value of + * fifo byte counter + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_fifo_length(uint16_t *fifo_length, struct bma4_dev *dev); + +/*! + * @brief This API aligns and compensates the Mag data of BMM150/AKM9916 + * sensor. + * + * @param[in] mag_fifo_data: Structure object which stores the Mag x,yand z + * axis FIFO data which is to be aligned and/or compensated. + * @param[in] mag_second_if: Variable used to select the Mag sensor. + * Value | mag_second_if + * --------|---------------------- + * 1 | BMA4_SEC_IF_BMM150 + * 2 | BMA4_SEC_IF_AKM09916 + * + * @param[out] compensated_mag_data: Pointer variable used to store the + * compensated Mag xyz axis data + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_second_if_mag_compensate_xyz(struct bma4_mag_fifo_data mag_fifo_data, + uint8_t mag_second_if, + struct bma4_mag *compensated_mag_data); + +/*! + * @brief This API reads Mag. x,y and z axis data from either BMM150 or + * AKM9916 sensor + * + * @param[out] mag : Structure pointer used to store the Mag x,y, and z axis + * data read from the sensor. + * + * @param[in] sensor_select : Variable used to select the Mag sensor + * Value | Sensor + * ---------|---------------------- + * 0 | BMA4_SEC_IF_NULL + * 1 | BMA4_SEC_IF_BMM150 + * 2 | BMA4_SEC_IF_AKM09916 + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_mag_xyz(struct bma4_mag *mag, uint8_t sensor_select, struct bma4_dev *dev); + +/*! + * @brief This API reads the auxiliary I2C interface configuration which + * is set in the sensor. + * + * @param[out] if_mode : Pointer variable used to store the auxiliary + * interface configuration. + * Value | Description + * ----- |---------------------------------- + * 0x00 | auxiliary interface:off + * 0x01 | auxiliary interface:on + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_if_mode(uint8_t *if_mode, struct bma4_dev *dev); + +/*! + * @brief This API sets the auxiliary interface configuration in the sensor. + * + * @param[in] if_mode : Variable used to select the auxiliary interface + * configuration. + * Value | Description + * ----- |-------------------------- + * 0x00 | auxiliary interface:off + * 0x01 | auxiliary interface:on + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_if_mode(uint8_t if_mode, struct bma4_dev *dev); + +/*! + * @brief This API reads the data ready status of Accel from the sensor. + * @note The status get reset when Accel data register is read. + * + * @param[out] data_rdy : Pointer variable to store the data ready status + * @param[in] dev : structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_accel_data_rdy(uint8_t *data_rdy, struct bma4_dev *dev); + +/*! + * @brief This API reads the data ready status of Mag from the sensor. + * The status get reset when Mag data register is read. + * + * @param[out] data_rdy : Pointer variable to store the data ready status + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_mag_data_rdy(uint8_t *data_rdy, struct bma4_dev *dev); + +/*! + * @brief This API reads the ASIC status from the sensor. + * The status information is mentioned in the below table. + * + *@verbatim + ******************************************************************************* + * Status | Description + **************************|**************************************************** + * sleep | ASIC is in sleep/halt state. + * irq_ovrn | Dedicated interrupt is set again before previous + * | interrupt was acknowledged. + * wc_event | Watchcell event detected (ASIC stopped). + * stream_transfer_active | stream transfer has started. + ******************************************************************************* + *@endverbatim + * + * @param[out] asic_status : Structure pointer used to store the ASIC + * status read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_asic_status(struct bma4_asic_status *asic_status, struct bma4_dev *dev); + +/*! + * @brief This API enables the offset compensation for filtered and + * unfiltered Accel data. + * + * @param[in] offset_en : Variable used to enable or disable offset + * compensation + * offset_en | Description + * ------------|---------------------- + * 0 | BMA4_DISABLE + * 1 | BMA4_ENABLE + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_offset_comp(uint8_t offset_en, struct bma4_dev *dev); + +/*! + * @brief This API gets the status of Accel offset compensation + * + * @param[out] offset_en : Pointer variable used to store the Accel offset + * enable or disable status. + * offset_en | Description + * ----------|-------------- + * 0 | BMA4_DISABLE + * 1 | BMA4_ENABLE + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_offset_comp(uint8_t *offset_en, struct bma4_dev *dev); + +/*! + * @brief This API parses and extracts the accelerometer frames from + * FIFO data read by the "bma4_read_fifo_data" API and stores it in the + * "accel_data" structure instance. + * + * @note The bma4_extract_accel API should be called only after reading + * the FIFO data by calling the bma4_read_fifo_data() API + * + * @param[in,out] accel_data : Structure instance of bma4_accel where + * the accelerometer data in FIFO is stored. + * @param[in,out] accel_length : Number of accelerometer frames + * (x,y,z axes data) + * @param[in,out] dev : Structure instance of bma4_dev. + * + * @note accel_length has the number of accelerometer frames + * (1 accel frame = 6 bytes) which the user needs to extract and store is + * provided as input parameter by the user and the Number of valid + * accelerometer frames extracted and stored is updated in + * "accel_length" at the end of execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_extract_accel(struct bma4_accel *accel_data, uint16_t *accel_length, const struct bma4_dev *dev); + +/*! + * @brief This API parses and extracts the magnetometer frames from + * FIFO data read by the "bma4_read_fifo_data" API and stores it in the + * "mag_data" structure instance parameter of this API + * + * @note The bma4_extract_mag API should be called only after reading + * the FIFO data by calling the bma4_read_fifo_data() API + * + * @param[in,out] mag_data : Structure instance of bma4_mag_xyzr where + * the magnetometer data in FIFO is stored. + * @param[in,out] mag_length : Number of magnetometer frames (x,y,z,r data) + * @param[in,out] dev : Structure instance of bma4_dev. + * + * @note mag_length has the number of magnetometer frames(x,y,z,r data) + * (1 mag frame = 8 bytes) which the user needs to extract and store,It is + * provided as input parameter by the user and the number of valid + * magnetometer frames extracted and stored is updated in + * "mag_length" at the end of execution of this API. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_extract_mag(struct bma4_mag *mag_data, uint16_t *mag_length, const struct bma4_dev *dev); + +/*! + * @brief This API performs Fast Offset Compensation for Accel. + * @param[in] accel_g_value : Array which stores the Accel g units + * for x,y and z axis. + * accel_g_value | Description + * --------------------------|--------------------------------------- + * accel_g_value[0] | x axis g units + * accel_g_value[1] | y axis g units + * accel_g_value[2] | z axis g units + * + * @param[in] dev : structure instance of dev + * + * @note The g-values to be passed to the parameter should be + * multiples of 1000000. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_perform_accel_foc(const int32_t accel_g_value[3], struct bma4_dev *dev); +/*! + * @brief This API checks whether the self test functionality of the sensor + * is working or not + * + * @param[in] result : Pointer variable used to store the result of self test + * operation + * result | Description + * ---------|-------------------- + * 0x00 | BMA4_SELFTEST_PASS + * 0x01 | BMA4_SELFTEST_FAIL + * @param[in] dev : Structure instance of bma4_dev + * + * @return results of self test + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_perform_accel_selftest(uint8_t *result, struct bma4_dev *dev); + +/*! + * @brief This API performs the steps needed for Self test operation + * before reading the Accel Self test data. + * + * @param[in] sign: Variable used to specify the self test sign + * @param[in] dev : Structure instance of bma4_dev + * + * @return results of self test + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_selftest_config(uint8_t sign, struct bma4_dev *dev); + +/*! + * @brief API sets the interrupt to either interrupt1 or + * interrupt2 pin in the sensor. + * + * @param[in] int_line: Variable used to select interrupt pin1 or pin2 + * int_line | interrupt selection + * ---------|------------------- + * 0 | BMA4_INTR1_MAP + * 1 | BMA4_INTR2_MAP + * + * @param[in] int_map: Variable used to select a particular interrupt + * in the sensor + * + * @param[in] enable : Variable used to enable or disable the interrupt + * Value | Behaviour + * ---------|------------------- + * 0x01 | BMA4_ENABLE + * 0x00 | BMA4_DISABLE + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_map_interrupt(uint8_t int_line, uint16_t int_map, uint8_t enable, struct bma4_dev *dev); + +/*! + * @brief This API sets the interrupt mode in the sensor. + * + * @param[in] mode: Variable used to specify the interrupt mode which + * is to be set in the sensor. + * Mode | Value + * ----------------------- |--------- + * BMA4_NON_LATCH_MODE | 0 + * BMA4_LATCH_MODE | 1 + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_interrupt_mode(uint8_t mode, struct bma4_dev *dev); + +/*! + * @brief This API gets the interrupt mode which is set in the sensor. + * + * @param[out] mode: Pointer variable used to store the interrupt mode set in + * in the sensor. + * Mode | Value + * ---------------------|--------------- + * BMA4_NON_LATCH_MODE | 0 + * BMA4_LATCH_MODE | 1 + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_interrupt_mode(uint8_t *mode, struct bma4_dev *dev); + +/*! + * @brief This API sets the auxiliary Mag(BMM150 or AKM9916) output data + * rate and offset. + * + * @param[in] aux_mag : Pointer to structure variable used to specify + * the auxiliary Mag configuration. + *@verbatim + *------------------------------------------------------------------------------ + * Odr | Value + *----------------------------------------|--------------------------------- + * BMA4_OUTPUT_DATA_RATE_0_78HZ | 0x01 + * BMA4_OUTPUT_DATA_RATE_1_56HZ | 0x02 + * BMA4_OUTPUT_DATA_RATE_3_12HZ | 0x03 + * BMA4_OUTPUT_DATA_RATE_6_25HZ | 0x04 + * BMA4_OUTPUT_DATA_RATE_12_5HZ | 0x05 + * BMA4_OUTPUT_DATA_RATE_25HZ | 0x06 + * BMA4_OUTPUT_DATA_RATE_50HZ | 0x07 + * BMA4_OUTPUT_DATA_RATE_100HZ | 0x08 + * BMA4_OUTPUT_DATA_RATE_200HZ | 0x09 + * BMA4_OUTPUT_DATA_RATE_400HZ | 0x0A + * BMA4_OUTPUT_DATA_RATE_800HZ | 0x0B + * BMA4_OUTPUT_DATA_RATE_1600HZ | 0x0C + *------------------------------------------------------------------------------ + * Offset | Value + *--------------------------------------------|--------------------------------- + * BMA4_MAG_OFFSET_MAX | 0x00 + *--------------------------------------------|--------------------------------- + @endverbatim + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_aux_mag_config(const struct bma4_aux_mag_config *aux_mag, struct bma4_dev *dev); + +/*! + * @brief This API reads the auxiliary Mag(BMM150 or AKM9916) output data + * rate and offset. + * @note : Valid output data rates are mentioned in the below table + * + * @param[out] aux_mag : Pointer to structure variable used to store the + * auxiliary Mag configuration read from the sensor + *@verbatim + *------------------------------------------------------------------------ + * Odr | Value + *----------------------------------------|------------------------------- + * BMA4_OUTPUT_DATA_RATE_0_78HZ | 0x01 + * BMA4_OUTPUT_DATA_RATE_1_56HZ | 0x02 + * BMA4_OUTPUT_DATA_RATE_3_12HZ | 0x03 + * BMA4_OUTPUT_DATA_RATE_6_25HZ | 0x04 + * BMA4_OUTPUT_DATA_RATE_12_5HZ | 0x05 + * BMA4_OUTPUT_DATA_RATE_25HZ | 0x06 + * BMA4_OUTPUT_DATA_RATE_50HZ | 0x07 + * BMA4_OUTPUT_DATA_RATE_100HZ | 0x08 + * BMA4_OUTPUT_DATA_RATE_200HZ | 0x09 + * BMA4_OUTPUT_DATA_RATE_400HZ | 0x0A + * BMA4_OUTPUT_DATA_RATE_800HZ | 0x0B + * BMA4_OUTPUT_DATA_RATE_1600HZ | 0x0C + *------------------------------------------------------------------------- + * Offset | Value + *----------------------------------------|-------------------------------- + * BMA4_MAG_OFFSET_MAX | 0x00 + *------------------------------------------------------------------------- + @endverbatim + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_aux_mag_config(struct bma4_aux_mag_config *aux_mag, struct bma4_dev *dev); + +/*! @brief This API sets the FIFO configuration in the sensor. + * + * @param[in] config : Enum variable used to specify the FIFO + * configurations which are to be enabled or disabled in the sensor. + * + * @note : User can set either one or more or all FIFO configurations + * by ORing the below mentioned enums. + * config | Value + * ------------------------|--------------------------- + * BMA4_FIFO_STOP_ON_FULL | 0x01 + * BMA4_FIFO_TIME | 0x02 + * BMA4_FIFO_TAG_INTR2 | 0x04 + * BMA4_FIFO_TAG_INTR1 | 0x08 + * BMA4_FIFO_HEADER | 0x10 + * BMA4_FIFO_MAG | 0x20 + * BMA4_FIFO_ACCEL | 0x40 + * BMA4_FIFO_ALL | 0x7F + * + * @param[in] enable : Parameter used to enable or disable the above + * FIFO configuration + * @param[in] dev : Structure instance of bma4_dev. + * + * @return status of bus communication result + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_set_fifo_config(uint8_t config, uint8_t enable, struct bma4_dev *dev); + +/*! @brief This API reads the FIFO configuration from the sensor. + * + * @param[in] fifo_config : Enum variable used to get the below fifo + * configuration from the sensor. + * + * @note After calling this function user should do the AND operation with + * the enum value populated by this function to know which FIFO + * configuration is enabled. + * fifo_config | Value + * -------------------------|-------------------------- + * BMA4_FIFO_STOP_ON_FULL | 0x01 + * BMA4_FIFO_TIME | 0x02 + * BMA4_FIFO_TAG_INTR2 | 0x04 + * BMA4_FIFO_TAG_INTR1 | 0x08 + * BMA4_FIFO_HEADER | 0x10 + * BMA4_FIFO_MAG | 0x20 + * BMA4_FIFO_ACCEL | 0x40 + * BMA4_FIFO_ALL | 0x7F + * + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_fifo_config(uint8_t *fifo_config, struct bma4_dev *dev); + +/*! @brief This function sets the electrical behaviour of interrupt pin1 or + * pin2 in the sensor. + * + * @param[in] int_pin_config : Pointer to structure variable which specifies + * the configuration data of either interrupt pin1 or 2. + *@verbatim + * ************************************************************************ + * Structure field members | Macros + * ********************************|*************************************** + * edge_ctrl | BMA4_LEVEL_TRIGGER(0) + * | BMA4_EDGE_TRIGGER(1) + * ********************************|*************************************** + * lvl | BMA4_ACTIVE_LOW(0) + * | BMA4_ACTIVE_HIGH(1) + * ********************************|*************************************** + * od | BMA4_PUSH_PULL(0) + * | BMA4_OPEN_DRAIN(1) + * ********************************|*************************************** + * output_en | BMA4_OUTPUT_DISABLE(0) + * | BMA4_OUTPUT_ENABLE(1) + * ********************************|*************************************** + * input_en | BMA4_INPUT_DISABLE(0) + * | BMA4_INPUT_ENABLE(1) + * ************************************************************************ + *@endverbatim + * @param[in] int_line : Variable used to select the interrupt pin1 or + * pin2 for interrupt configuration. + * int_line | Value + * ----------------|---------------------- + * BMA4_INTR1_MAP | 0 + * BMA4_INTR2_MAP | 1 + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma4_set_int_pin_config(const struct bma4_int_pin_config *int_pin_config, uint8_t int_line, + struct bma4_dev *dev); + +/*! @brief This API reads the electrical behavior of interrupt pin1 or pin2 + * from the sensor. + * + * @param[out] int_pin_config : Pointer to structure variable which stores the + * configuration data of either interrupt pin1 or pin2 read from the sensor + *@verbatim + * ************************************************************************ + * Structure field members | Macros + * ************************|*********************************************** + * edge_ctrl | BMA4_LEVEL_TRIGGER(0) + * | BMA4_EDGE_TRIGGER(1) + * ************************|*********************************************** + * lvl | BMA4_ACTIVE_LOW(0) + * | BMA4_ACTIVE_HIGH(1) + * ************************|*********************************************** + * od | BMA4_PUSH_PULL(0) + * | BMA4_OPEN_DRAIN(1) + * ************************|*********************************************** + * output_en | BMA4_OUTPUT_DISABLE(0) + * | BMA4_OUTPUT_ENABLE(1) + * ************************|*********************************************** + * input_en | BMA4_INPUT_DISABLE(0) + * | BMA4_INPUT_ENABLE(1) + * ************************************************************************ + *@endverbatim + * @param[in] int_line : Variable used to select the interrupt pin1 or + * pin2 for interrupt configuration. + * int_line | Value + * -------------------|--------------- + * BMA4_INTR1_MAP | 0 + * BMA4_INTR2_MAP | 1 + * + * @param[in] dev : Structure instance of bma4_dev + * + * @return status of bus communication function + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_get_int_pin_config(struct bma4_int_pin_config *int_pin_config, uint8_t int_line, struct bma4_dev *dev); + +/*! + * @brief This API reads the Feature and Hardware interrupt status from the sensor. + * + * @param[out] int_status : Variable used to get the interrupt status. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_int_status(uint16_t *int_status, struct bma4_dev *dev); + +/*! + * @brief This API reads the Feature interrupt status from the sensor. + * + * @param[out] int_status_0 : Variable used to get the interrupt status. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_int_status_0(uint8_t *int_status_0, struct bma4_dev *dev); + +/*! + * @brief This API reads the Hardware interrupt status from the sensor. + * + * @param[out] int_status_1 : Variable used to get the interrupt status. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_read_int_status_1(uint8_t *int_status_1, struct bma4_dev *dev); + +/*! + * @brief This API initializes the auxiliary interface to access + * auxiliary sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_aux_interface_init(struct bma4_dev *dev); + +/*! + * @brief This API reads the data from the auxiliary sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * @param[in] len : User specified data length + * @param[out] aux_data : Pointer variable to store data read + * @param[in] aux_reg_addr : Variable to pass address from where + * data is to be read + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_aux_read(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev); + +/*! + * @brief This API writes the data into the auxiliary sensor + * + * @param[in] dev : Structure instance of bma4_dev. + * @param[in] len : User specified data length + * @param[out] aux_data : Pointer variable to store data read + * @param[in] aux_reg_addr : Variable to pass address from where + * data is to be written + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + * + */ +uint16_t bma4_aux_write(uint8_t aux_reg_addr, uint8_t *aux_data, uint16_t len, struct bma4_dev *dev); + + +#endif +/* End of __BMA4_H__ */ \ No newline at end of file diff --git a/src/bma423.c b/src/bma423.c new file mode 100644 index 0000000..cc8035b --- /dev/null +++ b/src/bma423.c @@ -0,0 +1,1695 @@ +/**\mainpage +* +**************************************************************************** +* Copyright (C) 2017 - 2018 Bosch Sensortec GmbH +* +* File : bma423.c +* +* Date: 12 Oct 2017 +* +* Revision : 1.1.4 $ +* +* Usage: Sensor Driver for BMA423 sensor +* +**************************************************************************** +* +* \section Disclaimer +* +* Common: +* Bosch Sensortec products are developed for the consumer goods industry. +* They may only be used within the parameters of the respective valid +* product data sheet. Bosch Sensortec products are provided with the +* express understanding that there is no warranty of fitness for a +* particular purpose.They are not fit for use in life-sustaining, +* safety or security sensitive systems or any system or device +* that may lead to bodily harm or property damage if the system +* or device malfunctions. In addition,Bosch Sensortec products are +* not fit for use in products which interact with motor vehicle systems. +* The resale and or use of products are at the purchasers own risk and +* his own responsibility. The examination of fitness for the intended use +* is the sole responsibility of the Purchaser. +* +* The purchaser shall indemnify Bosch Sensortec from all third party +* claims, including any claims for incidental, or consequential damages, +* arising from any product use not covered by the parameters of +* the respective valid product data sheet or not approved by +* Bosch Sensortec and reimburse Bosch Sensortec for all costs in +* connection with such claims. +* +* The purchaser must monitor the market for the purchased products, +* particularly with regard to product safety and inform Bosch Sensortec +* without delay of all security relevant incidents. +* +* Engineering Samples are marked with an asterisk (*) or (e). +* Samples may vary from the valid technical specifications of the product +* series. They are therefore not intended or fit for resale to third +* parties or for use in end products. Their sole purpose is internal +* client testing. The testing of an engineering sample may in no way +* replace the testing of a product series. Bosch Sensortec assumes +* no liability for the use of engineering samples. +* By accepting the engineering samples, the Purchaser agrees to indemnify +* Bosch Sensortec from all claims arising from the use of engineering +* samples. +* +* Special: +* This software module (hereinafter called "Software") and any information +* on application-sheets (hereinafter called "Information") is provided +* free of charge for the sole purpose to support your application work. +* The Software and Information is subject to the following +* terms and conditions: +* +* The Software is specifically designed for the exclusive use for +* Bosch Sensortec products by personnel who have special experience +* and training. Do not use this Software if you do not have the +* proper experience or training. +* +* This Software package is provided `` as is `` and without any expressed +* or implied warranties,including without limitation, the implied warranties +* of merchantability and fitness for a particular purpose. +* +* Bosch Sensortec and their representatives and agents deny any liability +* for the functional impairment +* of this Software in terms of fitness, performance and safety. +* Bosch Sensortec and their representatives and agents shall not be liable +* for any direct or indirect damages or injury, except as +* otherwise stipulated in mandatory applicable law. +* +* The Information provided is believed to be accurate and reliable. +* Bosch Sensortec assumes no responsibility for the consequences of use +* of such Information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of Bosch. Specifications mentioned in the Information are +* subject to change without notice. +**************************************************************************/ +/*! \file bma423.c + \brief Sensor Driver for BMA423 sensor */ + +#include "bma423.h" + +/**\name Feature configuration file */ +const uint8_t bma423_config_file[] = { + 0x80, 0x2e, 0xfc, 0x00, 0x80, 0x2e, 0xfe, 0x00, 0xc8, 0x2e, 0x00, 0x2e, + 0x80, 0x2e, 0xfa, 0x00, 0x80, 0x2e, 0x23, 0xb1, 0x80, 0x2e, 0xfd, 0x00, + 0x80, 0x2e, 0xfb, 0x00, 0x80, 0x2e, 0x5a, 0xb1, 0x50, 0x39, 0x21, 0x2e, + 0xb0, 0xf0, 0x10, 0x30, 0x21, 0x2e, 0x16, 0xf0, 0x80, 0x2e, 0xfc, 0x01, + 0x5d, 0x50, 0x45, 0x52, 0x01, 0x42, 0x3b, 0x80, 0x41, 0x30, 0x01, 0x42, + 0x3c, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x21, 0x2e, 0xff, 0xaf, + 0xb8, 0x2e, 0xb6, 0xd6, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0xfd, 0x2d, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0xf8, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x49, 0x00, 0x24, 0x01, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x98, 0x2e, + 0x99, 0x01, 0x20, 0x26, 0x98, 0x2e, 0xf6, 0x00, 0x98, 0x2e, 0xe9, 0x01, + 0x10, 0x30, 0x21, 0x2e, 0x59, 0xf0, 0x98, 0x2e, 0xd8, 0x00, 0x00, 0x2e, + 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0xdd, 0x00, 0x01, 0x2e, 0x56, 0x00, + 0x00, 0xb2, 0x11, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x56, 0x00, 0x41, 0x50, + 0x98, 0x2e, 0xcc, 0xb0, 0x41, 0x50, 0x98, 0x2e, 0x8f, 0xb4, 0x01, 0x2e, + 0x03, 0xf0, 0x0d, 0xbc, 0x0f, 0xb8, 0x00, 0x90, 0x02, 0x2f, 0x45, 0x50, + 0x21, 0x2e, 0xbc, 0xf0, 0x01, 0x2e, 0x55, 0x00, 0x00, 0xb2, 0x1a, 0x2f, + 0x00, 0x30, 0x21, 0x2e, 0x55, 0x00, 0x43, 0x50, 0x98, 0x2e, 0xcc, 0xb0, + 0x43, 0x50, 0x98, 0x2e, 0xdc, 0xb1, 0x43, 0x50, 0x98, 0x2e, 0x92, 0xb5, + 0x43, 0x50, 0x98, 0x2e, 0x00, 0xb0, 0x01, 0x2e, 0x1c, 0x01, 0x0f, 0xbc, + 0x0f, 0xb8, 0x00, 0x90, 0x45, 0x50, 0x02, 0x2f, 0x21, 0x2e, 0xbc, 0xf0, + 0x02, 0x2d, 0x21, 0x2e, 0xba, 0xf0, 0x98, 0x2e, 0xd8, 0x00, 0xc3, 0x2d, + 0x01, 0x2e, 0x55, 0xf0, 0xc0, 0x2e, 0x21, 0x2e, 0x55, 0xf0, 0x03, 0x2e, + 0x00, 0xf0, 0x45, 0x54, 0x01, 0x2e, 0x59, 0xf0, 0x4a, 0x0e, 0x02, 0x2f, + 0xf1, 0x33, 0x0d, 0x2c, 0x01, 0x08, 0xf2, 0x30, 0x4a, 0x08, 0x79, 0x84, + 0x82, 0xa2, 0x04, 0x2f, 0x02, 0x34, 0x82, 0x0a, 0x47, 0xa2, 0x03, 0x2c, + 0x10, 0x22, 0x45, 0x52, 0x01, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, 0x59, 0xf0, + 0x00, 0x31, 0xc0, 0x2e, 0x21, 0x2e, 0xba, 0xf0, 0xc8, 0x2e, 0xc8, 0x2e, + 0xc8, 0x2e, 0xc8, 0x2e, 0xc8, 0x2e, 0x44, 0x47, 0xaa, 0x00, 0x05, 0x00, + 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, + 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, + 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, + 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, + 0x0e, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x43, 0x28, 0x88, 0x00, + 0x52, 0x00, 0x4f, 0x00, 0x80, 0x00, 0x5b, 0x00, 0x00, 0x40, 0xaf, 0x00, + 0xff, 0x00, 0xff, 0xb7, 0x00, 0x02, 0x00, 0xb0, 0x05, 0x80, 0xb1, 0xf0, + 0xc0, 0x00, 0x00, 0x01, 0x5e, 0xf0, 0x39, 0xf0, 0x89, 0xf0, 0x00, 0x20, + 0xff, 0x7f, 0x7d, 0x00, 0x5e, 0x00, 0x62, 0x00, 0x7c, 0x00, 0xff, 0xfb, + 0x52, 0xf0, 0x56, 0xf0, 0x33, 0x09, 0x33, 0x07, 0x00, 0x08, 0x90, 0x01, + 0x00, 0xf8, 0x67, 0x00, 0x4c, 0x04, 0xa0, 0x00, 0xe8, 0x03, 0x81, 0x00, + 0x82, 0x00, 0x6a, 0x00, 0x6d, 0x00, 0x6c, 0x00, 0xeb, 0x07, 0xae, 0x07, + 0x72, 0x00, 0x6f, 0x00, 0xa1, 0x01, 0x1e, 0x05, 0x47, 0xfd, 0x73, 0x00, + 0x77, 0x00, 0x79, 0x00, 0x76, 0x00, 0xcc, 0x00, 0x30, 0x50, 0x50, 0x40, + 0x00, 0x18, 0x50, 0x40, 0x56, 0x25, 0x47, 0x25, 0x00, 0x18, 0x2e, 0x00, + 0x41, 0x40, 0xa7, 0x02, 0x09, 0x18, 0xc6, 0x00, 0xfb, 0x7f, 0x00, 0x30, + 0x49, 0x52, 0x05, 0x30, 0x05, 0x2c, 0x17, 0x03, 0x1e, 0xbd, 0xd2, 0xba, + 0x92, 0xb8, 0x6a, 0x0b, 0x61, 0x0e, 0xf9, 0x2f, 0x61, 0x1a, 0x01, 0x2f, + 0x5d, 0x0e, 0xf5, 0x2f, 0xd4, 0x7f, 0x02, 0x30, 0x1f, 0x2c, 0xe3, 0x7f, + 0x85, 0x01, 0xd1, 0x03, 0x7c, 0x0e, 0x03, 0x2f, 0x7c, 0x1a, 0x0f, 0x2f, + 0x73, 0x0f, 0x0d, 0x2f, 0xe3, 0x6f, 0xde, 0x04, 0x5f, 0xba, 0x11, 0xbf, + 0xb4, 0x0b, 0xd4, 0x6f, 0x27, 0x07, 0xb3, 0x25, 0xd1, 0xbf, 0xeb, 0x7f, + 0x07, 0x00, 0xb4, 0x25, 0x96, 0x02, 0xdb, 0x7f, 0x2f, 0xbf, 0x9e, 0xbf, + 0x01, 0xb8, 0xd2, 0xba, 0x21, 0xb9, 0x92, 0xb8, 0x06, 0x0a, 0x6f, 0x0b, + 0x40, 0x90, 0xdf, 0x2f, 0x40, 0x91, 0xdd, 0x2f, 0xfb, 0x6f, 0xd0, 0x5f, + 0xb8, 0x2e, 0x57, 0x50, 0x41, 0x30, 0x02, 0x40, 0x51, 0x0a, 0x01, 0x42, + 0x18, 0x82, 0x4b, 0x50, 0x60, 0x42, 0x70, 0x3c, 0x4d, 0x54, 0x42, 0x42, + 0x69, 0x82, 0x82, 0x32, 0x43, 0x40, 0x18, 0x08, 0x02, 0x0a, 0x40, 0x42, + 0x42, 0x80, 0x02, 0x3f, 0x01, 0x40, 0x10, 0x50, 0x4a, 0x08, 0xfb, 0x7f, + 0x11, 0x42, 0x0b, 0x31, 0x0b, 0x42, 0x3e, 0x80, 0x01, 0x32, 0x01, 0x42, + 0x00, 0x2e, 0x01, 0x2e, 0x40, 0xf0, 0x13, 0x90, 0x20, 0x2f, 0x03, 0x30, + 0x51, 0x50, 0x4f, 0x54, 0xf4, 0x34, 0x06, 0x30, 0x55, 0x52, 0x55, 0x32, + 0x1d, 0x1a, 0xe3, 0x22, 0x18, 0x1a, 0x53, 0x58, 0xe3, 0x22, 0x04, 0x30, + 0xd5, 0x40, 0xb5, 0x0d, 0xe1, 0xbe, 0x6f, 0xbb, 0x80, 0x91, 0xa9, 0x0d, + 0x01, 0x89, 0xb5, 0x23, 0x10, 0xa1, 0xf7, 0x2f, 0xda, 0x0e, 0xf4, 0x34, + 0xeb, 0x2f, 0x01, 0x2e, 0x25, 0x00, 0x70, 0x1a, 0x00, 0x30, 0x21, 0x30, + 0x02, 0x2c, 0x08, 0x22, 0x30, 0x30, 0x00, 0xb2, 0x06, 0x2f, 0x21, 0x2e, + 0x59, 0xf0, 0x98, 0x2e, 0xd8, 0x00, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, + 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb1, 0xf0, 0x59, 0x52, + 0x01, 0x0a, 0x21, 0x2e, 0xb1, 0xf0, 0x01, 0x2e, 0x1c, 0x01, 0x0f, 0xbc, + 0x0f, 0xb8, 0x00, 0x90, 0x45, 0x50, 0x02, 0x2f, 0xc0, 0x2e, 0x21, 0x2e, + 0xbc, 0xf0, 0xc0, 0x2e, 0x21, 0x2e, 0xba, 0xf0, 0x1a, 0x24, 0x26, 0x00, + 0x80, 0x2e, 0x8f, 0x00, 0x03, 0x2e, 0x01, 0x01, 0x05, 0x2e, 0x01, 0x01, + 0x92, 0xbd, 0x20, 0x50, 0x03, 0x2e, 0x01, 0x01, 0xbf, 0xba, 0x21, 0xbd, + 0x2f, 0xbb, 0x1f, 0xba, 0x40, 0x91, 0xf0, 0x7f, 0x04, 0x2f, 0x80, 0x91, + 0x02, 0x2f, 0x00, 0xb3, 0x90, 0x2e, 0xc7, 0xb0, 0x03, 0x2e, 0x7b, 0x00, + 0x01, 0x80, 0x40, 0x90, 0x14, 0x2f, 0x41, 0x84, 0xf1, 0x6f, 0x25, 0x2e, + 0x7b, 0x00, 0x41, 0x40, 0x23, 0x2e, 0x5a, 0x00, 0x47, 0x52, 0x12, 0x40, + 0x52, 0x42, 0x02, 0x30, 0x00, 0x40, 0x40, 0x42, 0xe0, 0x5f, 0x25, 0x2e, + 0x57, 0x00, 0x25, 0x2e, 0x58, 0x00, 0x25, 0x2e, 0x5d, 0x00, 0xb8, 0x2e, + 0x07, 0x2e, 0x00, 0x01, 0x03, 0x2e, 0x01, 0x01, 0x05, 0x2e, 0x00, 0x01, + 0x24, 0xbd, 0x0f, 0x2e, 0x59, 0x00, 0xb5, 0xbd, 0x93, 0xbc, 0x2f, 0xb9, + 0xb5, 0xb9, 0x93, 0xb8, 0x3a, 0x1a, 0x06, 0x2f, 0x07, 0x30, 0x25, 0x2e, + 0x59, 0x00, 0x2f, 0x2e, 0x57, 0x00, 0x2f, 0x2e, 0x58, 0x00, 0x40, 0xb3, + 0x05, 0x30, 0x07, 0x30, 0x0a, 0x2f, 0xf7, 0x6f, 0xe6, 0x7f, 0x00, 0x2e, + 0xc6, 0x41, 0x0f, 0x2e, 0x5a, 0x00, 0xb7, 0x05, 0x80, 0xa9, 0xee, 0x05, + 0xf7, 0x23, 0xe6, 0x6f, 0x80, 0xb3, 0x06, 0x30, 0x09, 0x2f, 0xe7, 0x7f, + 0x00, 0x2e, 0x06, 0x40, 0x0f, 0x2e, 0x5b, 0x00, 0xb7, 0x05, 0x80, 0xa9, + 0xee, 0x05, 0xb7, 0x23, 0xe7, 0x6f, 0x00, 0xb3, 0x04, 0x30, 0x0b, 0x2f, + 0xf4, 0x6f, 0x02, 0x89, 0xe7, 0x7f, 0x00, 0x2e, 0x04, 0x41, 0x0f, 0x2e, + 0x5c, 0x00, 0x27, 0x05, 0x00, 0xa9, 0xec, 0x05, 0x27, 0x23, 0xe7, 0x6f, + 0x7b, 0x0f, 0x17, 0x30, 0x0b, 0x2f, 0x73, 0x0f, 0x05, 0x30, 0x17, 0x30, + 0x07, 0x2f, 0x63, 0x0f, 0x15, 0x30, 0x17, 0x30, 0x00, 0x2f, 0x07, 0x30, + 0xe3, 0x0e, 0x00, 0x2f, 0x05, 0x30, 0x80, 0x90, 0x05, 0x2e, 0x57, 0x00, + 0x13, 0x30, 0x13, 0x29, 0xf2, 0x6f, 0x47, 0x5c, 0x17, 0x2f, 0xc0, 0x91, + 0x05, 0x30, 0x0b, 0x2f, 0x07, 0x2e, 0x58, 0x00, 0xc1, 0x86, 0x2b, 0x2e, + 0x57, 0x00, 0x59, 0x0e, 0x27, 0x2e, 0x58, 0x00, 0x24, 0x2f, 0x2b, 0x2e, + 0x5d, 0x00, 0x22, 0x2d, 0x61, 0x0e, 0x29, 0x2e, 0x57, 0x00, 0x2b, 0x2e, + 0x58, 0x00, 0x1b, 0x2f, 0x27, 0x2e, 0x5d, 0x00, 0x19, 0x2d, 0x40, 0x91, + 0x05, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0x57, 0x00, 0x23, 0x2e, 0x5d, 0x00, + 0x06, 0x2d, 0x29, 0x2e, 0x57, 0x00, 0x61, 0x0e, 0x01, 0x2f, 0x27, 0x2e, + 0x5d, 0x00, 0x81, 0x40, 0x23, 0x2e, 0x5a, 0x00, 0x30, 0x25, 0x47, 0x52, + 0xd4, 0x40, 0x54, 0x42, 0x00, 0x2e, 0xc3, 0x40, 0x43, 0x42, 0x00, 0x2e, + 0x03, 0x2e, 0x5d, 0x00, 0x40, 0xb2, 0x0d, 0x2f, 0x81, 0x40, 0x23, 0x2e, + 0x5a, 0x00, 0x11, 0x40, 0x91, 0x43, 0x01, 0x34, 0x00, 0x40, 0x80, 0x43, + 0x23, 0x2e, 0x5e, 0xf0, 0x03, 0x2d, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, + 0xe0, 0x5f, 0xb8, 0x2e, 0x50, 0x50, 0xf0, 0x7f, 0x1a, 0x25, 0x13, 0x40, + 0x7b, 0x84, 0xe0, 0x7f, 0x83, 0x42, 0x35, 0x30, 0x11, 0x40, 0x04, 0x40, + 0xc1, 0x7f, 0xd4, 0x7f, 0x86, 0x31, 0x07, 0x2e, 0x59, 0xf0, 0x03, 0x2e, + 0x1f, 0x01, 0x0d, 0x09, 0x02, 0xab, 0x05, 0x30, 0x8e, 0x09, 0x2c, 0x23, + 0xe3, 0xba, 0x42, 0xab, 0x16, 0x30, 0x75, 0x23, 0x59, 0x5c, 0x8e, 0x09, + 0x66, 0xbb, 0x82, 0xab, 0x27, 0x30, 0xbe, 0x23, 0x3e, 0x80, 0x25, 0x1a, + 0x06, 0x2f, 0x2e, 0x1a, 0x04, 0x2f, 0x26, 0x1a, 0x02, 0x2f, 0xf7, 0x3d, + 0x03, 0x2c, 0xdf, 0x08, 0x07, 0x32, 0xdf, 0x0a, 0x14, 0x01, 0x55, 0x01, + 0x04, 0x41, 0x14, 0x42, 0x16, 0x01, 0x42, 0x41, 0x45, 0x30, 0x4d, 0x09, + 0x04, 0x41, 0x12, 0x42, 0x04, 0x42, 0x40, 0xb3, 0x04, 0x2f, 0xf0, 0x6f, + 0x02, 0x30, 0x04, 0x40, 0x94, 0x04, 0x02, 0x42, 0x00, 0x32, 0x08, 0x08, + 0x00, 0xb2, 0x00, 0x30, 0x05, 0x2f, 0xe2, 0x6f, 0x00, 0x2e, 0x84, 0x40, + 0x04, 0x05, 0x84, 0x42, 0x00, 0x2e, 0x5b, 0x54, 0x4a, 0x08, 0x40, 0xb2, + 0xf1, 0x6f, 0x04, 0x2f, 0x42, 0x82, 0x00, 0x2e, 0x42, 0x40, 0x02, 0x04, + 0x40, 0x42, 0xb0, 0x5f, 0x27, 0x2e, 0x59, 0xf0, 0xb8, 0x2e, 0x50, 0x50, + 0xf7, 0x7f, 0x00, 0x2e, 0x0f, 0x2e, 0xb8, 0xf0, 0xf8, 0xbf, 0xff, 0xbb, + 0xc0, 0xb3, 0x2a, 0x2f, 0x0f, 0x2e, 0x01, 0xf0, 0xfe, 0xbf, 0xe6, 0x7f, + 0x7e, 0xbb, 0xd5, 0x7f, 0x37, 0x30, 0x5f, 0x5a, 0xbe, 0x05, 0x67, 0x41, + 0xc4, 0x7f, 0x78, 0xbe, 0x47, 0x41, 0x27, 0x0b, 0xb3, 0x7f, 0xe6, 0x11, + 0x41, 0x56, 0x43, 0x89, 0xd7, 0x42, 0x00, 0x2e, 0x27, 0x41, 0x05, 0x41, + 0xf8, 0xbf, 0x7d, 0x0b, 0x6e, 0x11, 0x03, 0x8f, 0xd5, 0x42, 0x14, 0x30, + 0xe5, 0x41, 0xc7, 0x41, 0xd8, 0xbe, 0x6f, 0x0b, 0x6e, 0x11, 0xc5, 0x42, + 0x29, 0x2e, 0x56, 0x00, 0x45, 0x56, 0x27, 0x2e, 0xb8, 0xf0, 0xe6, 0x6f, + 0xd5, 0x6f, 0xc4, 0x6f, 0xb3, 0x6f, 0xf7, 0x6f, 0xb0, 0x5f, 0xc8, 0x2e, + 0x50, 0x50, 0xe5, 0x7f, 0xd7, 0x7f, 0xf6, 0x7f, 0x36, 0x30, 0x0b, 0x2e, + 0x01, 0xf0, 0xde, 0xbe, 0xde, 0xbb, 0x61, 0x5a, 0xb7, 0x05, 0x67, 0x41, + 0xc4, 0x7f, 0x78, 0xbe, 0x47, 0x41, 0x27, 0x0b, 0xb3, 0x7f, 0xe6, 0x11, + 0x43, 0x56, 0x43, 0x89, 0xd7, 0x42, 0x00, 0x2e, 0x27, 0x41, 0x05, 0x41, + 0xf8, 0xbf, 0x7d, 0x0b, 0x6e, 0x11, 0x03, 0x8f, 0xd5, 0x42, 0x14, 0x30, + 0xe5, 0x41, 0xc7, 0x41, 0xd8, 0xbe, 0x6f, 0x0b, 0x6e, 0x11, 0xc5, 0x42, + 0x29, 0x2e, 0x55, 0x00, 0x03, 0x31, 0x27, 0x2e, 0xb8, 0xf0, 0xf6, 0x6f, + 0xe5, 0x6f, 0xd7, 0x6f, 0xc4, 0x6f, 0xb3, 0x6f, 0xb0, 0x5f, 0xc8, 0x2e, + 0x40, 0x50, 0xf6, 0x7f, 0x1a, 0x18, 0x63, 0x56, 0x33, 0x00, 0x06, 0x30, + 0xfe, 0x03, 0x0e, 0xb8, 0xf2, 0xbf, 0x07, 0x0a, 0x2a, 0x18, 0x63, 0x5a, + 0xb5, 0x01, 0x03, 0x30, 0xfb, 0x03, 0x6e, 0xbb, 0xf2, 0xbf, 0xe1, 0x7f, + 0xf7, 0x0b, 0x56, 0x40, 0x36, 0x25, 0x46, 0x40, 0x06, 0x28, 0xc7, 0x7f, + 0x22, 0x18, 0xd1, 0x7f, 0xb5, 0x00, 0x01, 0x30, 0x39, 0x03, 0x2e, 0xb9, + 0x42, 0xbe, 0x14, 0x0b, 0xf2, 0x6f, 0x10, 0x18, 0xb5, 0x00, 0xb9, 0x03, + 0x2e, 0xb9, 0x62, 0xbf, 0x96, 0x0a, 0xb6, 0x6f, 0x30, 0x18, 0x75, 0x01, + 0xb9, 0x03, 0x5c, 0x28, 0xe2, 0xbf, 0xde, 0xb9, 0xd6, 0x6f, 0xdf, 0x0a, + 0x8a, 0x28, 0xc4, 0x6f, 0x82, 0x43, 0x23, 0x29, 0xe5, 0x6f, 0xc0, 0x2e, + 0x44, 0x43, 0xc0, 0x5f, 0x40, 0x50, 0xd0, 0x7f, 0x4a, 0x17, 0x00, 0x40, + 0x01, 0x18, 0x46, 0x25, 0x07, 0x25, 0x65, 0x56, 0xd9, 0x04, 0x53, 0x18, + 0xeb, 0x18, 0x05, 0x30, 0x49, 0x16, 0x69, 0x06, 0xca, 0x18, 0xa6, 0x00, + 0xc7, 0x02, 0x65, 0x58, 0xcb, 0x7f, 0x98, 0x2e, 0x7f, 0xb6, 0xcb, 0x6f, + 0xd2, 0x6f, 0xc0, 0x2e, 0x80, 0x42, 0xc0, 0x5f, 0x09, 0x2e, 0x1b, 0x01, + 0x05, 0x2e, 0x1b, 0x01, 0xa3, 0xbc, 0x44, 0xbe, 0x90, 0x50, 0x4f, 0xb9, + 0x07, 0x2e, 0x1b, 0x01, 0x4a, 0x25, 0x9f, 0xb8, 0x39, 0x8f, 0xb2, 0xbd, + 0xf2, 0x7f, 0xbf, 0xb9, 0xeb, 0x7f, 0x8a, 0x0a, 0x37, 0x89, 0x0b, 0x30, + 0x93, 0x0a, 0x8b, 0x7f, 0xcb, 0x43, 0x0b, 0x43, 0x80, 0xb2, 0xd3, 0x7f, + 0xc1, 0x7f, 0x90, 0x2e, 0x87, 0xb2, 0x20, 0x25, 0x01, 0x2e, 0x64, 0x00, + 0x01, 0x90, 0x0e, 0x2f, 0x67, 0x52, 0x01, 0x2e, 0x61, 0x00, 0xb4, 0x7f, + 0xa2, 0x7f, 0x98, 0x2e, 0x8d, 0xb2, 0x00, 0x30, 0x21, 0x2e, 0x64, 0x00, + 0xc1, 0x6f, 0xd3, 0x6f, 0xa2, 0x6f, 0xb4, 0x6f, 0x0b, 0x30, 0x01, 0x2e, + 0x1b, 0x01, 0x06, 0xbc, 0x06, 0xbb, 0x57, 0x25, 0x01, 0x2e, 0x1b, 0x01, + 0x94, 0xb1, 0x05, 0xbc, 0xb6, 0x7f, 0x0f, 0xbb, 0x6b, 0x50, 0x80, 0xb3, + 0x0f, 0x2f, 0x0d, 0x2e, 0x1b, 0x01, 0x6f, 0x5e, 0xb7, 0x09, 0x2d, 0x2e, + 0x1b, 0x01, 0x71, 0x5c, 0x69, 0x5e, 0x9b, 0x43, 0x9b, 0x43, 0xdb, 0x43, + 0x9b, 0x43, 0x1b, 0x42, 0xcb, 0x43, 0x0b, 0x42, 0x8b, 0x43, 0x40, 0xb2, + 0x05, 0x2f, 0x69, 0x50, 0x00, 0x2e, 0x16, 0x40, 0x0b, 0x40, 0x76, 0x7f, + 0x8b, 0x7f, 0xcb, 0x0a, 0x01, 0x2e, 0x61, 0x00, 0x67, 0x52, 0x6d, 0x5c, + 0x98, 0x2e, 0xd3, 0xb2, 0x90, 0x6f, 0x00, 0xb2, 0x0b, 0x2f, 0xf0, 0x6f, + 0x00, 0xb2, 0x08, 0x2f, 0x69, 0x58, 0x6b, 0x50, 0x12, 0x41, 0x12, 0x42, + 0x21, 0x30, 0x04, 0x41, 0x04, 0x42, 0x23, 0x2e, 0x5e, 0xf0, 0xc0, 0x6f, + 0x00, 0xb2, 0x26, 0x2f, 0x74, 0x6f, 0x80, 0x6f, 0x71, 0x54, 0x88, 0xbd, + 0xc8, 0xb8, 0x4b, 0x0a, 0x94, 0x42, 0x91, 0x42, 0x90, 0x42, 0x88, 0xba, + 0x69, 0x52, 0xf3, 0x6f, 0x54, 0x42, 0x85, 0x42, 0xc0, 0x90, 0x40, 0x42, + 0x15, 0x2f, 0x6b, 0x52, 0x00, 0x2e, 0x52, 0x40, 0x41, 0x40, 0xa2, 0x04, + 0x41, 0x06, 0x40, 0xaa, 0x04, 0x2f, 0x40, 0x90, 0x0b, 0x2f, 0xb1, 0x6f, + 0x4a, 0x0f, 0x08, 0x2f, 0xb2, 0x6f, 0x80, 0xb2, 0x05, 0x2f, 0x6b, 0x54, + 0x21, 0x30, 0x94, 0x42, 0x80, 0x42, 0x23, 0x2e, 0x5e, 0xf0, 0xd0, 0x6f, + 0x00, 0xb2, 0x13, 0x2f, 0x01, 0x2e, 0x60, 0x00, 0x09, 0x2e, 0x7c, 0x00, + 0x04, 0x1a, 0x0d, 0x2f, 0x73, 0x50, 0x29, 0x2e, 0x60, 0x00, 0x24, 0x42, + 0x44, 0x30, 0x02, 0x40, 0x02, 0x42, 0x09, 0x80, 0x00, 0x2e, 0x04, 0x42, + 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x64, 0x00, 0xeb, 0x6f, 0x70, 0x5f, + 0xb8, 0x2e, 0x09, 0x86, 0x49, 0x54, 0xe4, 0x40, 0xc3, 0x80, 0x94, 0x04, + 0xc3, 0x40, 0x13, 0x05, 0x05, 0x40, 0x25, 0x05, 0x8a, 0x17, 0x73, 0x30, + 0x73, 0x09, 0x8c, 0x17, 0xf3, 0x08, 0xe3, 0x00, 0x4c, 0x82, 0x95, 0x00, + 0xb3, 0xb5, 0x23, 0xb5, 0x53, 0x42, 0x52, 0x42, 0x53, 0x42, 0x42, 0x42, + 0x71, 0x82, 0x75, 0x54, 0x52, 0x42, 0x10, 0x50, 0x77, 0x54, 0x52, 0x42, + 0xfb, 0x7f, 0x22, 0x30, 0x79, 0x56, 0x43, 0x42, 0x44, 0x82, 0x0b, 0x30, + 0x52, 0x42, 0x5b, 0x42, 0x7c, 0x84, 0x4b, 0x42, 0x35, 0x82, 0x8c, 0x80, + 0x8b, 0x42, 0x0b, 0x42, 0x39, 0x80, 0x04, 0x30, 0x0b, 0x42, 0x37, 0x80, + 0x15, 0x30, 0x60, 0x25, 0x98, 0x2e, 0xc6, 0xb2, 0x8b, 0x83, 0xfb, 0x6f, + 0x65, 0x42, 0xc0, 0x2e, 0x44, 0x42, 0xf0, 0x5f, 0x05, 0x80, 0x02, 0x30, + 0x51, 0x82, 0x02, 0x42, 0x13, 0x30, 0x41, 0x40, 0x4b, 0x08, 0x7b, 0x54, + 0x3e, 0x80, 0x51, 0x14, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x40, 0x51, + 0xd1, 0x7f, 0x12, 0x25, 0x02, 0x30, 0x42, 0x43, 0x32, 0x30, 0x82, 0x43, + 0xc6, 0x7f, 0xe5, 0x7f, 0xb4, 0x7f, 0xa3, 0x7f, 0x90, 0x7f, 0x8b, 0x7f, + 0x98, 0x2e, 0x54, 0x01, 0xc0, 0x7e, 0x00, 0xac, 0x01, 0x2f, 0x65, 0x50, + 0xc0, 0x7e, 0x00, 0x2e, 0x90, 0x6f, 0x09, 0x8a, 0xd1, 0x6f, 0x75, 0x7f, + 0x4c, 0x82, 0x63, 0x41, 0x65, 0x7f, 0x11, 0x7f, 0x00, 0x2e, 0x64, 0x41, + 0x44, 0x85, 0x52, 0x7f, 0x45, 0x7f, 0x00, 0x2e, 0xa6, 0x40, 0x80, 0x40, + 0x32, 0x7f, 0x82, 0x8e, 0xc2, 0x6e, 0x45, 0x41, 0xf0, 0x7f, 0x27, 0x7f, + 0x02, 0x7f, 0x98, 0x2e, 0x8a, 0xb1, 0x23, 0x6f, 0xd1, 0x6f, 0xc2, 0x40, + 0xf9, 0x86, 0x23, 0x7f, 0x80, 0xb2, 0xe0, 0x7e, 0x0f, 0x2f, 0x32, 0x6f, + 0x64, 0x6f, 0x82, 0x40, 0xf2, 0x7f, 0x4e, 0x82, 0x42, 0x6f, 0x50, 0x6f, + 0x73, 0x6f, 0x85, 0x40, 0xc3, 0x40, 0x04, 0x41, 0x06, 0x40, 0xe2, 0x6e, + 0x98, 0x2e, 0x8a, 0xb1, 0xe0, 0x7e, 0xf3, 0x31, 0x10, 0x6f, 0x36, 0x80, + 0xe1, 0x6e, 0x02, 0x40, 0x71, 0x7f, 0x51, 0x04, 0x02, 0x30, 0x40, 0xa8, + 0x91, 0x04, 0x4a, 0x22, 0x89, 0x16, 0x93, 0x08, 0x4a, 0x00, 0x95, 0xb4, + 0x09, 0x18, 0x8e, 0x16, 0x13, 0x30, 0x93, 0x08, 0x21, 0x6f, 0x60, 0x7f, + 0x4d, 0x86, 0x02, 0x80, 0xb2, 0x00, 0x41, 0x40, 0x21, 0xb5, 0x50, 0x7f, + 0x43, 0x7f, 0x98, 0x2e, 0xc2, 0xb1, 0x40, 0x6f, 0x62, 0x6f, 0x55, 0x6f, + 0x13, 0x40, 0x84, 0x40, 0x01, 0x40, 0x45, 0x41, 0x42, 0xbe, 0x1d, 0x18, + 0x4c, 0x04, 0x31, 0x0f, 0x04, 0x8a, 0xc0, 0x6f, 0x11, 0x30, 0x02, 0x2f, + 0x00, 0x2e, 0x03, 0x2c, 0x01, 0x42, 0x23, 0x30, 0x03, 0x42, 0x00, 0x2e, + 0xd6, 0x6f, 0x44, 0x41, 0x8a, 0x87, 0x76, 0x8b, 0x00, 0xb3, 0x53, 0x7f, + 0x15, 0x2f, 0x04, 0x6f, 0x7d, 0x5e, 0x8b, 0x8d, 0xe7, 0x01, 0xc0, 0xa5, + 0x84, 0x41, 0x01, 0x2f, 0x00, 0xa1, 0x03, 0x2f, 0xc0, 0xad, 0x08, 0x2f, + 0x00, 0xa5, 0x06, 0x2f, 0xc6, 0x40, 0x81, 0x8d, 0x07, 0x30, 0x3c, 0x05, + 0xd6, 0x42, 0x04, 0x2c, 0xc4, 0x42, 0x02, 0x2c, 0x07, 0x30, 0x07, 0x30, + 0x86, 0x86, 0x94, 0x6f, 0xd7, 0x7e, 0x0e, 0x8d, 0x00, 0x40, 0x74, 0x89, + 0xc7, 0x40, 0x02, 0xb2, 0xf9, 0x29, 0x45, 0x41, 0x86, 0x41, 0xbe, 0x80, + 0x21, 0x41, 0x75, 0x23, 0x82, 0x40, 0xc7, 0x42, 0x45, 0x7f, 0x34, 0x7f, + 0x20, 0x7f, 0x98, 0x2e, 0xc2, 0xb1, 0x31, 0x6f, 0x60, 0x6f, 0x24, 0x6f, + 0x22, 0x40, 0x05, 0x41, 0x43, 0x40, 0x13, 0x01, 0x43, 0x86, 0xac, 0x0f, + 0xd1, 0x6f, 0x30, 0x7f, 0x00, 0x2f, 0x44, 0x42, 0x48, 0x8a, 0x41, 0x88, + 0xe1, 0x40, 0x13, 0x7f, 0x04, 0x7f, 0xf5, 0x7e, 0x98, 0x2e, 0xc2, 0xb1, + 0x11, 0x6f, 0x60, 0x6f, 0x34, 0x6f, 0x42, 0x40, 0x03, 0x40, 0x9a, 0x04, + 0x04, 0x41, 0x43, 0x82, 0xa2, 0x0e, 0x03, 0x6f, 0x00, 0x2f, 0xc2, 0x42, + 0x00, 0x2e, 0x41, 0x40, 0x72, 0x6f, 0x98, 0x2e, 0xc2, 0xb1, 0x25, 0x6f, + 0x72, 0x6f, 0x53, 0x41, 0x93, 0x0e, 0xd1, 0x6f, 0x46, 0x80, 0x1b, 0x30, + 0x03, 0x30, 0x0c, 0x2f, 0x04, 0x40, 0x00, 0x91, 0x42, 0x42, 0x08, 0x2f, + 0xf6, 0x6e, 0x44, 0x6f, 0x86, 0x41, 0xb4, 0x0e, 0x03, 0x2f, 0x02, 0x88, + 0xdb, 0x7e, 0x03, 0x43, 0x0b, 0x42, 0x46, 0x8d, 0x44, 0x41, 0x47, 0x80, + 0x05, 0x6f, 0x94, 0x0f, 0x76, 0x7f, 0x60, 0x7f, 0x02, 0x2f, 0x45, 0x89, + 0x42, 0x43, 0x03, 0x43, 0x49, 0x88, 0xa5, 0x6f, 0x40, 0x91, 0xa4, 0x7f, + 0x15, 0x30, 0xe2, 0x6f, 0xd3, 0x6e, 0x03, 0x2f, 0x04, 0x30, 0x83, 0x42, + 0x80, 0x2e, 0x77, 0xb4, 0x04, 0x40, 0x25, 0x29, 0x04, 0x42, 0x83, 0x42, + 0x45, 0x82, 0x94, 0x6f, 0x04, 0x85, 0xc0, 0xb2, 0x90, 0x2e, 0x63, 0xb4, + 0x15, 0x87, 0x3c, 0x8c, 0xc4, 0x40, 0x46, 0x7f, 0xc2, 0x86, 0x07, 0x40, + 0x86, 0x41, 0xf4, 0xbf, 0x00, 0xb3, 0x0c, 0x2f, 0x90, 0x6f, 0x16, 0x80, + 0x46, 0x25, 0x00, 0x40, 0x57, 0x25, 0x04, 0x18, 0xae, 0x0e, 0x10, 0x30, + 0x06, 0x30, 0x75, 0x25, 0x46, 0x23, 0x60, 0x6f, 0x64, 0x25, 0xc4, 0x40, + 0xfa, 0x86, 0x00, 0xb3, 0x33, 0x7f, 0x09, 0x2f, 0x93, 0x6f, 0xd8, 0x88, + 0x53, 0x6f, 0x04, 0x41, 0xc3, 0x40, 0xdc, 0x0e, 0x13, 0x30, 0x04, 0x30, + 0xdc, 0x22, 0xb3, 0x25, 0x40, 0xb3, 0x02, 0x2f, 0x3b, 0x25, 0xc0, 0x90, + 0x05, 0x2f, 0x91, 0x6f, 0xd0, 0x6f, 0x98, 0x2e, 0xc6, 0xb2, 0x4d, 0x2c, + 0x04, 0x30, 0x8d, 0x88, 0x43, 0x40, 0x82, 0x40, 0x54, 0x7f, 0xda, 0x0f, + 0x04, 0x30, 0x08, 0x2f, 0xc1, 0x80, 0x40, 0x42, 0xc2, 0x0f, 0x02, 0x2f, + 0x00, 0x30, 0xc0, 0x7e, 0x1b, 0x2d, 0xc0, 0x7e, 0x19, 0x2d, 0xe1, 0xbc, + 0x92, 0x6f, 0x4f, 0x04, 0x90, 0x84, 0x40, 0xa8, 0x21, 0x05, 0x83, 0x40, + 0x4c, 0x22, 0x4b, 0x0e, 0xb6, 0x84, 0x21, 0x30, 0x02, 0x2f, 0x11, 0x30, + 0x04, 0x2c, 0xc1, 0x7e, 0xe3, 0x6f, 0xc1, 0x7e, 0xc1, 0x42, 0x00, 0x2e, + 0x00, 0x40, 0x81, 0x40, 0x04, 0xbd, 0x40, 0x6f, 0x98, 0x2e, 0xc2, 0xb1, + 0x50, 0x6f, 0x11, 0x30, 0x02, 0x40, 0x51, 0x08, 0xc3, 0x6e, 0x03, 0x80, + 0x99, 0x15, 0x0b, 0x40, 0xb1, 0x6f, 0xd0, 0x6f, 0xb6, 0x7f, 0x5b, 0x7f, + 0x04, 0x30, 0x4d, 0x54, 0x03, 0x30, 0x11, 0x2c, 0x10, 0x80, 0x55, 0x6f, + 0x06, 0x40, 0x75, 0x01, 0x58, 0xbb, 0x6a, 0x09, 0x05, 0x42, 0xc1, 0x86, + 0x47, 0x40, 0x51, 0x25, 0xbe, 0x01, 0x56, 0x43, 0x00, 0x2e, 0x46, 0x41, + 0xf4, 0x03, 0xb6, 0x6f, 0x47, 0x43, 0x5e, 0x0e, 0xed, 0x2f, 0x31, 0x6f, + 0x60, 0x6f, 0x42, 0x40, 0x15, 0x30, 0x02, 0x82, 0x95, 0x08, 0x04, 0x42, + 0x52, 0x42, 0x02, 0x2c, 0x44, 0x42, 0x04, 0x30, 0x3e, 0x8e, 0x91, 0x6f, + 0x4f, 0x8c, 0x02, 0x40, 0x83, 0x41, 0xb5, 0x8d, 0x93, 0x0e, 0xd0, 0x6f, + 0x01, 0x2f, 0x98, 0x2e, 0xc6, 0xb2, 0x00, 0x2e, 0xc0, 0x41, 0x81, 0x41, + 0xc1, 0x0f, 0xc0, 0x6f, 0x01, 0x2f, 0x04, 0x42, 0x00, 0x2e, 0x70, 0x6f, + 0x3c, 0x82, 0x00, 0x40, 0x41, 0x40, 0x89, 0x16, 0x95, 0x08, 0x4a, 0x00, + 0x04, 0xbc, 0x91, 0xb4, 0x01, 0x0e, 0xe0, 0x6f, 0x07, 0x2f, 0xa1, 0x6f, + 0x00, 0x2e, 0x41, 0x40, 0x40, 0xb2, 0x02, 0x2f, 0xa1, 0x6f, 0x05, 0x42, + 0x44, 0x42, 0x00, 0x2e, 0x8b, 0x6f, 0xc0, 0x5e, 0xb8, 0x2e, 0x03, 0x2e, + 0x1c, 0x01, 0x9c, 0xbc, 0x1d, 0xb9, 0x02, 0x82, 0x25, 0x2e, 0x8e, 0x00, + 0x83, 0x56, 0x13, 0x18, 0x01, 0x2e, 0x66, 0x00, 0x43, 0x40, 0xd8, 0x04, + 0x05, 0x2e, 0x65, 0x00, 0x40, 0x50, 0x27, 0x2e, 0x65, 0x00, 0xfb, 0x7f, + 0xda, 0x05, 0x8b, 0x50, 0x4b, 0x40, 0x02, 0x40, 0x81, 0x82, 0x01, 0x42, + 0x03, 0x80, 0x81, 0x52, 0xb1, 0x00, 0x03, 0x40, 0x3b, 0x82, 0x85, 0x58, + 0x14, 0x01, 0xc0, 0xb2, 0x37, 0x2e, 0x66, 0x00, 0xd1, 0x7f, 0xe2, 0x7f, + 0x04, 0x2f, 0x05, 0x2e, 0x6b, 0x00, 0x81, 0x84, 0x25, 0x2e, 0x6b, 0x00, + 0x62, 0x40, 0x3a, 0x0f, 0x45, 0x40, 0xc1, 0x7f, 0x21, 0x30, 0x12, 0x30, + 0x42, 0x2f, 0x0d, 0x2e, 0x69, 0x00, 0x3e, 0x0e, 0x33, 0x2f, 0x05, 0x2e, + 0x6a, 0x00, 0x01, 0x35, 0x91, 0x0e, 0x01, 0x30, 0x03, 0x2f, 0x09, 0x2e, + 0x6e, 0x00, 0x00, 0xb3, 0x24, 0x2f, 0xc0, 0x35, 0x90, 0x0e, 0x39, 0x2f, + 0x8f, 0x50, 0x02, 0x30, 0x01, 0x40, 0x7f, 0x82, 0x43, 0xa2, 0x02, 0x2f, + 0x00, 0x2e, 0x0c, 0x2c, 0x01, 0x30, 0xc0, 0xb2, 0x11, 0x30, 0x02, 0x2f, + 0x25, 0x2e, 0x6d, 0x00, 0x03, 0x2d, 0x23, 0x2e, 0x6d, 0x00, 0x21, 0x30, + 0x25, 0x2e, 0x6b, 0x00, 0x42, 0xb2, 0x04, 0x2f, 0x41, 0xb2, 0x02, 0x2f, + 0x25, 0x2e, 0x6d, 0x00, 0x31, 0x30, 0x3e, 0x80, 0x04, 0x86, 0x25, 0x2e, + 0x6c, 0x00, 0x02, 0x42, 0xc2, 0x42, 0x18, 0x2d, 0x02, 0x35, 0x01, 0x42, + 0x25, 0x2e, 0x6a, 0x00, 0x13, 0x2d, 0x2c, 0x04, 0x38, 0x1e, 0x21, 0x2e, + 0x69, 0x00, 0x7f, 0x50, 0x11, 0x30, 0x22, 0x30, 0x98, 0x2e, 0x66, 0xb5, + 0x09, 0x2c, 0x01, 0x30, 0x2c, 0x00, 0x38, 0x1c, 0x21, 0x2e, 0x68, 0x00, + 0x7f, 0x50, 0x98, 0x2e, 0x66, 0xb5, 0x01, 0x30, 0xc0, 0x6f, 0xd4, 0xb1, + 0xf5, 0xbd, 0x6b, 0xba, 0x91, 0x5a, 0x02, 0x40, 0x15, 0x18, 0xf5, 0xbe, + 0xeb, 0xbb, 0xe3, 0x0a, 0x3d, 0x0b, 0xd2, 0x6f, 0xe3, 0x00, 0x84, 0x40, + 0x63, 0x05, 0x93, 0x58, 0x2c, 0x18, 0xf5, 0xbe, 0x03, 0x42, 0xeb, 0xbb, + 0xfd, 0x0b, 0xe0, 0x6f, 0x58, 0x01, 0xdf, 0x01, 0x7d, 0x1f, 0x95, 0x42, + 0x18, 0x04, 0x85, 0x40, 0x5d, 0x05, 0x2c, 0x18, 0x75, 0xbe, 0xeb, 0xba, + 0x2c, 0x0b, 0xdc, 0x04, 0x18, 0x1c, 0x80, 0x42, 0x84, 0x80, 0x02, 0x30, + 0x00, 0x40, 0x00, 0xb2, 0x0c, 0x2f, 0x01, 0x2e, 0x6b, 0x00, 0x03, 0x35, + 0x83, 0x0e, 0x07, 0x2f, 0x8d, 0x50, 0x3e, 0x80, 0x25, 0x2e, 0x6d, 0x00, + 0x02, 0x42, 0x03, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x40, 0xb2, 0x04, 0x2f, + 0x8b, 0x50, 0x04, 0x80, 0x25, 0x2e, 0x6a, 0x00, 0x02, 0x42, 0x42, 0xb2, + 0x89, 0x56, 0x9a, 0x22, 0x41, 0xb2, 0x01, 0x2e, 0x1c, 0x01, 0x87, 0x52, + 0x0b, 0xbc, 0x8a, 0x22, 0x0f, 0xb8, 0x00, 0x90, 0x01, 0x32, 0x06, 0x2f, + 0x10, 0x30, 0x90, 0x08, 0x80, 0xb2, 0x08, 0x2f, 0x23, 0x2e, 0x5e, 0xf0, + 0x06, 0x2d, 0x20, 0x30, 0x90, 0x08, 0x80, 0xb2, 0x01, 0x2f, 0x23, 0x2e, + 0x5e, 0xf0, 0xfb, 0x6f, 0xc0, 0x5f, 0xb8, 0x2e, 0x07, 0x86, 0xfc, 0x88, + 0xc6, 0x40, 0x05, 0x41, 0x31, 0x1a, 0x12, 0x2f, 0x80, 0x91, 0x22, 0x2f, + 0x01, 0x35, 0x29, 0x0f, 0x0a, 0x2f, 0x06, 0x80, 0x00, 0x2e, 0x00, 0x40, + 0x00, 0xb2, 0x01, 0x2f, 0x44, 0xa9, 0x03, 0x2f, 0x00, 0x30, 0xc0, 0x42, + 0x00, 0x43, 0xb8, 0x2e, 0xc2, 0x42, 0x01, 0x43, 0xb8, 0x2e, 0x01, 0x35, + 0xa9, 0x0e, 0x0e, 0x2f, 0x03, 0x3b, 0xeb, 0x00, 0xcc, 0xa8, 0x0a, 0x2f, + 0x05, 0x86, 0xc2, 0x80, 0xc3, 0x40, 0x02, 0x42, 0x3c, 0x84, 0xc1, 0x80, + 0x81, 0x42, 0x82, 0x84, 0xc0, 0x2e, 0x80, 0x42, 0x00, 0x2e, 0xb8, 0x2e, + 0x03, 0x2e, 0x1d, 0x01, 0x9f, 0xbc, 0x9f, 0xb8, 0x90, 0x50, 0x40, 0xb2, + 0x90, 0x2e, 0x71, 0xb6, 0x12, 0x40, 0x03, 0x30, 0x11, 0x40, 0x80, 0xa8, + 0x5a, 0x05, 0x9f, 0x58, 0x55, 0x23, 0x00, 0x40, 0x75, 0x7f, 0x40, 0xa8, + 0x16, 0x41, 0xd9, 0x05, 0xcf, 0x23, 0x56, 0x05, 0x40, 0xa9, 0x9d, 0x05, + 0x87, 0x7f, 0x6e, 0x23, 0x17, 0x41, 0xa5, 0x7f, 0x3e, 0x8b, 0x04, 0x41, + 0x52, 0x43, 0x00, 0xa8, 0x98, 0x05, 0xf2, 0x7f, 0x86, 0x22, 0xcf, 0x05, + 0xc0, 0xa9, 0x9f, 0x05, 0xbe, 0x23, 0x04, 0x05, 0x92, 0x7f, 0x00, 0xa9, + 0xdc, 0x05, 0x51, 0x43, 0xb6, 0x7f, 0x27, 0x23, 0xa7, 0x54, 0xe1, 0x7f, + 0x02, 0x18, 0x7d, 0x83, 0x40, 0x43, 0xeb, 0xba, 0x75, 0xbd, 0xaa, 0x0a, + 0x0b, 0x2e, 0x71, 0x00, 0x77, 0x5c, 0x2e, 0x18, 0xf5, 0xbe, 0x6b, 0xbb, + 0x75, 0x0b, 0xaa, 0x00, 0xc4, 0x7f, 0x25, 0x2e, 0x71, 0x00, 0xb2, 0x6f, + 0xa5, 0x6f, 0xaa, 0x00, 0x54, 0x01, 0x84, 0x6f, 0x72, 0x6f, 0x94, 0x05, + 0x80, 0xa9, 0xde, 0x05, 0xb7, 0x23, 0x99, 0x5e, 0x77, 0x0e, 0x41, 0x40, + 0x97, 0x5c, 0xb1, 0x01, 0xd5, 0x7f, 0x00, 0x2e, 0x85, 0x41, 0x0e, 0x2f, + 0x00, 0xa0, 0x0c, 0x2f, 0x14, 0x0f, 0x04, 0x2f, 0xe0, 0x6f, 0x00, 0xac, + 0x10, 0x30, 0x08, 0x2c, 0x18, 0x22, 0xf0, 0x6f, 0x00, 0xac, 0x30, 0x30, + 0x24, 0x30, 0x02, 0x2c, 0x20, 0x22, 0x40, 0x30, 0x0d, 0x2e, 0x71, 0x00, + 0x80, 0xa1, 0x1e, 0x23, 0x79, 0x5e, 0x37, 0x0f, 0xbc, 0x23, 0x00, 0x90, + 0x14, 0x30, 0x10, 0x30, 0x18, 0x2f, 0x9d, 0x50, 0x30, 0x00, 0x9b, 0x56, + 0x43, 0x0e, 0x02, 0x2f, 0x10, 0x30, 0x0a, 0x2c, 0x03, 0x30, 0x99, 0x50, + 0x10, 0x0e, 0x13, 0x30, 0x00, 0x2f, 0x03, 0x30, 0x90, 0x0f, 0x10, 0x30, + 0x00, 0x2f, 0x00, 0x30, 0x00, 0x90, 0x10, 0x30, 0x00, 0x2f, 0x00, 0x30, + 0xc0, 0x90, 0x13, 0x30, 0x00, 0x2f, 0x03, 0x30, 0x40, 0xb2, 0x87, 0x5c, + 0x22, 0x2f, 0x41, 0x90, 0x4a, 0x2f, 0xa5, 0x50, 0x00, 0x2e, 0x01, 0x40, + 0x41, 0x82, 0x01, 0x42, 0x02, 0x80, 0x4a, 0xa8, 0x01, 0x40, 0x06, 0x2f, + 0xd0, 0x6f, 0x85, 0x0e, 0x3e, 0x2f, 0x41, 0x80, 0x21, 0x2e, 0x78, 0x00, + 0x3b, 0x2d, 0x95, 0x50, 0xfb, 0x7f, 0x4a, 0xa8, 0x06, 0x2f, 0x98, 0x2e, + 0x73, 0xb6, 0xc0, 0x90, 0xfb, 0x6f, 0x32, 0x2f, 0x00, 0x2e, 0x30, 0x2d, + 0x98, 0x2e, 0x73, 0xb6, 0x29, 0x2e, 0x7a, 0x00, 0x2b, 0x2c, 0xfb, 0x6f, + 0xa1, 0x52, 0xd2, 0x6f, 0x95, 0x0e, 0x41, 0x40, 0x05, 0x2f, 0x00, 0x90, + 0x17, 0x2f, 0x05, 0x2e, 0x7a, 0x00, 0x80, 0x90, 0x13, 0x2f, 0x7f, 0x82, + 0x40, 0xac, 0x23, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x18, 0x2f, 0xa1, 0x54, + 0x82, 0x84, 0x23, 0x2e, 0x77, 0x00, 0x82, 0x40, 0x80, 0xb2, 0x11, 0x2f, + 0x00, 0x90, 0x23, 0x2e, 0x79, 0x00, 0x0d, 0x2f, 0x29, 0x2e, 0x72, 0x00, + 0x0b, 0x2d, 0x41, 0x80, 0x21, 0x2e, 0x77, 0x00, 0x0f, 0xa4, 0x05, 0x2f, + 0xa3, 0x50, 0x3e, 0x80, 0xf1, 0x30, 0x29, 0x2e, 0x79, 0x00, 0x01, 0x42, + 0x06, 0x30, 0x34, 0x08, 0x00, 0xb2, 0x02, 0x2f, 0x80, 0x30, 0x21, 0x2e, + 0x5e, 0xf0, 0x70, 0x5f, 0xb8, 0x2e, 0x04, 0x84, 0x01, 0x30, 0x81, 0x42, + 0x82, 0x84, 0x01, 0x42, 0xa1, 0x42, 0x81, 0x42, 0x82, 0x84, 0x00, 0x2e, + 0x91, 0x42, 0x81, 0x42, 0xb8, 0x2e, 0x30, 0x50, 0xf3, 0x7f, 0xc0, 0xac, + 0xe4, 0x7f, 0xd5, 0x7f, 0x03, 0x2f, 0x00, 0x30, 0x82, 0x04, 0xf3, 0x6f, + 0xc3, 0x06, 0x40, 0xad, 0x05, 0x2f, 0xe0, 0x6f, 0x05, 0x30, 0x28, 0x04, + 0xd1, 0x6f, 0x69, 0x07, 0xe0, 0x7f, 0x40, 0xa1, 0x01, 0x30, 0x20, 0x2f, + 0x13, 0x25, 0x02, 0x25, 0x04, 0x32, 0x06, 0x30, 0x02, 0x30, 0x03, 0x30, + 0xaf, 0xbb, 0xb1, 0xbd, 0xdf, 0x0a, 0x9f, 0xbb, 0x21, 0xbd, 0x97, 0x0a, + 0x8f, 0xbb, 0x91, 0xbc, 0x01, 0xbc, 0x4f, 0x0a, 0x6b, 0x0e, 0x04, 0x2f, + 0x6b, 0x1a, 0x07, 0x2f, 0xe7, 0x6f, 0x7a, 0x0f, 0x04, 0x2f, 0xe7, 0x6f, + 0x97, 0x04, 0x17, 0x30, 0x07, 0x0a, 0xdd, 0x06, 0x81, 0x8d, 0x34, 0x0e, + 0xe6, 0x2f, 0x00, 0x2e, 0x0d, 0x2d, 0x6b, 0x0e, 0x00, 0x30, 0x05, 0x2f, + 0x6b, 0x1a, 0x07, 0x2f, 0xe0, 0x6f, 0x42, 0x0f, 0x00, 0x30, 0x03, 0x2f, + 0xe0, 0x6f, 0x90, 0x04, 0xdd, 0x06, 0x10, 0x30, 0xf5, 0x6f, 0xc3, 0x7f, + 0xb2, 0x7f, 0x40, 0xad, 0x06, 0x2f, 0x03, 0x30, 0xb2, 0x6f, 0x9a, 0x04, + 0xc4, 0x6f, 0xdc, 0x06, 0xb2, 0x7f, 0xc3, 0x7f, 0x00, 0x2e, 0xd2, 0x6f, + 0xaa, 0x0c, 0x80, 0xac, 0x02, 0x30, 0x01, 0x2f, 0x10, 0x04, 0x51, 0x06, + 0xd0, 0x5f, 0xb8, 0x2e, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, + 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00, 0x80, 0x2e, 0x18, 0x00 +}; + +/***************************************************************************/ +/*! Static Function Declarations +****************************************************************************/ +/*! + * @brief This API update the length for read and write. + * + * @param[in] len : Length for read and write + * @param[in] feature : Variable to specify the features + * which are to be set in the sensor. + * @param[in] enable : Variable which specifies whether to enable or + * disable the features in the bma455 sensor. + * enable | Macros + * ----------------|------------------- + * 0x01 | BMA4_EN + * 0x00 | BMA4_DIS + * @param[in] dev : Structure instance of bma4_dev. + * + * @return none + */ +static void update_len(uint8_t *len, uint8_t feature, uint8_t enable); + +/*! + * @brief This API enables the features of sensor. + * + * @param[in] feature : Variable to specify the features + * which are to be set in the sensor. + * @param[in] len : length to read and write + * @param[in] feature_config : Array address which stores the feature + * configuration data + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any positive value mentioned in ERROR CODES -> Fail + */ +static uint16_t feature_enable(uint8_t feature, uint8_t len, uint8_t *feature_config, struct bma4_dev *dev); + +/*! + * @brief This API disables the features of sensor. + * + * @param[in] feature : Variable to specify the features + * which are to be unset in the sensor. + * @param[in] len : length to read and write + * @param[in] feature_config : Array address which stores the feature + * configuration data + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any positive value mentioned in ERROR CODES -> Fail + */ +static uint16_t feature_disable(uint8_t feature, uint8_t len, uint8_t *feature_config, struct bma4_dev *dev); + +/*! + * @brief This API update the settings of step counter into write array. + * + * @param[in] setting : Pointer to structure variable which stores the + * settings parameter1 to parameter25. + * @param[in] index : value for array traversing. + * @param[out] feature_config : Pointer to store the settings + * + * @return none + */ +static void update_stepcounter_parameter(const struct bma423_stepcounter_settings *setting, + uint8_t index, uint8_t *feature_config); +/*! + * @brief This API copy the settings of step counter into the + * structure of bma423_stepcounter_settings, which is read from sensor. + * + * @param[out] setting : Pointer to structure variable which stores the + * settings parameter1 to parameter25 read from sensor. + * @param[in] data_p : Pointer of array which stores the parameters. + * + * @return none + */ +static void extract_stepcounter_parameter(struct bma423_stepcounter_settings *setting, const uint16_t *data_p); + +/***************************************************************************/ +/**\name Function definitions +****************************************************************************/ + +/*! + * @brief This API is the entry point. + * Call this API before using all other APIs. + * This API reads the chip-id of the sensor and sets the resolution. + */ +#include +uint16_t bma423_init(struct bma4_dev *dev) +{ + uint16_t rslt; + + rslt = bma4_init(dev); + + if (rslt == BMA4_OK) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Resolution of BMA423 sensor is 12 bit */ + dev->resolution = 12; + dev->feature_len = BMA423_FEATURE_SIZE; + dev->variant = BMA42X_VARIANT; + } else { + rslt |= BMA4_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This API is used to upload the config file to enable + * the features of the sensor. + */ +uint16_t bma423_write_config_file(struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Config. stream read/write length boundary check */ + if ((dev->read_write_len >= BMA423_RD_WR_MIN_LEN) && + (dev->read_write_len <= BMA423_FEATURE_SIZE)) { + /* Even or odd check */ + if ((dev->read_write_len % 2) != 0) + dev->read_write_len = dev->read_write_len - 1; + /*Assign stream data */ + dev->config_file_ptr = bma423_config_file; + rslt = bma4_write_config_file(dev); + } else { + rslt = BMA4_E_RD_WR_LENGTH_INVALID; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + return rslt; +} + +/*! + * @brief This API is used to get the configuration id of the sensor. + */ +uint16_t bma423_get_config_id(uint16_t *config_id, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_CONFIG_ID_OFFSET; + uint16_t rslt = BMA4_OK; + uint16_t config_id_lsb = 0; + uint16_t config_id_msb = 0; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + config_id_lsb = (uint16_t)feature_config[index]; + config_id_msb = ((uint16_t)feature_config[index + 1]) << 8; + *config_id = config_id_lsb | config_id_msb; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets/unsets the user provided interrupt to either + * interrupt pin1 or pin2 in the sensor. + */ +uint16_t bma423_map_interrupt(uint8_t int_line, uint16_t int_map, uint8_t enable, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + if (int_line <= 1) { + /* Map/Unmap the interrupt */ + rslt = bma4_map_interrupt(int_line, int_map, enable, dev); + } else { + rslt = BMA4_E_INT_LINE_INVALID; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the bma423 interrupt status from the sensor. + */ +uint16_t bma423_read_int_status(uint16_t *int_status, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Read the interrupt status */ + rslt = bma4_read_int_status(int_status, dev); + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables the features of the sensor. + */ +uint16_t bma423_feature_enable(uint8_t feature, uint8_t enable, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint16_t rslt = BMA4_OK; + uint8_t len; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Update the length for read and write */ + update_len(&len, feature, enable); + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, len, dev); + if (rslt == BMA4_OK) { + if (enable == TRUE) { + /* Enables the feature */ + rslt |= feature_enable(feature, len, feature_config, dev); + } else { + /* Disables the feature */ + rslt |= feature_disable(feature, len, feature_config, dev); + } + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs x, y and z axis remapping in the sensor. + */ +uint16_t bma423_set_remap_axes(const struct bma423_axes_remap *remap_data, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_AXES_REMAP_OFFSET; + uint16_t rslt = BMA4_OK; + uint8_t x_axis = 0; + uint8_t x_axis_sign = 0; + uint8_t y_axis = 0; + uint8_t y_axis_sign = 0; + uint8_t z_axis = 0; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + x_axis = remap_data->x_axis & BMA423_X_AXIS_MASK; + x_axis_sign = (remap_data->x_axis_sign << 2) & BMA423_X_AXIS_SIGN_MASK; + y_axis = (remap_data->y_axis << 3) & BMA423_Y_AXIS_MASK; + y_axis_sign = (remap_data->y_axis_sign << 5) & BMA423_Y_AXIS_SIGN_MASK; + z_axis = (remap_data->z_axis << 6) & BMA423_Z_AXIS_MASK; + feature_config[index] = x_axis | x_axis_sign | y_axis | y_axis_sign | z_axis; + feature_config[index + 1] = remap_data->z_axis_sign & BMA423_Z_AXIS_SIGN_MASK; + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the x, y and z axis remap data from the sensor. + */ +uint16_t bma423_get_remap_axes(struct bma423_axes_remap *remap_data, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_AXES_REMAP_OFFSET; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + remap_data->x_axis = feature_config[index] & BMA423_X_AXIS_MASK; + remap_data->x_axis_sign = (feature_config[index] & BMA423_X_AXIS_SIGN_MASK) >> 2; + remap_data->y_axis = (feature_config[index] & BMA423_Y_AXIS_MASK) >> 3; + remap_data->y_axis_sign = (feature_config[index] & BMA423_Y_AXIS_SIGN_MASK) >> 5; + remap_data->z_axis = (feature_config[index] & BMA423_Z_AXIS_MASK) >> 6; + remap_data->z_axis_sign = (feature_config[index + 1] & BMA423_Z_AXIS_SIGN_MASK); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables the any motion feature according to the axis + * set by the user in the sensor. + */ +uint16_t bma423_anymotion_enable_axis(uint8_t axis, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_ANYMOTION_EN_LEN + 2] = {0}; + /* Anymotion axis enable bit pos. is 3 byte ahead of the + anymotion base address(0x00) */ + uint8_t index = 3; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_ANYMOTION_EN_LEN + 2, dev); + if (rslt == BMA4_OK) { + feature_config[index] = BMA4_SET_BITSLICE(feature_config[index], + BMA423_ANY_NO_MOTION_AXIS_EN, axis); + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_ANYMOTION_EN_LEN + 2, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! @brief This API sets the configuration of Any motion feature in + * the sensor. + */ +uint16_t bma423_set_any_motion_config(const struct bma423_anymotion_config *any_motion, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_ANYMOTION_EN_LEN + 2] = {0}; + uint8_t index = BMA423_ANY_NO_MOTION_OFFSET; + uint16_t duration_lsb = 0; + uint16_t duration_msb = 0; + uint16_t duration = 0; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_ANYMOTION_EN_LEN + 2, dev); + if (rslt == BMA4_OK) { + /* Assign threshold value in feature config array */ + feature_config[index++] = BMA4_GET_LSB(any_motion->threshold); + feature_config[index] = BMA4_GET_MSB(any_motion->threshold); + /* Assign no motion selection value in feature config array*/ + feature_config[index++] |= (uint8_t) + (any_motion->nomotion_sel << BMA423_ANY_NO_MOTION_SEL_POS); + + /* Extract duration */ + duration_lsb = feature_config[index]; + duration_msb = feature_config[index + 1] << 8; + duration = duration_lsb | duration_msb; + duration = BMA4_SET_BITS_POS_0(duration, + BMA423_ANY_NO_MOTION_DUR, any_motion->duration); + /* Assign duration value in feature config array*/ + feature_config[index++] = BMA4_GET_LSB(duration); + feature_config[index] = BMA4_GET_MSB(duration); + + /* Write any motion settings to the sensor*/ + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_ANYMOTION_EN_LEN + 2, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! @brief This API gets the configuration of any motion feature from + * the sensor. + */ +uint16_t bma423_get_any_motion_config(struct bma423_anymotion_config *any_motion, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_ANYMOTION_EN_LEN + 2] = {0}; + uint8_t anymotion = 0; + uint16_t rslt = BMA4_OK; + uint16_t *data_p = (uint16_t *)feature_config; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_ANYMOTION_EN_LEN + 2, dev); + if (rslt == BMA4_OK) { + /* Extract threshold value */ + any_motion->threshold = (*data_p) & BMA423_ANY_NO_MOTION_THRES_MSK; + /* Extract threshold & nomotion selection + * data */ + anymotion = ((uint8_t)(*(data_p++) >> 8)) & BMA423_ANY_NO_MOTION_SEL_MSK; + /* Extract no motion field */ + any_motion->nomotion_sel = anymotion >> + BMA423_ANY_NO_MOTION_SEL_POS; + /* Extract duration value */ + any_motion->duration = (*(data_p)) & BMA423_ANY_NO_MOTION_DUR_MSK; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables or disables the step detector feature in the + * sensor. + */ +uint16_t bma423_step_detector_enable(uint8_t enable, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint16_t rslt = BMA4_OK; + /* Step detector enable bit pos. is 1 byte ahead of the base address */ + uint8_t index = BMA423_STEP_CNTR_OFFSET + 1; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + feature_config[index] = BMA4_SET_BITSLICE(feature_config[index], + BMA423_STEP_DETECTOR_EN, enable); + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the watermark level for step counter + * interrupt in the sensor. + */ +uint16_t bma423_step_counter_set_watermark(uint16_t step_counter_wm, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_STEP_CNTR_OFFSET; + uint16_t wm_lsb = 0; + uint16_t wm_msb = 0; + uint16_t rslt = BMA4_OK; + uint16_t data = 0; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + wm_lsb = feature_config[index]; + wm_msb = feature_config[index + 1] << 8; + data = wm_lsb | wm_msb; + /* Sets only watermark bits in the complete + 16 bits of data */ + data = BMA4_SET_BITS_POS_0(data, BMA423_STEP_CNTR_WM, step_counter_wm); + /* Splits 16 bits of data to individual + 8 bits data */ + feature_config[index] = BMA4_GET_LSB(data); + feature_config[index + 1] = BMA4_GET_MSB(data); + /* Writes stepcounter watermark settings + in the sensor */ + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the water mark level set for step counter interrupt + * in the sensor + */ +uint16_t bma423_step_counter_get_watermark(uint16_t *step_counter_wm, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_STEP_CNTR_OFFSET; + uint16_t wm_lsb = 0; + uint16_t wm_msb = 0; + uint16_t rslt = BMA4_OK; + uint16_t data = 0; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + wm_lsb = feature_config[index]; + wm_msb = feature_config[index + 1] << 8; + data = wm_lsb | wm_msb; + *step_counter_wm = BMA4_GET_BITS_POS_0(data, BMA423_STEP_CNTR_WM); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API resets the counted steps of step counter. + */ +uint16_t bma423_reset_step_counter(struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + /* Reset bit is 1 byte ahead of base address */ + uint8_t index = BMA423_STEP_CNTR_OFFSET + 1; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + + if (rslt == BMA4_OK) { + feature_config[index] = BMA4_SET_BITSLICE(feature_config[index], + BMA423_STEP_CNTR_RST, 1); + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + return rslt; +} + +/*! + * @brief This API gets the number of counted steps of the step counter + * feature from the sensor. + */ +uint16_t bma423_step_counter_output(uint32_t *step_count, struct bma4_dev *dev) +{ + uint8_t data[BMA423_STEP_CNTR_DATA_SIZE] = {0}; + uint16_t rslt = BMA4_OK; + uint32_t step_count_0 = 0; + uint32_t step_count_1 = 0; + uint32_t step_count_2 = 0; + uint32_t step_count_3 = 0; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Reads the step counter output data from the + gpio register */ + rslt = bma4_read_regs(BMA4_STEP_CNT_OUT_0_ADDR, data, + BMA423_STEP_CNTR_DATA_SIZE, dev); + + if (rslt == BMA4_OK) { + step_count_0 = (uint32_t)data[0]; + step_count_1 = (uint32_t)data[1] << 8; + step_count_2 = (uint32_t)data[2] << 16; + step_count_3 = (uint32_t)data[3] << 24; + *step_count = step_count_0 | step_count_1 | step_count_2 | step_count_3; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the output for activity feature. + */ +uint16_t bma423_activity_output(uint8_t *activity, struct bma4_dev *dev) +{ + uint8_t data = 0; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + /* Reads the activity output from the gpio register */ + rslt = bma4_read_regs(BMA4_ACTIVITY_OUT_ADDR, &data, 1, dev); + if (rslt == BMA4_OK) + *activity = data; + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API select the platform configuration wrist(Default) or phone. + */ +uint16_t bma423_select_platform(uint8_t platform, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + struct bma423_stepcounter_settings sc_settings = {0}; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + switch (platform) { + case BMA423_PHONE_CONFIG: + sc_settings.param1 = BMA423_PHONE_SC_PARAM_1; + sc_settings.param2 = BMA423_PHONE_SC_PARAM_2; + sc_settings.param3 = BMA423_PHONE_SC_PARAM_3; + sc_settings.param4 = BMA423_PHONE_SC_PARAM_4; + sc_settings.param5 = BMA423_PHONE_SC_PARAM_5; + sc_settings.param6 = BMA423_PHONE_SC_PARAM_6; + sc_settings.param7 = BMA423_PHONE_SC_PARAM_7; + sc_settings.param8 = BMA423_PHONE_SC_PARAM_8; + sc_settings.param9 = BMA423_PHONE_SC_PARAM_9; + sc_settings.param10 = BMA423_PHONE_SC_PARAM_10; + sc_settings.param11 = BMA423_PHONE_SC_PARAM_11; + sc_settings.param12 = BMA423_PHONE_SC_PARAM_12; + sc_settings.param13 = BMA423_PHONE_SC_PARAM_13; + sc_settings.param14 = BMA423_PHONE_SC_PARAM_14; + sc_settings.param15 = BMA423_PHONE_SC_PARAM_15; + sc_settings.param16 = BMA423_PHONE_SC_PARAM_16; + sc_settings.param17 = BMA423_PHONE_SC_PARAM_17; + sc_settings.param18 = BMA423_PHONE_SC_PARAM_18; + sc_settings.param19 = BMA423_PHONE_SC_PARAM_19; + sc_settings.param20 = BMA423_PHONE_SC_PARAM_20; + sc_settings.param21 = BMA423_PHONE_SC_PARAM_21; + sc_settings.param22 = BMA423_PHONE_SC_PARAM_22; + sc_settings.param23 = BMA423_PHONE_SC_PARAM_23; + sc_settings.param24 = BMA423_PHONE_SC_PARAM_24; + sc_settings.param25 = BMA423_PHONE_SC_PARAM_25; + break; + + case BMA423_WRIST_CONFIG: + sc_settings.param1 = BMA423_WRIST_SC_PARAM_1; + sc_settings.param2 = BMA423_WRIST_SC_PARAM_2; + sc_settings.param3 = BMA423_WRIST_SC_PARAM_3; + sc_settings.param4 = BMA423_WRIST_SC_PARAM_4; + sc_settings.param5 = BMA423_WRIST_SC_PARAM_5; + sc_settings.param6 = BMA423_WRIST_SC_PARAM_6; + sc_settings.param7 = BMA423_WRIST_SC_PARAM_7; + sc_settings.param8 = BMA423_WRIST_SC_PARAM_8; + sc_settings.param9 = BMA423_WRIST_SC_PARAM_9; + sc_settings.param10 = BMA423_WRIST_SC_PARAM_10; + sc_settings.param11 = BMA423_WRIST_SC_PARAM_11; + sc_settings.param12 = BMA423_WRIST_SC_PARAM_12; + sc_settings.param13 = BMA423_WRIST_SC_PARAM_13; + sc_settings.param14 = BMA423_WRIST_SC_PARAM_14; + sc_settings.param15 = BMA423_WRIST_SC_PARAM_15; + sc_settings.param16 = BMA423_WRIST_SC_PARAM_16; + sc_settings.param17 = BMA423_WRIST_SC_PARAM_17; + sc_settings.param18 = BMA423_WRIST_SC_PARAM_18; + sc_settings.param19 = BMA423_WRIST_SC_PARAM_19; + sc_settings.param20 = BMA423_WRIST_SC_PARAM_20; + sc_settings.param21 = BMA423_WRIST_SC_PARAM_21; + sc_settings.param22 = BMA423_WRIST_SC_PARAM_22; + sc_settings.param23 = BMA423_WRIST_SC_PARAM_23; + sc_settings.param24 = BMA423_WRIST_SC_PARAM_24; + sc_settings.param25 = BMA423_WRIST_SC_PARAM_25; + break; + + default: + rslt = BMA4_E_OUT_OF_RANGE; + break; + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + if (rslt == BMA4_OK) { + /* Set the step counter parameter */ + rslt = bma423_stepcounter_set_parameter(&sc_settings, dev); + } + + return rslt; +} + +/*! + * @brief This API gets the parameter1 to parameter7 settings of the + * step counter feature. + */ +uint16_t bma423_stepcounter_get_parameter(struct bma423_stepcounter_settings *setting, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint16_t *data_p = (uint16_t *)feature_config; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + + if (rslt == BMA4_OK) { + /* To convert 8bit to 16 bit address */ + data_p = data_p + BMA423_STEP_CNTR_PARAM_OFFSET/2; + extract_stepcounter_parameter(setting, data_p); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the parameter1 to parameter7 settings of the + * step counter feature in the sensor. + */ +uint16_t bma423_stepcounter_set_parameter(const struct bma423_stepcounter_settings *setting, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_STEP_CNTR_PARAM_OFFSET; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + update_stepcounter_parameter(setting, index, feature_config); + /* Writes stepcounter parameter settings + in the sensor */ + rslt = bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensitivity of wake up feature in the sensor + */ +uint16_t bma423_wakeup_set_sensitivity(uint8_t sensitivity, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_WAKEUP_OFFSET; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + feature_config[index] = BMA4_SET_BITSLICE(feature_config[index], + BMA423_WAKEUP_SENS, sensitivity); + /* Writes sensitivity settings in the sensor */ + rslt |= bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensitivity of wake up feature in the sensor + */ +uint16_t bma423_wakeup_get_sensitivity(uint8_t *sensitivity, struct bma4_dev *dev) +{ + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0}; + uint8_t index = BMA423_WAKEUP_OFFSET; + uint16_t rslt = BMA4_OK; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + /* Extracts sensitivity data */ + *sensitivity = BMA4_GET_BITSLICE(feature_config[index], BMA423_WAKEUP_SENS); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API is used to select single/double tap + * feature in the sensor + */ +uint16_t bma423_tap_selection(const uint8_t tap_select, struct bma4_dev *dev) +{ + uint16_t rslt = BMA4_OK; + uint8_t feature_config[BMA423_FEATURE_SIZE] = {0,}; + uint8_t index = BMA423_WAKEUP_OFFSET; + + if (dev != NULL) { + if (dev->chip_id == BMA423_CHIP_ID) { + rslt = bma4_read_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, BMA423_FEATURE_SIZE, dev); + if (rslt == BMA4_OK) { + feature_config[index] = BMA4_SET_BITSLICE(feature_config[index], + BMA423_TAP_SEL, tap_select); + rslt = bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, + BMA423_FEATURE_SIZE, dev); + } + } else { + rslt = BMA4_E_INVALID_SENSOR; + } + } else { + rslt = BMA4_E_NULL_PTR; + } + + return rslt; +} +/*! + * @brief This API update the length for read and write. + */ +static void update_len(uint8_t *len, uint8_t feature, uint8_t enable) +{ + uint8_t length = BMA423_FEATURE_SIZE; + + if ((feature == BMA423_ANY_MOTION) || (feature == BMA423_NO_MOTION)) { + /* Change the feature length to 2 for reading and writing of 2 bytes for + any/no-motion enable */ + length = BMA423_ANYMOTION_EN_LEN; + + /* Read and write 4 byte to disable the any/no motion completely along with + all axis */ + if (enable == BMA4_DISABLE) { + /*Change the feature length to 4 for reading and writing + of 4 bytes for any/no-motion enable */ + length = length + 2; + } + } + + *len = length; +} + +/*! + * @brief This API enables the features of the sensor. + */ +static uint16_t feature_enable(uint8_t feature, uint8_t len, uint8_t *feature_config, struct bma4_dev *dev) +{ + uint8_t index = 0; + uint16_t rslt; + + /* Enable step counter */ + if ((feature & BMA423_STEP_CNTR) > 0) { + /* Step counter enable bit pos. is 1 byte ahead of the + base address */ + index = BMA423_STEP_CNTR_OFFSET + 1; + feature_config[index] = feature_config[index] | BMA423_STEP_CNTR_EN_MSK; + } + + /* Enable activity */ + if ((feature & BMA423_ACTIVITY) > 0) { + /* Activity enable bit pos. is 1 byte ahead of the + base address */ + index = BMA423_STEP_CNTR_OFFSET + 1; + feature_config[index] = feature_config[index] | BMA423_ACTIVITY_EN_MSK; + } + /* Enable tilt */ + if ((feature & BMA423_TILT) > 0) { + /* Tilt enable bit pos. is the base address(0x3A) of tilt */ + index = BMA423_TILT_OFFSET; + feature_config[index] = feature_config[index] | BMA423_TILT_EN_MSK; + } + + /* Enable wakeup */ + if ((feature & BMA423_WAKEUP) > 0) { + /* Wakeup enable bit pos. is the base address(0x38) of wakeup */ + index = BMA423_WAKEUP_OFFSET; + feature_config[index] = feature_config[index] | BMA423_WAKEUP_EN_MSK; + } + + /* Enable anymotion/nomotion */ + if ((feature & BMA423_ANY_MOTION) > 0 || (feature & BMA423_NO_MOTION) > 0) { + /* Any/Nomotion enable bit pos. is 1 bytes ahead of the + any/nomotion base address(0x00) */ + index = 1; + + if ((feature & BMA423_ANY_MOTION) > 0) { + /* Enable anymotion */ + feature_config[index] = feature_config[index] & (~BMA423_ANY_NO_MOTION_SEL_MSK); + } else { + /* Enable nomotion */ + feature_config[index] = feature_config[index] | BMA423_ANY_NO_MOTION_SEL_MSK; + } + } + + /* Write the feature enable settings in the sensor */ + rslt = bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, len, dev); + + return rslt; +} + +/*! + * @brief This API disables the features of the sensor. + */ +static uint16_t feature_disable(uint8_t feature, uint8_t len, uint8_t *feature_config, struct bma4_dev *dev) +{ + uint8_t index = 0; + uint16_t rslt; + + /* Disable step counter */ + if ((feature & BMA423_STEP_CNTR) > 0) { + /* Step counter enable bit pos. is 1 byte ahead of the + base address */ + index = BMA423_STEP_CNTR_OFFSET + 1; + feature_config[index] = feature_config[index] & (~BMA423_STEP_CNTR_EN_MSK); + } + + /* Disable activity */ + if ((feature & BMA423_ACTIVITY) > 0) { + /* Activity enable bit pos. is 1 byte ahead of the + base address */ + index = BMA423_STEP_CNTR_OFFSET + 1; + feature_config[index] = feature_config[index] & (~BMA423_ACTIVITY_EN_MSK); + } + /* Disable tilt */ + if ((feature & BMA423_TILT) > 0) { + /* Tilt enable bit pos. is the base address(0x3A) of tilt */ + index = BMA423_TILT_OFFSET; + feature_config[index] = feature_config[index] & (~BMA423_TILT_EN_MSK); + } + + /* Disable wakeup */ + if ((feature & BMA423_WAKEUP) > 0) { + /* Tilt enable bit pos. is the base address(0x38) of wakeup */ + index = BMA423_WAKEUP_OFFSET; + feature_config[index] = feature_config[index] & (~BMA423_WAKEUP_EN_MSK); + } + + /* Disable anymotion/nomotion */ + if ((feature & BMA423_ANY_MOTION) > 0 || (feature & BMA423_NO_MOTION) > 0) { + /* Any/Nomotion enable bit pos. is 1 bytes ahead of the + any/nomotion base address(0x00) */ + index = 1; + + if ((feature & BMA423_ANY_MOTION) > 0) { + /* Disable anymotion */ + feature_config[index] = feature_config[index] | BMA423_ANY_NO_MOTION_SEL_MSK; + } else { + /* Disable nomotion */ + feature_config[index] = feature_config[index] & (~BMA423_ANY_NO_MOTION_SEL_MSK); + } + /* Any/Nomotion axis enable bit pos. is 3 byte ahead of the + any/nomotion base address(0x00) */ + index = 3; + feature_config[index] = feature_config[index] & (~BMA423_ANY_NO_MOTION_AXIS_EN_MSK); + } + /* Write the configured settings in the sensor */ + rslt = bma4_write_regs(BMA4_FEATURE_CONFIG_ADDR, feature_config, len, dev); + + return rslt; +} + +/*! + * @brief This API update the settings of step counter. + */ +static void update_stepcounter_parameter(const struct bma423_stepcounter_settings *setting, + uint8_t index, uint8_t *feature_config) +{ + feature_config[index++] = BMA4_GET_LSB(setting->param1); + feature_config[index++] = BMA4_GET_MSB(setting->param1); + feature_config[index++] = BMA4_GET_LSB(setting->param2); + feature_config[index++] = BMA4_GET_MSB(setting->param2); + feature_config[index++] = BMA4_GET_LSB(setting->param3); + feature_config[index++] = BMA4_GET_MSB(setting->param3); + feature_config[index++] = BMA4_GET_LSB(setting->param4); + feature_config[index++] = BMA4_GET_MSB(setting->param4); + feature_config[index++] = BMA4_GET_LSB(setting->param5); + feature_config[index++] = BMA4_GET_MSB(setting->param5); + feature_config[index++] = BMA4_GET_LSB(setting->param6); + feature_config[index++] = BMA4_GET_MSB(setting->param6); + feature_config[index++] = BMA4_GET_LSB(setting->param7); + feature_config[index++] = BMA4_GET_MSB(setting->param7); + feature_config[index++] = BMA4_GET_LSB(setting->param8); + feature_config[index++] = BMA4_GET_MSB(setting->param8); + feature_config[index++] = BMA4_GET_LSB(setting->param9); + feature_config[index++] = BMA4_GET_MSB(setting->param9); + feature_config[index++] = BMA4_GET_LSB(setting->param10); + feature_config[index++] = BMA4_GET_MSB(setting->param10); + feature_config[index++] = BMA4_GET_LSB(setting->param11); + feature_config[index++] = BMA4_GET_MSB(setting->param11); + feature_config[index++] = BMA4_GET_LSB(setting->param12); + feature_config[index++] = BMA4_GET_MSB(setting->param12); + feature_config[index++] = BMA4_GET_LSB(setting->param13); + feature_config[index++] = BMA4_GET_MSB(setting->param13); + feature_config[index++] = BMA4_GET_LSB(setting->param14); + feature_config[index++] = BMA4_GET_MSB(setting->param14); + feature_config[index++] = BMA4_GET_LSB(setting->param15); + feature_config[index++] = BMA4_GET_MSB(setting->param15); + feature_config[index++] = BMA4_GET_LSB(setting->param16); + feature_config[index++] = BMA4_GET_MSB(setting->param16); + feature_config[index++] = BMA4_GET_LSB(setting->param17); + feature_config[index++] = BMA4_GET_MSB(setting->param17); + feature_config[index++] = BMA4_GET_LSB(setting->param18); + feature_config[index++] = BMA4_GET_MSB(setting->param18); + feature_config[index++] = BMA4_GET_LSB(setting->param19); + feature_config[index++] = BMA4_GET_MSB(setting->param19); + feature_config[index++] = BMA4_GET_LSB(setting->param20); + feature_config[index++] = BMA4_GET_MSB(setting->param20); + feature_config[index++] = BMA4_GET_LSB(setting->param21); + feature_config[index++] = BMA4_GET_MSB(setting->param21); + feature_config[index++] = BMA4_GET_LSB(setting->param22); + feature_config[index++] = BMA4_GET_MSB(setting->param22); + feature_config[index++] = BMA4_GET_LSB(setting->param23); + feature_config[index++] = BMA4_GET_MSB(setting->param23); + feature_config[index++] = BMA4_GET_LSB(setting->param24); + feature_config[index++] = BMA4_GET_MSB(setting->param24); + feature_config[index++] = BMA4_GET_LSB(setting->param25); + feature_config[index] = BMA4_GET_MSB(setting->param25); +} + +/*! + * @brief This API copy the settings of step counter into the + * structure of bma423_stepcounter_settings, which is read from sensor. + */ +static void extract_stepcounter_parameter(struct bma423_stepcounter_settings *setting, const uint16_t *data_p) +{ + setting->param1 = *(data_p++); + setting->param2 = *(data_p++); + setting->param3 = *(data_p++); + setting->param4 = *(data_p++); + setting->param5 = *(data_p++); + setting->param6 = *(data_p++); + setting->param7 = *(data_p++); + setting->param8 = *(data_p++); + setting->param9 = *(data_p++); + setting->param10 = *(data_p++); + setting->param11 = *(data_p++); + setting->param12 = *(data_p++); + setting->param13 = *(data_p++); + setting->param14 = *(data_p++); + setting->param15 = *(data_p++); + setting->param16 = *(data_p++); + setting->param17 = *(data_p++); + setting->param18 = *(data_p++); + setting->param19 = *(data_p++); + setting->param20 = *(data_p++); + setting->param21 = *(data_p++); + setting->param22 = *(data_p++); + setting->param23 = *(data_p++); + setting->param24 = *(data_p++); + setting->param25 = *data_p; +} \ No newline at end of file diff --git a/src/bma423.h b/src/bma423.h new file mode 100644 index 0000000..14a3f85 --- /dev/null +++ b/src/bma423.h @@ -0,0 +1,804 @@ +/* +* +**************************************************************************** +* Copyright (C) 2017 - 2018 Bosch Sensortec GmbH +* +* File : bma423.h +* +* Date: 12 Oct 2017 +* +* Revision : 1.1.4 $ +* +* Usage: Sensor Driver for BMA423 sensor +* +**************************************************************************** +* +* Disclaimer +* +* Common: +* Bosch Sensortec products are developed for the consumer goods industry. +* They may only be used within the parameters of the respective valid +* product data sheet. Bosch Sensortec products are provided with the +* express understanding that there is no warranty of fitness for a +* particular purpose.They are not fit for use in life-sustaining, +* safety or security sensitive systems or any system or device +* that may lead to bodily harm or property damage if the system +* or device malfunctions. In addition,Bosch Sensortec products are +* not fit for use in products which interact with motor vehicle systems. +* The resale and or use of products are at the purchasers own risk and +* his own responsibility. The examination of fitness for the intended use +* is the sole responsibility of the Purchaser. +* +* The purchaser shall indemnify Bosch Sensortec from all third party +* claims, including any claims for incidental, or consequential damages, +* arising from any product use not covered by the parameters of +* the respective valid product data sheet or not approved by +* Bosch Sensortec and reimburse Bosch Sensortec for all costs in +* connection with such claims. +* +* The purchaser must monitor the market for the purchased products, +* particularly with regard to product safety and inform Bosch Sensortec +* without delay of all security relevant incidents. +* +* Engineering Samples are marked with an asterisk (*) or (e). +* Samples may vary from the valid technical specifications of the product +* series. They are therefore not intended or fit for resale to third +* parties or for use in end products. Their sole purpose is internal +* client testing. The testing of an engineering sample may in no way +* replace the testing of a product series. Bosch Sensortec assumes +* no liability for the use of engineering samples. +* By accepting the engineering samples, the Purchaser agrees to indemnify +* Bosch Sensortec from all claims arising from the use of engineering +* samples. +* +* Special: +* This software module (hereinafter called "Software") and any information +* on application-sheets (hereinafter called "Information") is provided +* free of charge for the sole purpose to support your application work. +* The Software and Information is subject to the following +* terms and conditions: +* +* The Software is specifically designed for the exclusive use for +* Bosch Sensortec products by personnel who have special experience +* and training. Do not use this Software if you do not have the +* proper experience or training. +* +* This Software package is provided `` as is `` and without any expressed +* or implied warranties,including without limitation, the implied warranties +* of merchantability and fitness for a particular purpose. +* +* Bosch Sensortec and their representatives and agents deny any liability +* for the functional impairment +* of this Software in terms of fitness, performance and safety. +* Bosch Sensortec and their representatives and agents shall not be liable +* for any direct or indirect damages or injury, except as +* otherwise stipulated in mandatory applicable law. +* +* The Information provided is believed to be accurate and reliable. +* Bosch Sensortec assumes no responsibility for the consequences of use +* of such Information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of Bosch. Specifications mentioned in the Information are +* subject to change without notice. +**************************************************************************/ +/*! \file bma423.h + \brief Sensor Driver for BMA423 sensor */ +#ifndef BMA423_H +#define BMA423_H + +#ifdef __cplusplus +extern "C" { +#endif +#include "bma4.h" + +/**\name Chip ID of BMA423 sensor */ +#define BMA423_CHIP_ID UINT8_C(0x13) + +/**\name Sensor feature size */ +#define BMA423_FEATURE_SIZE UINT8_C(64) +#define BMA423_ANYMOTION_EN_LEN UINT8_C(2) +#define BMA423_RD_WR_MIN_LEN UINT8_C(2) + +/**\name Feature offset address */ +#define BMA423_ANY_NO_MOTION_OFFSET UINT8_C(0x00) +#define BMA423_STEP_CNTR_OFFSET UINT8_C(0x36) +#define BMA423_STEP_CNTR_PARAM_OFFSET UINT8_C(0x04) +#define BMA423_WAKEUP_OFFSET UINT8_C(0x38) +#define BMA423_TILT_OFFSET UINT8_C(0x3A) +#define BMA423_CONFIG_ID_OFFSET UINT8_C(0x3C) +#define BMA423_AXES_REMAP_OFFSET UINT8_C(0x3E) + + +/**************************************************************/ +/**\name Remap Axes */ +/**************************************************************/ +#define BMA423_X_AXIS_MASK UINT8_C(0x03) +#define BMA423_X_AXIS_SIGN_MASK UINT8_C(0x04) +#define BMA423_Y_AXIS_MASK UINT8_C(0x18) +#define BMA423_Y_AXIS_SIGN_MASK UINT8_C(0x20) +#define BMA423_Z_AXIS_MASK UINT8_C(0xC0) +#define BMA423_Z_AXIS_SIGN_MASK UINT8_C(0x01) + +/**************************************************************/ +/**\name Step Counter & Detector */ +/**************************************************************/ +/**\name Step counter enable macros */ +#define BMA423_STEP_CNTR_EN_POS UINT8_C(4) +#define BMA423_STEP_CNTR_EN_MSK UINT8_C(0x10) +#define BMA423_ACTIVITY_EN_MSK UINT8_C(0x20) + +/**\name Step counter watermark macros */ +#define BMA423_STEP_CNTR_WM_MSK UINT16_C(0x03FF) + +/**\name Step counter reset macros */ +#define BMA423_STEP_CNTR_RST_POS UINT8_C(2) +#define BMA423_STEP_CNTR_RST_MSK UINT8_C(0x04) + +/**\name Step detector enable macros */ +#define BMA423_STEP_DETECTOR_EN_POS UINT8_C(3) +#define BMA423_STEP_DETECTOR_EN_MSK UINT8_C(0x08) + +/**\name Tilt enable macros */ +#define BMA423_TILT_EN_MSK UINT8_C(0x01) + +/**\name Step count output length*/ +#define BMA423_STEP_CNTR_DATA_SIZE UINT16_C(4) + +/**\name Wakeup enable macros */ +#define BMA423_WAKEUP_EN_MSK UINT8_C(0x01) + +/**\name Wake up sensitivity macros */ +#define BMA423_WAKEUP_SENS_POS UINT8_C(1) +#define BMA423_WAKEUP_SENS_MSK UINT8_C(0x0E) + +/**\name Tap selection macro */ +#define BMA423_TAP_SEL_POS UINT8_C(4) +#define BMA423_TAP_SEL_MSK UINT8_C(0x10) + +/**************************************************************/ +/**\name Any Motion */ +/**************************************************************/ +/**\name Any motion threshold macros */ +#define BMA423_ANY_NO_MOTION_THRES_POS UINT8_C(0) +#define BMA423_ANY_NO_MOTION_THRES_MSK UINT16_C(0x07FF) + +/**\name Any motion selection macros */ +#define BMA423_ANY_NO_MOTION_SEL_POS UINT8_C(3) +#define BMA423_ANY_NO_MOTION_SEL_MSK UINT8_C(0x08) + +/**\name Any motion enable macros */ +#define BMA423_ANY_NO_MOTION_AXIS_EN_POS UINT8_C(5) +#define BMA423_ANY_NO_MOTION_AXIS_EN_MSK UINT8_C(0xE0) + +/**\name Any motion duration macros */ +#define BMA423_ANY_NO_MOTION_DUR_MSK UINT16_C(0x1FFF) + +/**************************************************************/ +/**\name User macros */ +/**************************************************************/ + +/**\name Anymotion/Nomotion axis enable macros */ +#define BMA423_X_AXIS_EN UINT8_C(0x01) +#define BMA423_Y_AXIS_EN UINT8_C(0x02) +#define BMA423_Z_AXIS_EN UINT8_C(0x04) +#define BMA423_ALL_AXIS_EN UINT8_C(0x07) +#define BMA423_ALL_AXIS_DIS UINT8_C(0x00) + +/**\name Feature enable macros for the sensor */ +#define BMA423_STEP_CNTR UINT8_C(0x01) +/**\name Below macros are mutually exclusive */ +#define BMA423_ANY_MOTION UINT8_C(0x02) +#define BMA423_NO_MOTION UINT8_C(0x04) +#define BMA423_ACTIVITY UINT8_C(0x08) +#define BMA423_TILT UINT8_C(0x10) +#define BMA423_WAKEUP UINT8_C(0x20) + +/**\name Interrupt status macros */ +#define BMA423_STEP_CNTR_INT UINT8_C(0x02) +#define BMA423_ACTIVITY_INT UINT8_C(0x04) +#define BMA423_TILT_INT UINT8_C(0x08) +#define BMA423_WAKEUP_INT UINT8_C(0x20) +#define BMA423_ANY_NO_MOTION_INT UINT8_C(0x40) +#define BMA423_ERROR_INT UINT8_C(0x80) + +/**\name Activity recognition macros */ +#define BMA423_USER_STATIONARY UINT8_C(0x00) +#define BMA423_USER_WALKING UINT8_C(0x01) +#define BMA423_USER_RUNNING UINT8_C(0x02) +#define BMA423_STATE_INVALID UINT8_C(0x03) + +/**\name Configuration selection macros */ +#define BMA423_PHONE_CONFIG UINT8_C(0x00) +#define BMA423_WRIST_CONFIG UINT8_C(0x01) + +/**\name Step counter parameter setting(1-25) for phone */ +#define BMA423_PHONE_SC_PARAM_1 UINT16_C(0x132) +#define BMA423_PHONE_SC_PARAM_2 UINT16_C(0x78E6) +#define BMA423_PHONE_SC_PARAM_3 UINT16_C(0x84) +#define BMA423_PHONE_SC_PARAM_4 UINT16_C(0x6C9C) +#define BMA423_PHONE_SC_PARAM_5 UINT8_C(0x07) +#define BMA423_PHONE_SC_PARAM_6 UINT16_C(0x7564) +#define BMA423_PHONE_SC_PARAM_7 UINT16_C(0x7EAA) +#define BMA423_PHONE_SC_PARAM_8 UINT16_C(0x55F) +#define BMA423_PHONE_SC_PARAM_9 UINT16_C(0xABE) +#define BMA423_PHONE_SC_PARAM_10 UINT16_C(0x55F) +#define BMA423_PHONE_SC_PARAM_11 UINT16_C(0xE896) +#define BMA423_PHONE_SC_PARAM_12 UINT16_C(0x41EF) +#define BMA423_PHONE_SC_PARAM_13 UINT8_C(0x01) +#define BMA423_PHONE_SC_PARAM_14 UINT8_C(0x0C) +#define BMA423_PHONE_SC_PARAM_15 UINT8_C(0x0C) +#define BMA423_PHONE_SC_PARAM_16 UINT8_C(0x4A) +#define BMA423_PHONE_SC_PARAM_17 UINT8_C(0xA0) +#define BMA423_PHONE_SC_PARAM_18 UINT8_C(0x00) +#define BMA423_PHONE_SC_PARAM_19 UINT8_C(0x0C) +#define BMA423_PHONE_SC_PARAM_20 UINT16_C(0x3CF0) +#define BMA423_PHONE_SC_PARAM_21 UINT16_C(0x100) +#define BMA423_PHONE_SC_PARAM_22 UINT8_C(0x00) +#define BMA423_PHONE_SC_PARAM_23 UINT8_C(0x00) +#define BMA423_PHONE_SC_PARAM_24 UINT8_C(0x00) +#define BMA423_PHONE_SC_PARAM_25 UINT8_C(0x00) + +/**\name Step counter parameter setting(1-25) for wrist (Default) */ +#define BMA423_WRIST_SC_PARAM_1 UINT16_C(0x12D) +#define BMA423_WRIST_SC_PARAM_2 UINT16_C(0x7BD4) +#define BMA423_WRIST_SC_PARAM_3 UINT16_C(0x13B) +#define BMA423_WRIST_SC_PARAM_4 UINT16_C(0x7ADB) +#define BMA423_WRIST_SC_PARAM_5 UINT8_C(0x04) +#define BMA423_WRIST_SC_PARAM_6 UINT16_C(0x7B3F) +#define BMA423_WRIST_SC_PARAM_7 UINT16_C(0x6CCD) +#define BMA423_WRIST_SC_PARAM_8 UINT16_C(0x4C3) +#define BMA423_WRIST_SC_PARAM_9 UINT16_C(0x985) +#define BMA423_WRIST_SC_PARAM_10 UINT16_C(0x4C3) +#define BMA423_WRIST_SC_PARAM_11 UINT16_C(0xE6EC) +#define BMA423_WRIST_SC_PARAM_12 UINT16_C(0x460C) +#define BMA423_WRIST_SC_PARAM_13 UINT8_C(0x01) +#define BMA423_WRIST_SC_PARAM_14 UINT8_C(0x27) +#define BMA423_WRIST_SC_PARAM_15 UINT8_C(0x19) +#define BMA423_WRIST_SC_PARAM_16 UINT8_C(0x96) +#define BMA423_WRIST_SC_PARAM_17 UINT8_C(0xA0) +#define BMA423_WRIST_SC_PARAM_18 UINT8_C(0x01) +#define BMA423_WRIST_SC_PARAM_19 UINT8_C(0x0C) +#define BMA423_WRIST_SC_PARAM_20 UINT16_C(0x3CF0) +#define BMA423_WRIST_SC_PARAM_21 UINT16_C(0x100) +#define BMA423_WRIST_SC_PARAM_22 UINT8_C(0x01) +#define BMA423_WRIST_SC_PARAM_23 UINT8_C(0x03) +#define BMA423_WRIST_SC_PARAM_24 UINT8_C(0x01) +#define BMA423_WRIST_SC_PARAM_25 UINT8_C(0x0E) + +/*! + * @brief Any motion configuration + */ +struct bma423_anymotion_config { + /*! Expressed in 50 Hz samples (20 ms) */ + uint16_t duration; + /*! Threshold value for Any-motion / No-motion detection in + 5.11g format */ + uint16_t threshold; + /*! Indicates if No-motion or Any-motion is selected */ + uint8_t nomotion_sel; +}; + +/*! + * @brief Axes remapping configuration + */ +struct bma423_axes_remap { + uint8_t x_axis; + uint8_t x_axis_sign; + uint8_t y_axis; + uint8_t y_axis_sign; + uint8_t z_axis; + uint8_t z_axis_sign; +}; + +/*! + * @brief Step counter param settings + */ +struct bma423_stepcounter_settings { + /*! Step Counter param 1 */ + uint16_t param1; + /*! Step Counter param 2 */ + uint16_t param2; + /*! Step Counter param 3 */ + uint16_t param3; + /*! Step Counter param 4 */ + uint16_t param4; + /*! Step Counter param 5 */ + uint16_t param5; + /*! Step Counter param 6 */ + uint16_t param6; + /*! Step Counter param 7 */ + uint16_t param7; + /*! Step Counter param 8 */ + uint16_t param8; + /*! Step Counter param 9 */ + uint16_t param9; + /*! Step Counter param 10 */ + uint16_t param10; + /*! Step Counter param 11 */ + uint16_t param11; + /*! Step Counter param 12 */ + uint16_t param12; + /*! Step Counter param 13 */ + uint16_t param13; + /*! Step Counter param 14 */ + uint16_t param14; + /*! Step Counter param 15 */ + uint16_t param15; + /*! Step Counter param 16 */ + uint16_t param16; + /*! Step Counter param 17 */ + uint16_t param17; + /*! Step Counter param 18 */ + uint16_t param18; + /*! Step Counter param 19 */ + uint16_t param19; + /*! Step Counter param 20 */ + uint16_t param20; + /*! Step Counter param 21 */ + uint16_t param21; + /*! Step Counter param 22 */ + uint16_t param22; + /*! Step Counter param 23 */ + uint16_t param23; + /*! Step Counter param 24 */ + uint16_t param24; + /*! Step Counter param 25 */ + uint16_t param25; +}; + +/*! + * @brief This API is the entry point. + * Call this API before using all other APIs. + * This API reads the chip-id of the sensor and sets the resolution. + * + * @param[in,out] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_init(struct bma4_dev *dev); + +/*! + * @brief This API is used to upload the config file to enable + * the features of the sensor. + * + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_write_config_file(struct bma4_dev *dev); + +/*! + * @brief This API is used to get the configuration id of the sensor. + * + * @param[out] config_id : Pointer variable used to store + * the configuration id. + * @param[in] dev : Structure instance of bma4_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_get_config_id(uint16_t *config_id, struct bma4_dev *dev); + + +/*! + * @brief This API sets/unsets the user provided interrupt to either + * interrupt pin1 or pin2 in the sensor. + * + * @param[in] int_line: Variable to select either interrupt pin1 or pin2. + * int_line | Macros + * ------------|------------------- + * 0 | BMA4_INTR1_MAP + * 1 | BMA4_INTR2_MAP + * @param[in] int_map : Variable to specify the interrupts. + * @param[in] enable : Variable to specify mapping or unmapping of + * interrupts. + * enable | Macros + * --------------------|------------------- + * 0x00 | BMA4_DISABLE + * 0x01 | BMA4_ENABLE + * @param[in] dev : Structure instance of bma4_dev. + * + * @note Below macros specify the interrupts. + * Feature Interrupts + * - BMA423_STEP_CNTR_INT + * - BMA423_ACTIVITY_INT + * - BMA423_TILT_INT + * - BMA423_WAKEUP_INT + * - BMA423_ANY_NO_MOTION_INT + * - BMA423_ERROR_INT + * + * Hardware Interrupts + * - BMA4_FIFO_FULL_INT + * - BMA4_FIFO_WM_INT + * - BMA4_DATA_RDY_INT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_map_interrupt(uint8_t int_line, uint16_t int_map, uint8_t enable, struct bma4_dev *dev); + +/*! + * @brief This API reads the bma423 interrupt status from the sensor. + * + * @param[out] int_status : Variable to store the interrupt status + * read from the sensor. + * @param[in] dev : Structure instance of bma4_dev. + * + * @note Below macros are used to check the interrupt status. + * Feature Interrupts + * + * - BMA423_STEP_CNTR_INT + * - BMA423_ACTIVITY_INT + * - BMA423_TILT_INT + * - BMA423_WAKEUP_INT + * - BMA423_ANY_NO_MOTION_INT + * - BMA423_ERROR_INT + * + * + * Hardware Interrupts + * - BMA4_FIFO_FULL_INT + * - BMA4_FIFO_WM_INT + * - BMA4_MAG_DATA_RDY_INT + * - BMA4_ACCEL_DATA_RDY_INT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_read_int_status(uint16_t *int_status, struct bma4_dev *dev); + +/*! + * @brief This API enables/disables the features of the sensor. + * + * @param[in] feature : Variable to specify the features + * which are to be set in bma423 sensor. + * @param[in] enable : Variable which specifies whether to enable or + * disable the features in the bma423 sensor + * enable | Macros + * --------------------|------------------- + * 0x00 | BMA4_DISABLE + * 0x01 | BMA4_ENABLE + * @param[in] dev : Structure instance of bma4_dev. + * + * @note User should use the below macros to enable or disable the + * features of bma423 sensor + * - BMA423_STEP_CNTR + * - BMA423_ANY_MOTION (or) BMA423_NO_MOTION + * - BMA423_ACTIVITY + * - BMA423_WAKEUP + * - BMA423_TILT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_feature_enable(uint8_t feature, uint8_t enable, struct bma4_dev *dev); + + +/*! + * @brief This API performs x, y and z axis remapping in the sensor. + * + * @param[in] remap_data : Pointer to store axes remapping data. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_set_remap_axes(const struct bma423_axes_remap *remap_data, struct bma4_dev *dev); + +/*! + * @brief This API reads the x, y and z axis remap data from the sensor. + * + * @param[out] remap_data : Pointer to store axis remap data which is read + * from the bma423 sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_get_remap_axes(struct bma423_axes_remap *remap_data, struct bma4_dev *dev); + + +/*! + * @brief This API sets the watermark level for step counter + * interrupt in the sensor. + * + * @param[in] step_counter_wm : Variable which specifies watermark level + * count + * @note Valid values are from 1 to 1023 + * @note Value 0 is used for step detector interrupt + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_step_counter_set_watermark(uint16_t step_counter_wm, struct bma4_dev *dev); + +/*! + * @brief This API gets the water mark level set for step counter interrupt + * in the sensor + * + * @param[out] step_counter_wm : Pointer variable which stores + * the water mark level read from the sensor. + * @note valid values are from 1 to 1023 + * @note value 0 is used for step detector interrupt + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_step_counter_get_watermark(uint16_t *step_counter_wm, struct bma4_dev *dev); + +/*! + * @brief This API resets the counted steps of step counter. + * + * @param[in] dev : structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_reset_step_counter(struct bma4_dev *dev); + +/*! + * @brief This API gets the number of counted steps of the step counter + * feature from the sensor. + * + * @param[out] step_count : Pointer variable which stores counted steps + * read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_step_counter_output(uint32_t *step_count, struct bma4_dev *dev); + +/*! + * @brief This API gets the output for activity feature. + * + * @param[out] activity : Pointer variable which stores activity output + * read from the sensor. + * activity | State + * --------------|------------------------ + * 0x00 | BMA423_USER_STATIONARY + * 0x01 | BMA423_USER_WALKING + * 0x02 | BMA423_USER_RUNNING + * 0x03 | BMA423_STATE_INVALID + * + * @param[in] dev : Structure instance of bma4_dev + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_activity_output(uint8_t *activity, struct bma4_dev *dev); + +/*! + * @brief This API select the platform configuration wrist(default) or phone. + * + * @param[in] platform : Variable to select wrist/phone + * + * platform | Macros + * -------------|------------------------ + * 0x00 | BMA423_PHONE_CONFIG + * 0x01 | BMA423_WRIST_CONFIG + * + * @param[in] dev : Structure instance of bma4_dev + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_select_platform(uint8_t platform, struct bma4_dev *dev); + +/*! + * @brief This API gets the parameter1 to parameter7 settings of the + * step counter feature. + * + * @param[out] setting : Pointer to structure variable which stores the + * parameter1 to parameter7 read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_stepcounter_get_parameter(struct bma423_stepcounter_settings *setting, struct bma4_dev *dev); + +/*! + * @brief This API sets the parameter1 to parameter7 settings of the + * step counter feature in the sensor. + * + * @param[in] setting : Pointer to structure variable which stores the + * parameter1 to parameter7 settings read from the sensor. + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_stepcounter_set_parameter(const struct bma423_stepcounter_settings *setting, struct bma4_dev *dev); + +/*! + * @brief This API enables or disables the step detector feature in the + * sensor. + * + * @param[in] enable : Variable used to enable or disable step detector + * enable | Macros + * --------------------|------------------- + * 0x00 | BMA4_DISABLE + * 0x01 | BMA4_ENABLE + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_step_detector_enable(uint8_t enable, struct bma4_dev *dev); + +/*! + * @brief This API enables the any motion feature according to the axis + * set by the user in the sensor. + * + * @param[in] axis : Variable to specify the axis of the any motion feature + * to be enabled in the sensor. + * Value | Axis + * ---------|------------------------- + * 0x00 | BMA423_ALL_AXIS_DIS + * 0x01 | BMA423_X_AXIS_EN + * 0x02 | BMA423_Y_AXIS_EN + * 0x04 | BMA423_Z_AXIS_EN + * 0x07 | BMA423_ALL_AXIS_EN + * @param[in] dev : Structure instance of bma4_dev + * + * @return result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_anymotion_enable_axis(uint8_t axis, struct bma4_dev *dev); + +/*! @brief This API sets the configuration of Any motion feature in + * the sensor. + * + * @param[in] any_motion : Pointer to structure variable to specify + * the any motion feature settings. + * Structure members are provided in the table below + *@verbatim + * ------------------------------------------------------------------------- + * Structure parameters | Description + * --------------------------------|---------------------------------------- + * | Defines the number of + * | consecutive data points for + * | which the threshold condition + * duration | must be respected, for interrupt + * | assertion. It is expressed in + * | 50 Hz samples (20 ms). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * --------------------------------|---------------------------------------- + * | Slope threshold value for + * | Any-motion / No-motion detection + * threshold | in 5.11g format. + * | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * --------------------------------|---------------------------------------- + * | Indicates if No motion (1) or + * nomotion_sel | Any-motion (0) is selected; + * | default value is 0 Any-motion. + * ------------------------------------------------------------------------- + *@endverbatim + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_set_any_motion_config(const struct bma423_anymotion_config *any_motion, struct bma4_dev *dev); + +/*! @brief This API gets the configuration of any motion feature from + * the sensor. + * + * @param[out] any_motion : Pointer to structure variable used to store + * the any motion feature settings read from the sensor. + * Structure members are provided in the table below + *@verbatim + * ------------------------------------------------------------------------- + * Structure parameters | Description + * --------------------------------|---------------------------------------- + * | Defines the number of + * | consecutive data points for + * | which the threshold condition + * duration | must be respected, for interrupt + * | assertion. It is expressed in + * | 50 Hz samples (20 ms). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * --------------------------------|---------------------------------------- + * | Slope threshold value for + * | Any-motion / No-motion detection + * threshold | in 5.11g format. + * | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * --------------------------------|---------------------------------------- + * | Indicates if No motion (1) or + * nomotion_sel | Any-motion (0) is selected; + * | default value is 0 Any-motion. + * ------------------------------------------------------------------------- + *@endverbatim + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_get_any_motion_config(struct bma423_anymotion_config *any_motion, struct bma4_dev *dev); + +/*! + * @brief This API sets the sensitivity of wake up feature in the sensor + * + * @param[in] sensitivity : Variable used to specify the sensitivity of the + * Wake up feature. + * Value | Sensitivity + * --------|------------------------- + * 0x00 | MOST SENSITIVE + * 0x07 | LEAST SENSITIVE + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_wakeup_set_sensitivity(uint8_t sensitivity, struct bma4_dev *dev); + +/*! + * @brief This API gets the sensitivity of wake up feature in the sensor + * + * @param[out] sensitivity : Pointer variable which stores the sensitivity + * value read from the sensor. + * Value | Sensitivity + * --------|------------------------- + * 0x00 | MOST SENSITIVE + * 0x07 | LEAST SENSITIVE + * @param[in] dev : Structure instance of bma4_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval Any non zero value -> Fail + */ +uint16_t bma423_wakeup_get_sensitivity(uint8_t *sensitivity, struct bma4_dev *dev); + +/*! + * @brief This API is used to select single/double tap + * feature in the sensor + * + * @param tap_select : Variable used to specify the single or + * double tap selection in the sensor + * tap_select | description + * ------------|------------------------ + * 0x00 | Double tap selected + * 0x01 | single tap selected + * + * @param dev : Structure instance of bma4_dev + * + * @return results of stream_transfer operation + * @retval 0 -> Success + * @retval Any positive value mentioned in ERROR CODES -> Fail + * + */ +uint16_t bma423_tap_selection(const uint8_t tap_select, struct bma4_dev *dev); + +#ifdef __cplusplus +} +#endif /*End of CPP guard */ + +#endif /*End of header guard macro */ \ No newline at end of file diff --git a/src/bma4_defs.h b/src/bma4_defs.h new file mode 100644 index 0000000..34e421b --- /dev/null +++ b/src/bma4_defs.h @@ -0,0 +1,960 @@ +/* +* +**************************************************************************** +* Copyright (C) 2017 - 2018 Bosch Sensortec GmbH +* +* File : bma4_defs.h +* +* Date: 12 Oct 2017 +* +* Revision: 2.1.9 $ +* +* Usage: Sensor Driver for BMA4 family of sensors +* +**************************************************************************** +* +* Disclaimer +* +* Common: +* Bosch Sensortec products are developed for the consumer goods industry. +* They may only be used within the parameters of the respective valid +* product data sheet. Bosch Sensortec products are provided with the +* express understanding that there is no warranty of fitness for a +* particular purpose.They are not fit for use in life-sustaining, +* safety or security sensitive systems or any system or device +* that may lead to bodily harm or property damage if the system +* or device malfunctions. In addition,Bosch Sensortec products are +* not fit for use in products which interact with motor vehicle systems. +* The resale and or use of products are at the purchasers own risk and +* his own responsibility. The examination of fitness for the intended use +* is the sole responsibility of the Purchaser. +* +* The purchaser shall indemnify Bosch Sensortec from all third party +* claims, including any claims for incidental, or consequential damages, +* arising from any product use not covered by the parameters of +* the respective valid product data sheet or not approved by +* Bosch Sensortec and reimburse Bosch Sensortec for all costs in +* connection with such claims. +* +* The purchaser must monitor the market for the purchased products, +* particularly with regard to product safety and inform Bosch Sensortec +* without delay of all security relevant incidents. +* +* Engineering Samples are marked with an asterisk (*) or (e). +* Samples may vary from the valid technical specifications of the product +* series. They are therefore not intended or fit for resale to third +* parties or for use in end products. Their sole purpose is internal +* client testing. The testing of an engineering sample may in no way +* replace the testing of a product series. Bosch Sensortec assumes +* no liability for the use of engineering samples. +* By accepting the engineering samples, the Purchaser agrees to indemnify +* Bosch Sensortec from all claims arising from the use of engineering +* samples. +* +* Special: +* This software module (hereinafter called "Software") and any information +* on application-sheets (hereinafter called "Information") is provided +* free of charge for the sole purpose to support your application work. +* The Software and Information is subject to the following +* terms and conditions: +* +* The Software is specifically designed for the exclusive use for +* Bosch Sensortec products by personnel who have special experience +* and training. Do not use this Software if you do not have the +* proper experience or training. +* +* This Software package is provided `` as is `` and without any expressed +* or implied warranties,including without limitation, the implied warranties +* of merchantability and fitness for a particular purpose. +* +* Bosch Sensortec and their representatives and agents deny any liability +* for the functional impairment +* of this Software in terms of fitness, performance and safety. +* Bosch Sensortec and their representatives and agents shall not be liable +* for any direct or indirect damages or injury, except as +* otherwise stipulated in mandatory applicable law. +* +* The Information provided is believed to be accurate and reliable. +* Bosch Sensortec assumes no responsibility for the consequences of use +* of such Information nor for any infringement of patents or +* other rights of third parties which may result from its use. +* No license is granted by implication or otherwise under any patent or +* patent rights of Bosch. Specifications mentioned in the Information are +* subject to change without notice. +**************************************************************************/ +/*! \file bma4_defs.h + \brief Sensor Driver for BMA4 family of sensors */ +#ifndef BMA4_DEFS_H__ +#define BMA4_DEFS_H__ +/*********************************************************************/ +/**\ header files */ +#ifdef __KERNEL__ +#include +#else +#include +#include +#include +#endif + +/*********************************************************************/ +/* macro definitions */ + +/* +#if (LONG_MAX) > 0x7fffffff +#define __have_long64 1 +#elif (LONG_MAX) == 0x7fffffff +#define __have_long32 1 +#endif +*/ + +#if !defined(UINT8_C) +#define INT8_C(x) x +#if (INT_MAX) > 0x7f +#define UINT8_C(x) x +#else +#define UINT8_C(x) x##U +#endif +#endif + +#if !defined(UINT16_C) +#define INT16_C(x) x +#if (INT_MAX) > 0x7fff +#define UINT16_C(x) x +#else +#define UINT16_C(x) x##U +#endif +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#if __have_long32 +#define INT32_C(x) x##L +#define UINT32_C(x) x##UL +#else +#define INT32_C(x) x +#define UINT32_C(x) x##U +#endif +#endif + +#if !defined(INT64_C) && !defined(UINT64_C) +#if __have_long64 +#define INT64_C(x) x##L +#define UINT64_C(x) x##UL +#else +#define INT64_C(x) x##LL +#define UINT64_C(x) x##ULL +#endif +#endif + +/**\name CHIP ID ADDRESS*/ +#define BMA4_CHIP_ID_ADDR UINT8_C(0x00) + +/**\name ERROR STATUS*/ +#define BMA4_ERROR_ADDR UINT8_C(0X02) + +/**\name STATUS REGISTER FOR SENSOR STATUS FLAG*/ +#define BMA4_STATUS_ADDR UINT8_C(0X03) + +/**\name AUX/ACCEL DATA BASE ADDRESS REGISTERS*/ +#define BMA4_DATA_0_ADDR UINT8_C(0X0A) +#define BMA4_DATA_8_ADDR UINT8_C(0X12) + +/**\name SENSOR TIME REGISTERS*/ +#define BMA4_SENSORTIME_0_ADDR UINT8_C(0X18) + +/**\name INTERRUPT/FEATURE STATUS REGISTERS*/ +#define BMA4_INT_STAT_0_ADDR UINT8_C(0X1C) + +/**\name INTERRUPT/FEATURE STATUS REGISTERS*/ +#define BMA4_INT_STAT_1_ADDR UINT8_C(0X1D) + +/**\name TEMPERATURE REGISTERS*/ +#define BMA4_TEMPERATURE_ADDR UINT8_C(0X22) + +/**\name FIFO REGISTERS*/ +#define BMA4_FIFO_LENGTH_0_ADDR UINT8_C(0X24) +#define BMA4_FIFO_DATA_ADDR UINT8_C(0X26) + +/**\name ACCEL CONFIG REGISTERS*/ +#define BMA4_ACCEL_CONFIG_ADDR UINT8_C(0X40) + +/**\name ACCEL RANGE ADDRESS*/ +#define BMA4_ACCEL_RANGE_ADDR UINT8_C(0X41) + +/**\name AUX CONFIG REGISTERS*/ +#define BMA4_AUX_CONFIG_ADDR UINT8_C(0X44) + +/**\name FIFO DOWN SAMPLING REGISTER ADDRESS FOR ACCEL*/ +#define BMA4_FIFO_DOWN_ADDR UINT8_C(0X45) + +/**\name FIFO WATERMARK REGISTER ADDRESS*/ +#define BMA4_FIFO_WTM_0_ADDR UINT8_C(0X46) + +/**\name FIFO CONFIG REGISTERS*/ +#define BMA4_FIFO_CONFIG_0_ADDR UINT8_C(0X48) +#define BMA4_FIFO_CONFIG_1_ADDR UINT8_C(0X49) + +/**\name MAG INTERFACE REGISTERS*/ +#define BMA4_AUX_DEV_ID_ADDR UINT8_C(0X4B) +#define BMA4_AUX_IF_CONF_ADDR UINT8_C(0X4C) +#define BMA4_AUX_RD_ADDR UINT8_C(0X4D) +#define BMA4_AUX_WR_ADDR UINT8_C(0X4E) +#define BMA4_AUX_WR_DATA_ADDR UINT8_C(0X4F) + +/**\name INTERRUPT ENABLE REGISTERS*/ +#define BMA4_INT1_IO_CTRL_ADDR UINT8_C(0X53) +#define BMA4_INT2_IO_CTRL_ADDR UINT8_C(0X54) + +/**\name LATCH DURATION REGISTERS*/ +#define BMA4_INTR_LATCH_ADDR UINT8_C(0X55) + +/**\name MAP INTERRUPT 1 and 2 REGISTERS*/ +#define BMA4_INT_MAP_1_ADDR UINT8_C(0X56) +#define BMA4_INT_MAP_2_ADDR UINT8_C(0X57) +#define BMA4_INT_MAP_DATA_ADDR UINT8_C(0x58) +#define BMA4_INIT_CTRL_ADDR UINT8_C(0x59) + +/**\name FEATURE CONFIG RELATED */ +#define BMA4_RESERVED_REG_5B_ADDR UINT8_C(0x5B) +#define BMA4_RESERVED_REG_5C_ADDR UINT8_C(0x5C) +#define BMA4_FEATURE_CONFIG_ADDR UINT8_C(0x5E) +#define BMA4_INTERNAL_ERROR UINT8_C(0x5F) + +/**\name SERIAL INTERFACE SETTINGS REGISTER*/ +#define BMA4_IF_CONFIG_ADDR UINT8_C(0X6B) + +/**\name SELF_TEST REGISTER*/ +#define BMA4_ACC_SELF_TEST_ADDR UINT8_C(0X6D) + +/**\name SPI,I2C SELECTION REGISTER*/ +#define BMA4_NV_CONFIG_ADDR UINT8_C(0x70) + +/**\name ACCEL OFFSET REGISTERS*/ +#define BMA4_OFFSET_0_ADDR UINT8_C(0X71) +#define BMA4_OFFSET_1_ADDR UINT8_C(0X72) +#define BMA4_OFFSET_2_ADDR UINT8_C(0X73) + +/**\name POWER_CTRL REGISTER*/ +#define BMA4_POWER_CONF_ADDR UINT8_C(0x7C) +#define BMA4_POWER_CTRL_ADDR UINT8_C(0x7D) + +/**\name COMMAND REGISTER*/ +#define BMA4_CMD_ADDR UINT8_C(0X7E) + +/**\name GPIO REGISTERS*/ +#define BMA4_STEP_CNT_OUT_0_ADDR UINT8_C(0x1E) +#define BMA4_HIGH_G_OUT_ADDR UINT8_C(0x1F) +#define BMA4_ACTIVITY_OUT_ADDR UINT8_C(0x27) +#define BMA4_ORIENTATION_OUT_ADDR UINT8_C(0x28) +#define BMA4_INTERNAL_STAT UINT8_C(0x2A) + +/*! + * @brief Block size for config write */ +#define BMA4_BLOCK_SIZE UINT8_C(32) + +/**\name I2C slave address */ +#define BMA4_I2C_ADDR_PRIMARY UINT8_C(0x18) +#define BMA4_I2C_ADDR_SECONDARY UINT8_C(0x19) +#define BMA4_I2C_BMM150_ADDR UINT8_C(0x10) + +/**\name Interface selection macro */ +#define BMA4_SPI_INTERFACE UINT8_C(1) +#define BMA4_I2C_INTERFACE UINT8_C(2) + +/**\name Interface selection macro */ +#define BMA4_SPI_WR_MASK UINT8_C(0x7F) +#define BMA4_SPI_RD_MASK UINT8_C(0x80) + +/**\name Chip ID macros */ +#define BMA4_CHIP_ID_MIN UINT8_C(0x10) +#define BMA4_CHIP_ID_MAX UINT8_C(0x15) + +/**\name Auxiliary sensor selection macro */ +#define BMM150_SENSOR UINT8_C(1) +#define AKM9916_SENSOR UINT8_C(2) +#define BMA4_ASIC_INITIALIZED UINT8_C(0x01) + +/**\name Auxiliary sensor chip id macros */ +#define BMM150_CHIP_ID UINT8_C(0x32) + +/**\name Auxiliary sensor other macros */ +#define BMM150_POWER_CONTROL_REG UINT8_C(0x4B) +#define BMM150_POWER_MODE_REG UINT8_C(0x4C) + +/**\name CONSTANTS */ +#define BMA4_FIFO_CONFIG_LENGTH UINT8_C(2) +#define BMA4_ACCEL_CONFIG_LENGTH UINT8_C(2) +#define BMA4_FIFO_WM_LENGTH UINT8_C(2) +#define BMA4_CONFIG_STREAM_SIZE UINT16_C(6144) +#define BMA4_NON_LATCH_MODE UINT8_C(0) +#define BMA4_LATCH_MODE UINT8_C(1) +#define BMA4_OPEN_DRAIN UINT8_C(1) +#define BMA4_PUSH_PULL UINT8_C(0) +#define BMA4_ACTIVE_HIGH UINT8_C(1) +#define BMA4_ACTIVE_LOW UINT8_C(0) +#define BMA4_EDGE_TRIGGER UINT8_C(1) +#define BMA4_LEVEL_TRIGGER UINT8_C(0) +#define BMA4_OUTPUT_ENABLE UINT8_C(1) +#define BMA4_OUTPUT_DISABLE UINT8_C(0) +#define BMA4_INPUT_ENABLE UINT8_C(1) +#define BMA4_INPUT_DISABLE UINT8_C(0) + +/**\name ACCEL RANGE CHECK*/ +#define BMA4_ACCEL_RANGE_2G UINT8_C(0) +#define BMA4_ACCEL_RANGE_4G UINT8_C(1) +#define BMA4_ACCEL_RANGE_8G UINT8_C(2) +#define BMA4_ACCEL_RANGE_16G UINT8_C(3) + +/**\name CONDITION CHECK FOR READING AND WRTING DATA*/ +#define BMA4_MAX_VALUE_FIFO_FILTER UINT8_C(1) +#define BMA4_MAX_VALUE_SPI3 UINT8_C(1) +#define BMA4_MAX_VALUE_SELFTEST_AMP UINT8_C(1) +#define BMA4_MAX_IF_MODE UINT8_C(3) +#define BMA4_MAX_VALUE_SELFTEST_SIGN UINT8_C(1) + +/**\name BUS READ AND WRITE LENGTH FOR MAG & ACCEL*/ +#define BMA4_MAG_TRIM_DATA_SIZE UINT8_C(16) +#define BMA4_MAG_XYZ_DATA_LENGTH UINT8_C(6) +#define BMA4_MAG_XYZR_DATA_LENGTH UINT8_C(8) +#define BMA4_ACCEL_DATA_LENGTH UINT8_C(6) +#define BMA4_FIFO_DATA_LENGTH UINT8_C(2) +#define BMA4_TEMP_DATA_SIZE UINT8_C(1) + +/**\name TEMPERATURE CONSTANT */ +#define BMA4_OFFSET_TEMP UINT8_C(23) +#define BMA4_DEG UINT8_C(1) +#define BMA4_FAHREN UINT8_C(2) +#define BMA4_KELVIN UINT8_C(3) + +/**\name DELAY DEFINITION IN MSEC*/ +#define BMA4_AUX_IF_DELAY UINT8_C(5) +#define BMA4_BMM150_WAKEUP_DELAY1 UINT8_C(2) +#define BMA4_BMM150_WAKEUP_DELAY2 UINT8_C(3) +#define BMA4_BMM150_WAKEUP_DELAY3 UINT8_C(1) +#define BMA4_GEN_READ_WRITE_DELAY UINT8_C(1) +#define BMA4_AUX_COM_DELAY UINT8_C(10) + +/**\name ARRAY PARAMETER DEFINITIONS*/ +#define BMA4_SENSOR_TIME_MSB_BYTE UINT8_C(2) +#define BMA4_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMA4_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMA4_MAG_X_LSB_BYTE UINT8_C(0) +#define BMA4_MAG_X_MSB_BYTE UINT8_C(1) +#define BMA4_MAG_Y_LSB_BYTE UINT8_C(2) +#define BMA4_MAG_Y_MSB_BYTE UINT8_C(3) +#define BMA4_MAG_Z_LSB_BYTE UINT8_C(4) +#define BMA4_MAG_Z_MSB_BYTE UINT8_C(5) +#define BMA4_MAG_R_LSB_BYTE UINT8_C(6) +#define BMA4_MAG_R_MSB_BYTE UINT8_C(7) +#define BMA4_TEMP_BYTE UINT8_C(0) +#define BMA4_FIFO_LENGTH_MSB_BYTE UINT8_C(1) + +/**\name ERROR CODES */ +#define BMA4_OK UINT16_C(0) +#define BMA4_E_NULL_PTR UINT16_C(1) +#define BMA4_E_OUT_OF_RANGE UINT16_C(1 << 1) +#define BMA4_E_INVALID_SENSOR UINT16_C(1 << 2) +#define BMA4_E_CONFIG_STREAM_ERROR UINT16_C(1 << 3) +#define BMA4_E_SELF_TEST_FAIL UINT16_C(1 << 4) +#define BMA4_E_FOC_FAIL UINT16_C(1 << 5) +#define BMA4_E_FAIL UINT16_C(1 << 6) +#define BMA4_E_INT_LINE_INVALID UINT16_C(1 << 7) +#define BMA4_E_RD_WR_LENGTH_INVALID UINT16_C(1 << 8) +#define BMA4_E_AUX_CONFIG_FAIL UINT16_C(1 << 9) +#define BMA4_E_SC_FIFO_HEADER_ERR UINT16_C(1 << 10) +#define BMA4_E_SC_FIFO_CONFIG_ERR UINT16_C(1 << 11) + +/**\name UTILITY MACROS */ +#define BMA4_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMA4_SET_HIGH_BYTE UINT16_C(0xFF00) +#define BMA4_SET_LOW_NIBBLE UINT8_C(0x0F) + +/**\name FOC RELATED MACROS */ +#define BMA4_ACCEL_CONFIG_FOC UINT8_C(0xB7) + +/* Macros used for Self test */ +/* Self-test: Resulting minimum difference signal in mg for BMA42x */ +#define BMA42X_ST_ACC_X_AXIS_SIGNAL_DIFF UINT16_C(400) +#define BMA42X_ST_ACC_Y_AXIS_SIGNAL_DIFF UINT16_C(800) +#define BMA42X_ST_ACC_Z_AXIS_SIGNAL_DIFF UINT16_C(400) + +/* Self-test: Resulting minimum difference signal in mg for BMA45x */ +#define BMA45X_ST_ACC_X_AXIS_SIGNAL_DIFF UINT16_C(1800) +#define BMA45X_ST_ACC_Y_AXIS_SIGNAL_DIFF UINT16_C(1800) +#define BMA45X_ST_ACC_Z_AXIS_SIGNAL_DIFF UINT16_C(1800) + +/**\name BOOLEAN TYPES*/ +#ifndef TRUE +#define TRUE UINT8_C(0x01) +#endif + +#ifndef FALSE +#define FALSE UINT8_C(0x00) +#endif + +#ifndef NULL +#define NULL UINT8_C(0x00) +#endif + +/**\name ERROR STATUS POSITION AND MASK*/ +#define BMA4_FATAL_ERR_MSK UINT8_C(0x01) +#define BMA4_CMD_ERR_POS UINT8_C(1) +#define BMA4_CMD_ERR_MSK UINT8_C(0x02) +#define BMA4_ERR_CODE_POS UINT8_C(2) +#define BMA4_ERR_CODE_MSK UINT8_C(0x1C) +#define BMA4_FIFO_ERR_POS UINT8_C(6) +#define BMA4_FIFO_ERR_MSK UINT8_C(0x40) +#define BMA4_AUX_ERR_POS UINT8_C(7) +#define BMA4_AUX_ERR_MSK UINT8_C(0x80) + +/**\name Maximum number of bytes to be read from the sensor */ +#define BMA4_MAX_BUFFER_SIZE UINT8_C(81) + +/**\name NV_CONFIG POSITION AND MASK*/ +/* NV_CONF Description - Reg Addr --> (0x70), Bit --> 3 */ +#define BMA4_NV_ACCEL_OFFSET_POS UINT8_C(3) +#define BMA4_NV_ACCEL_OFFSET_MSK UINT8_C(0x08) + +/**\name MAG DATA XYZ POSITION AND MASK*/ +#define BMA4_DATA_MAG_X_LSB_POS UINT8_C(3) +#define BMA4_DATA_MAG_X_LSB_MSK UINT8_C(0xF8) +#define BMA4_DATA_MAG_Y_LSB_POS UINT8_C(3) +#define BMA4_DATA_MAG_Y_LSB_MSK UINT8_C(0xF8) +#define BMA4_DATA_MAG_Z_LSB_POS UINT8_C(1) +#define BMA4_DATA_MAG_Z_LSB_MSK UINT8_C(0xFE) +#define BMA4_DATA_MAG_R_LSB_POS UINT8_C(2) +#define BMA4_DATA_MAG_R_LSB_MSK UINT8_C(0xFC) + +/**\name ACCEL DATA READY POSITION AND MASK*/ +#define BMA4_STAT_DATA_RDY_ACCEL_POS UINT8_C(7) +#define BMA4_STAT_DATA_RDY_ACCEL_MSK UINT8_C(0x80) + +/**\name MAG DATA READY POSITION AND MASK*/ +#define BMA4_STAT_DATA_RDY_MAG_POS UINT8_C(5) +#define BMA4_STAT_DATA_RDY_MAG_MSK UINT8_C(0x20) + +/**\name ADVANCE POWER SAVE POSITION AND MASK*/ +#define BMA4_ADVANCE_POWER_SAVE_MSK UINT8_C(0x01) + +/**\name ACCELEROMETER ENABLE POSITION AND MASK*/ +#define BMA4_ACCEL_ENABLE_POS UINT8_C(2) +#define BMA4_ACCEL_ENABLE_MSK UINT8_C(0x04) + +/**\name MAGNETOMETER ENABLE POSITION AND MASK*/ +#define BMA4_MAG_ENABLE_MSK UINT8_C(0x01) + +/**\name ACCEL CONFIGURATION POSITION AND MASK*/ +#define BMA4_ACCEL_ODR_MSK UINT8_C(0x0F) +#define BMA4_ACCEL_BW_POS UINT8_C(4) +#define BMA4_ACCEL_BW_MSK UINT8_C(0x70) +#define BMA4_ACCEL_RANGE_MSK UINT8_C(0x03) +#define BMA4_ACCEL_PERFMODE_POS UINT8_C(7) +#define BMA4_ACCEL_PERFMODE_MSK UINT8_C(0x80) + +/**\name MAG CONFIGURATION POSITION AND MASK*/ +#define BMA4_MAG_CONFIG_OFFSET_POS UINT8_C(4) +#define BMA4_MAG_CONFIG_OFFSET_LEN UINT8_C(4) +#define BMA4_MAG_CONFIG_OFFSET_MSK UINT8_C(0xF0) +#define BMA4_MAG_CONFIG_OFFSET_REG (BMA4_AUX_CONFIG_ADDR) + +/**\name FIFO SELF WAKE UP POSITION AND MASK*/ +#define BMA4_FIFO_SELF_WAKE_UP_POS UINT8_C(1) +#define BMA4_FIFO_SELF_WAKE_UP_MSK UINT8_C(0x02) + +/**\name FIFO BYTE COUNTER POSITION AND MASK*/ +#define BMA4_FIFO_BYTE_COUNTER_MSB_MSK UINT8_C(0x3F) + +/**\name FIFO DATA POSITION AND MASK*/ +#define BMA4_FIFO_DATA_POS UINT8_C(0) +#define BMA4_FIFO_DATA_MSK UINT8_C(0xFF) + +/**\name FIFO FILTER FOR ACCEL POSITION AND MASK*/ +#define BMA4_FIFO_DOWN_ACCEL_POS UINT8_C(4) +#define BMA4_FIFO_DOWN_ACCEL_MSK UINT8_C(0x70) +#define BMA4_FIFO_FILTER_ACCEL_POS UINT8_C(7) +#define BMA4_FIFO_FILTER_ACCEL_MSK UINT8_C(0x80) + +/**\name FIFO HEADER DATA DEFINITIONS */ +#define FIFO_HEAD_A UINT8_C(0x84) +#define FIFO_HEAD_M UINT8_C(0x90) +#define FIFO_HEAD_M_A UINT8_C(0x94) +#define FIFO_HEAD_SENSOR_TIME UINT8_C(0x44) +#define FIFO_HEAD_INPUT_CONFIG UINT8_C(0x48) +#define FIFO_HEAD_SKIP_FRAME UINT8_C(0x40) +#define FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) +#define FIFO_HEAD_SAMPLE_DROP UINT8_C(0x50) + +/**\name FIFO HEADERLESS MODE DATA ENABLE DEFINITIONS */ +#define BMA4_FIFO_M_A_ENABLE UINT8_C(0x60) +#define BMA4_FIFO_A_ENABLE UINT8_C(0x40) +#define BMA4_FIFO_M_ENABLE UINT8_C(0x20) + +/**\name FIFO CONFIGURATION SELECTION */ +#define BMA4_FIFO_STOP_ON_FULL UINT8_C(0x01) +#define BMA4_FIFO_TIME UINT8_C(0x02) +#define BMA4_FIFO_TAG_INTR2 UINT8_C(0x04) +#define BMA4_FIFO_TAG_INTR1 UINT8_C(0x08) +#define BMA4_FIFO_HEADER UINT8_C(0x10) +#define BMA4_FIFO_MAG UINT8_C(0x20) +#define BMA4_FIFO_ACCEL UINT8_C(0x40) +#define BMA4_FIFO_ALL UINT8_C(0x7F) +#define BMA4_FIFO_CONFIG_0_MASK UINT8_C(0x03) +#define BMA4_FIFO_CONFIG_1_MASK UINT8_C(0xFC) + +/**\name FIFO FRAME COUNT DEFINITION */ +#define FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) +#define FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) +#define BMA4_FIFO_TAG_INTR_MASK UINT8_C(0xFC) + +/**\name FIFO DROPPED FRAME DEFINITION */ +#define AUX_FIFO_DROP UINT8_C(0x04) +#define ACCEL_AUX_FIFO_DROP UINT8_C(0x05) +#define ACCEL_FIFO_DROP UINT8_C(0x01) + +/**\name FIFO MAG DEFINITION*/ +#define BMA4_MA_FIFO_A_X_LSB UINT8_C(8) + +/**\name FIFO sensor time length definitions*/ +#define BMA4_SENSOR_TIME_LENGTH UINT8_C(3) + +/**\name FIFO LENGTH DEFINITION*/ +#define BMA4_FIFO_A_LENGTH UINT8_C(6) +#define BMA4_FIFO_M_LENGTH UINT8_C(8) +#define BMA4_FIFO_MA_LENGTH UINT8_C(14) + +/**\name MAG I2C ADDRESS SELECTION POSITION AND MASK*/ +#define BMA4_I2C_DEVICE_ADDR_POS UINT8_C(1) +#define BMA4_I2C_DEVICE_ADDR_MSK UINT8_C(0xFE) + +/**\name MAG CONFIGURATION FOR SECONDARY INTERFACE POSITION AND MASK*/ +#define BMA4_MAG_BURST_MSK UINT8_C(0x03) +#define BMA4_MAG_MANUAL_ENABLE_POS UINT8_C(7) +#define BMA4_MAG_MANUAL_ENABLE_MSK UINT8_C(0x80) +#define BMA4_READ_ADDR_MSK UINT8_C(0xFF) +#define BMA4_WRITE_ADDR_MSK UINT8_C(0xFF) +#define BMA4_WRITE_DATA_MSK UINT8_C(0xFF) + +/**\name OUTPUT TYPE ENABLE POSITION AND MASK*/ +#define BMA4_INT_EDGE_CTRL_MASK UINT8_C(0x01) +#define BMA4_INT_EDGE_CTRL_POS UINT8_C(0x00) +#define BMA4_INT_LEVEL_MASK UINT8_C(0x02) +#define BMA4_INT_LEVEL_POS UINT8_C(0x01) +#define BMA4_INT_OPEN_DRAIN_MASK UINT8_C(0x04) +#define BMA4_INT_OPEN_DRAIN_POS UINT8_C(0x02) +#define BMA4_INT_OUTPUT_EN_MASK UINT8_C(0x08) +#define BMA4_INT_OUTPUT_EN_POS UINT8_C(0x03) +#define BMA4_INT_INPUT_EN_MASK UINT8_C(0x10) +#define BMA4_INT_INPUT_EN_POS UINT8_C(0x04) + +/**\name IF CONFIG POSITION AND MASK*/ +#define BMA4_CONFIG_SPI3_MSK UINT8_C(0x01) +#define BMA4_IF_CONFIG_IF_MODE_POS UINT8_C(4) +#define BMA4_IF_CONFIG_IF_MODE_MSK UINT8_C(0x10) + +/**\name ACCEL SELF TEST POSITION AND MASK*/ +#define BMA4_ACCEL_SELFTEST_ENABLE_MSK UINT8_C(0x01) +#define BMA4_ACCEL_SELFTEST_SIGN_POS UINT8_C(2) +#define BMA4_ACCEL_SELFTEST_SIGN_MSK UINT8_C(0x04) +#define BMA4_SELFTEST_AMP_POS UINT8_C(3) +#define BMA4_SELFTEST_AMP_MSK UINT8_C(0x08) + +/**\name ACCEL ODR */ +#define BMA4_OUTPUT_DATA_RATE_0_78HZ UINT8_C(0x01) +#define BMA4_OUTPUT_DATA_RATE_1_56HZ UINT8_C(0x02) +#define BMA4_OUTPUT_DATA_RATE_3_12HZ UINT8_C(0x03) +#define BMA4_OUTPUT_DATA_RATE_6_25HZ UINT8_C(0x04) +#define BMA4_OUTPUT_DATA_RATE_12_5HZ UINT8_C(0x05) +#define BMA4_OUTPUT_DATA_RATE_25HZ UINT8_C(0x06) +#define BMA4_OUTPUT_DATA_RATE_50HZ UINT8_C(0x07) +#define BMA4_OUTPUT_DATA_RATE_100HZ UINT8_C(0x08) +#define BMA4_OUTPUT_DATA_RATE_200HZ UINT8_C(0x09) +#define BMA4_OUTPUT_DATA_RATE_400HZ UINT8_C(0x0A) +#define BMA4_OUTPUT_DATA_RATE_800HZ UINT8_C(0x0B) +#define BMA4_OUTPUT_DATA_RATE_1600HZ UINT8_C(0x0C) + +/**\name ACCEL BANDWIDTH PARAMETER */ +#define BMA4_ACCEL_OSR4_AVG1 UINT8_C(0) +#define BMA4_ACCEL_OSR2_AVG2 UINT8_C(1) +#define BMA4_ACCEL_NORMAL_AVG4 UINT8_C(2) +#define BMA4_ACCEL_CIC_AVG8 UINT8_C(3) +#define BMA4_ACCEL_RES_AVG16 UINT8_C(4) +#define BMA4_ACCEL_RES_AVG32 UINT8_C(5) +#define BMA4_ACCEL_RES_AVG64 UINT8_C(6) +#define BMA4_ACCEL_RES_AVG128 UINT8_C(7) + +/**\name ACCEL PERFMODE PARAMETER */ +#define BMA4_CIC_AVG_MODE UINT8_C(0) +#define BMA4_CONTINUOUS_MODE UINT8_C(1) + +/**\name MAG OFFSET */ +#define BMA4_MAG_OFFSET_MAX UINT8_C(0x00) + +/**\name ENABLE/DISABLE SELECTIONS */ +#define BMA4_X_AXIS UINT8_C(0) +#define BMA4_Y_AXIS UINT8_C(1) +#define BMA4_Z_AXIS UINT8_C(2) + +/**\name SELF TEST*/ +#define BMA4_SELFTEST_PASS UINT8_C(0) +#define BMA4_SELFTEST_FAIL UINT8_C(1) + +/**\name INTERRUPT MAPS */ +#define BMA4_INTR1_MAP UINT8_C(0) +#define BMA4_INTR2_MAP UINT8_C(1) + +/**\name INTERRUPT MASKS */ +#define BMA4_FIFO_FULL_INT UINT16_C(0x0100) +#define BMA4_FIFO_WM_INT UINT16_C(0x0200) +#define BMA4_DATA_RDY_INT UINT16_C(0x0400) +#define BMA4_MAG_DATA_RDY_INT UINT16_C(0x2000) +#define BMA4_ACCEL_DATA_RDY_INT UINT16_C(0x8000) + + +/**\name AKM POWER MODE SELECTION */ +#define AKM_POWER_DOWN_MODE UINT8_C(0) +#define AKM_SINGLE_MEAS_MODE UINT8_C(1) + +/**\name SECONDARY_MAG POWER MODE SELECTION */ +#define BMA4_MAG_FORCE_MODE UINT8_C(0) +#define BMA4_MAG_SUSPEND_MODE UINT8_C(1) + +/**\name MAG POWER MODE SELECTION */ +#define FORCE_MODE UINT8_C(0) +#define SUSPEND_MODE UINT8_C(1) + +/**\name ACCEL POWER MODE */ +#define ACCEL_MODE_NORMAL UINT8_C(0x11) + +/**\name MAG POWER MODE */ +#define MAG_MODE_SUSPEND UINT8_C(0x18) + +/**\name ENABLE/DISABLE BIT VALUES */ +#define BMA4_ENABLE UINT8_C(0x01) +#define BMA4_DISABLE UINT8_C(0x00) + +/**\name DEFINITION USED FOR DIFFERENT WRITE */ +#define BMA4_MANUAL_DISABLE UINT8_C(0x00) +#define BMA4_MANUAL_ENABLE UINT8_C(0x01) +#define BMA4_ENABLE_MAG_IF_MODE UINT8_C(0x01) +#define BMA4_MAG_DATA_READ_REG UINT8_C(0x0A) +#define BMA4_BMM_POWER_MODE_REG UINT8_C(0x06) +#define BMA4_SEC_IF_NULL UINT8_C(0) +#define BMA4_SEC_IF_BMM150 UINT8_C(1) +#define BMA4_SEC_IF_AKM09916 UINT8_C(2) +#define BMA4_ENABLE_AUX_IF_MODE UINT8_C(0x01) + +/**\name SENSOR RESOLUTION */ +#define BMA4_12_BIT_RESOLUTION UINT8_C(12) +#define BMA4_14_BIT_RESOLUTION UINT8_C(14) +#define BMA4_16_BIT_RESOLUTION UINT8_C(16) + +/**\name MULTIPLIER */ +/*! for handling micro-g values */ +#define BMA4XY_MULTIPLIER UINT32_C(1000000) +/*! for handling float temperature values */ +#define BMA4_SCALE_TEMP INT32_C(1000) +/* BMA4_FAHREN_SCALED = 1.8 * 1000 */ +#define BMA4_FAHREN_SCALED INT32_C(1800) +/* BMA4_KELVIN_SCALED = 273.15 * 1000 */ +#define BMA4_KELVIN_SCALED INT32_C(273150) + + +/**\name MAP BURST READ LENGTHS */ +#define BMA4_AUX_READ_LEN_0 0 +#define BMA4_AUX_READ_LEN_1 1 +#define BMA4_AUX_READ_LEN_2 2 +#define BMA4_AUX_READ_LEN_3 3 + +#ifndef ABS +#define ABS(a) ((a) > 0 ? (a) : -(a)) /*!< Absolute value */ +#endif + +/**\name BIT SLICE GET AND SET FUNCTIONS */ +#define BMA4_GET_BITSLICE(regvar, bitname)\ + ((regvar & bitname##_MSK) >> bitname##_POS) +#define BMA4_SET_BITSLICE(regvar, bitname, val)\ + ((regvar & ~bitname##_MSK) | \ + ((val<> 8) + +#define BMA4_SET_BIT_VAL_0(reg_data, bitname) (reg_data & ~(bitname##_MSK)) + +#define BMA4_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMA4_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +/**\name TYPEDEF DEFINITIONS */ +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read and write functions of the user + */ +typedef uint16_t (*bma4_com_fptr_t)(uint8_t dev_addr, uint8_t reg_addr, uint8_t *read_data, uint16_t len); + +/*! delay function pointer */ +typedef void (*bma4_delay_fptr_t)(uint32_t); + +/******************************************************************************/ +/*! @name Enum Declarations */ +/******************************************************************************/ + +/*! @name Enum to define BMA4 variants */ +enum bma4_variant { + BMA42X_VARIANT = 1, + BMA45X_VARIANT +}; + +/**\name STRUCTURE DEFINITIONS*/ + +/*! + * @brief + * This structure holds asic info. for feature configuration. + */ +struct bma4_asic_data { + /* Feature config start addr (0-3 bits)*/ + uint8_t asic_lsb; + /* Feature config start addr (4-11 bits)*/ + uint8_t asic_msb; +}; + +/*! + * @brief Auxiliary configuration structure for user settings + */ +struct bma4_aux_config { + /*! Device address of auxiliary sensor */ + uint8_t aux_dev_addr; + /*! To enable manual or auto mode */ + uint8_t manual_enable; + /*! No of bytes to be read at a time */ + uint8_t burst_read_length; + /*! Variable to set the auxiliary interface */ + uint8_t if_mode; +}; + +/*! + * @brief + * This structure holds all relevant information about BMA4 + */ +struct bma4_dev { + /*! Chip id of BMA4 */ + uint8_t chip_id; + /*! Chip id of auxiliary sensor */ + uint8_t aux_chip_id; + /*! Device address of BMA4 */ + uint8_t dev_addr; + /*! Interface detail */ + uint8_t interface; + /*! Auxiliary sensor information */ + uint8_t aux_sensor; + /*! Decide SPI or I2C read mechanism */ + uint8_t dummy_byte; + /*! Resolution for FOC */ + uint8_t resolution; + /*! Define the BMA4 variant BMA42X or BMA45X */ + enum bma4_variant variant; +/* ! Used to check mag manual/auto mode status + int8_t mag_manual_enable;*/ + /*! FIFO related configurations */ + struct bma4_fifo_frame *fifo; + /*! Config stream data buffer address will be assigned*/ + const uint8_t *config_file_ptr; + /*! Max read/write length (maximum supported length is 32). + To be set by the user */ + uint8_t read_write_len; + /*! Feature len */ + uint8_t feature_len; + /*! Contains asic information */ + struct bma4_asic_data asic_data; + /*! Contains aux configuration settings */ + struct bma4_aux_config aux_config; + /*! Bus read function pointer */ + bma4_com_fptr_t bus_read; + /*! Bus write function pointer */ + bma4_com_fptr_t bus_write; + /*! delay(in ms) function pointer */ + bma4_delay_fptr_t delay; +}; + +/*! + * @brief This structure holds the information for usage of + * FIFO by the user. + */ +struct bma4_fifo_frame { + /*! Data buffer of user defined length is to be mapped here */ + uint8_t *data; + /*! Number of bytes of FIFO to be read as specified by the user */ + uint16_t length; + /*! Enabling of the FIFO header to stream in header mode */ + uint8_t fifo_header_enable; + /*! Streaming of the Accelerometer, Auxiliary + * sensor data or both in FIFO */ + uint8_t fifo_data_enable; + /*! Will be equal to length when no more frames are there to parse */ + uint16_t accel_byte_start_idx; + /*! Will be equal to length when no more frames are there to parse */ + uint16_t mag_byte_start_idx; + /*! Will be equal to length when no more frames are there to parse */ + uint16_t sc_frame_byte_start_idx; + /*! Value of FIFO sensor time time */ + uint32_t sensor_time; + /*! Value of Skipped frame counts */ + uint8_t skipped_frame_count; + /*! Value of accel dropped frame count */ + uint8_t accel_dropped_frame_count; + /*! Value of mag dropped frame count */ + uint8_t mag_dropped_frame_count; +}; + +/*! + * @brief Error Status structure + */ +struct bma4_err_reg { + /*! Indicates fatal error */ + uint8_t fatal_err; + /*! Indicates command error */ + uint8_t cmd_err; + /*! Indicates error code */ + uint8_t err_code; + /*! Indicates fifo error */ + uint8_t fifo_err; + /*! Indicates mag error */ + uint8_t aux_err; +}; + +/*! + * @brief Asic Status structure + */ +struct bma4_asic_status { + /*! Asic is in sleep/halt state */ + uint8_t sleep; + /*! Dedicated interrupt is set again before previous interrupt + was acknowledged */ + uint8_t irq_ovrn; + /*! Watchcell event detected (asic stopped) */ + uint8_t wc_event; + /*! Stream transfer has started and transactions are ongoing */ + uint8_t stream_transfer_active; +}; + +/*! + * @brief Interrupt Pin Configuration structure + */ +struct bma4_int_pin_config { + /*! Trigger condition of interrupt pin */ + uint8_t edge_ctrl; + /*! Level of interrupt pin */ + uint8_t lvl; + /*! Behaviour of interrupt pin to open drain */ + uint8_t od; + /*! Output enable for interrupt pin */ + uint8_t output_en; + /*! Input enable for interrupt pin */ + uint8_t input_en; +}; + +/*! +* @brief Accelerometer configuration structure */ +struct bma4_accel_config { + /*! Output data rate in Hz */ + uint8_t odr; + /*! Bandwidth parameter, determines filter configuration */ + uint8_t bandwidth; + /*! Filter performance mode */ + uint8_t perf_mode; + /*! G-range */ + uint8_t range; +}; + +/*! + * @brief Auxiliary magnetometer configuration structure + */ +struct bma4_aux_mag_config { + /*! Poll rate for the sensor attached to the Magnetometer interface */ + uint8_t odr; + /*! Trigger-readout offset in units of 2.5 ms. + If set to zero, the offset is maximum, i.e. after readout a trigger + is issued immediately */ + uint8_t offset; +}; + +/*! + * @brief ASIC Config structure + */ +struct bma4_asic_config { + /*! Enable/Disable ASIC Wake Up */ + uint8_t asic_en; + /*! Configure stream_transfer/FIFO mode */ + uint8_t fifo_mode_en; + /*! Mapping of instance RAM1 */ + uint8_t mem_conf_ram1; + /*! Mapping of instance RAM2 */ + uint8_t mem_conf_ram2; + /*! Mapping of instance RAM3 */ + uint8_t mem_conf_ram3; +}; +/*! + * @brief bmm150 or akm09916 + * magnetometer values structure + */ +struct bma4_mag { + /*! BMM150 and AKM09916 X raw data */ + int32_t x; + /*! BMM150 and AKM09916 Y raw data */ + int32_t y; + /*! BMM150 and AKM09916 Z raw data */ + int32_t z; +}; + +/*! + * @brief bmm150 xyz data structure + */ +struct bma4_mag_xyzr { + /*! BMM150 X raw data */ + int16_t x; + /*! BMM150 Y raw data */ + int16_t y; + /*! BMM150 Z raw data */ + int16_t z; + /*! BMM150 R raw data */ + uint16_t r; +}; + +/*! + * @brief Accel xyz data structure + */ +struct bma4_accel { + /*! Accel X data */ + int16_t x; + /*! Accel Y data */ + int16_t y; + /*! Accel Z data */ + int16_t z; +}; + +/*! + * @brief FIFO mag data structure + */ +struct bma4_mag_fifo_data { + /*! The value of mag x LSB data */ + uint8_t mag_x_lsb; + /*! The value of mag x MSB data */ + uint8_t mag_x_msb; + /*! The value of mag y LSB data */ + uint8_t mag_y_lsb; + /*! The value of mag y MSB data */ + uint8_t mag_y_msb; + /*! The value of mag z LSB data */ + uint8_t mag_z_lsb; + /*! The value of mag z MSB data */ + uint8_t mag_z_msb; + /*! The value of mag r for BMM150 Y2 for YAMAHA LSB data */ + uint8_t mag_r_y2_lsb; + /*! The value of mag r for BMM150 Y2 for YAMAHA MSB data */ + uint8_t mag_r_y2_msb; +}; + +#endif +/* End of __BMA4_H__ */ \ No newline at end of file diff --git a/src/i2c_bus.cpp b/src/i2c_bus.cpp new file mode 100644 index 0000000..8487eb2 --- /dev/null +++ b/src/i2c_bus.cpp @@ -0,0 +1,86 @@ +#include "i2c_bus.h" +#include "Wire.h" +#include + +void I2CBus::scan(void) +{ + uint8_t err, addr; + int nDevices = 0; + for (addr = 1; addr < 127; addr++) { + _port->beginTransmission(addr); + err = _port->endTransmission(); + if (err == 0) { + Serial.print("I2C device found at address 0x"); + if (addr < 16) + Serial.print("0"); + Serial.print(addr, HEX); + Serial.println(" !"); + nDevices++; + } else if (err == 4) { + Serial.print("Unknow error at address 0x"); + if (addr < 16) + Serial.print("0"); + Serial.println(addr, HEX); + } + } + if (nDevices == 0) + Serial.println("No I2C devices found\n"); + else + Serial.println("done\n"); +} + + +uint16_t I2CBus::readBytes(uint8_t addr, uint8_t *data, uint16_t len, uint16_t delay_ms) +{ + uint16_t ret = 0; + uint8_t cnt = _port->requestFrom(addr, (uint8_t)len, (uint8_t)1); + if (!cnt) { + ret = 1 << 13; + } + uint16_t index = 0; + while (_port->available()) { + if (index > len)return 1 << 14; + if (delay_ms)delay(delay_ms); + data[index++] = _port->read(); + } + return ret; +} + + +uint16_t I2CBus::readBytes(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len) +{ + uint16_t ret = 0; + _port->beginTransmission(addr); + _port->write(reg); + _port->endTransmission(); + uint8_t cnt = _port->requestFrom(addr, (uint8_t)len, (uint8_t)1); + if (!cnt) { + ret = 1 << 13; + } + uint16_t index = 0; + while (_port->available()) { + if (index > len)return 1 << 14; + data[index++] = _port->read(); + } + return ret; +} + +uint16_t I2CBus::writeBytes(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len) +{ + uint16_t ret = 0; + _port->beginTransmission(addr); + _port->write(reg); + for (uint16_t i = 0; i < len; i++) { + _port->write(data[i]); + } + ret = _port->endTransmission(); + return ret ? 1 << 12 : ret; +} + +bool I2CBus::deviceProbe(uint8_t addr) +{ + uint16_t ret = 0; + _port->beginTransmission(addr); + ret = _port->endTransmission(); + return (ret == 0); +} \ No newline at end of file diff --git a/src/i2c_bus.h b/src/i2c_bus.h new file mode 100644 index 0000000..78c589f --- /dev/null +++ b/src/i2c_bus.h @@ -0,0 +1,18 @@ +#include + +class I2CBus +{ +public: + I2CBus(TwoWire &port = Wire, int sda = 21, int scl = 22) + { + _port = &port; + _port->begin(sda, scl); + }; + void scan(); + uint16_t readBytes(uint8_t addr, uint8_t *data, uint16_t len, uint16_t delay_ms = 0); + uint16_t readBytes(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len); + uint16_t writeBytes(uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len); + bool deviceProbe(uint8_t addr); +private: + TwoWire *_port; +}; \ No newline at end of file