541 lines
18 KiB
C
541 lines
18 KiB
C
/**
|
|
* Copyright (c) 2025 Bosch Sensortec GmbH. All rights reserved.
|
|
*
|
|
* BSD-3-Clause
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
*
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
*
|
|
* 3. Neither the name of the copyright holder nor the names of its
|
|
* contributors may be used to endorse or promote products derived from
|
|
* this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
|
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
|
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* @file bmm350_oor.c
|
|
* @date 2025-10-30
|
|
* @version v1.10.0
|
|
*
|
|
*/
|
|
|
|
#include "bmm350.h"
|
|
#include "bmm350_oor.h"
|
|
|
|
#ifndef BMM350_USE_FIXED_POINT
|
|
#include "math.h"
|
|
|
|
#else
|
|
|
|
#ifndef __KERNEL__
|
|
#include "stdlib.h"
|
|
#endif
|
|
|
|
#endif
|
|
|
|
/********************** Static function declarations ************************/
|
|
|
|
/*!
|
|
* @brief This internal API is used to execute delay operation for the reset functions.
|
|
*/
|
|
static int8_t execute_delay_in_steps(uint32_t delay, uint32_t delay_step, const struct bmm350_dev *dev);
|
|
|
|
#ifdef BMM350_OOR_HALF_SELF_TEST
|
|
|
|
/*!
|
|
* @brief This internal API is used to trigger half self-test
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
* @param[in,out] dev : Device structure of the BMM350
|
|
*/
|
|
static int8_t trigger_half_selftest(const struct bmm350_oor_reset_delay *rdelay,
|
|
struct bmm350_oor_params *oor,
|
|
struct bmm350_dev *dev)
|
|
{
|
|
int8_t rslt = BMM350_OK;
|
|
|
|
uint8_t pmu_cmd = BMM350_PMU_CMD_BR;
|
|
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
|
|
/* Trigger a self-test on every alternate measurement if needed */
|
|
if (oor->enable_selftest)
|
|
{
|
|
oor->st_counter++;
|
|
|
|
switch (oor->st_counter)
|
|
{
|
|
case 1:
|
|
oor->st_cmd = BMM350_SELF_TEST_POS_X;
|
|
break;
|
|
case 2:
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
break;
|
|
case 3:
|
|
oor->st_cmd = BMM350_SELF_TEST_POS_Y;
|
|
break;
|
|
case 4:
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
break;
|
|
|
|
case 5:
|
|
if (dev->enable_auto_br == BMM350_DISABLE)
|
|
{
|
|
/* Trigger the Bit Reset */
|
|
pmu_cmd = BMM350_PMU_CMD_BR;
|
|
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
/* Bit Reset delay*/
|
|
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
oor->st_counter = 0;
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
}
|
|
|
|
break;
|
|
|
|
default:
|
|
oor->st_counter = 0;
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
break;
|
|
}
|
|
|
|
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
|
|
}
|
|
else
|
|
{
|
|
if (oor->last_st_cmd != BMM350_SELF_TEST_DISABLE)
|
|
{
|
|
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
|
|
oor->st_counter = 0;
|
|
}
|
|
}
|
|
|
|
return rslt;
|
|
}
|
|
|
|
/*!
|
|
* @brief This internal API is used to validate half self-test
|
|
* @param[out] data : Sensor data
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
*/
|
|
static void validate_half_selftest(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
|
|
{
|
|
switch (oor->last_st_cmd)
|
|
{
|
|
case BMM350_SELF_TEST_DISABLE:
|
|
oor->mag_x_st_dis = data->x;
|
|
oor->mag_y_st_dis = data->y;
|
|
break;
|
|
|
|
case BMM350_SELF_TEST_POS_X:
|
|
oor->mag_x_st_en = data->x;
|
|
oor->x_failed = (oor->mag_x_st_en - oor->mag_x_st_dis) < BMM350_HALF_ST_THRESHOLD ? true : false;
|
|
break;
|
|
|
|
case BMM350_SELF_TEST_POS_Y:
|
|
oor->mag_y_st_en = data->y;
|
|
oor->y_failed = (oor->mag_y_st_en - oor->mag_y_st_dis) < BMM350_HALF_ST_THRESHOLD ? true : false;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
#else
|
|
|
|
/*!
|
|
* @brief This internal API is used to trigger self-test
|
|
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
* @param[in,out] dev : Device structure of the BMM350
|
|
*
|
|
* @return Result of API execution status
|
|
* @retval = 0 -> Success
|
|
* @retval < 0 -> Error
|
|
*/
|
|
static int8_t trigger_selftest(const struct bmm350_oor_reset_delay *rdelay,
|
|
struct bmm350_oor_params *oor,
|
|
struct bmm350_dev *dev)
|
|
{
|
|
int8_t rslt = BMM350_OK;
|
|
uint8_t pmu_cmd = BMM350_PMU_CMD_BR;
|
|
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
|
|
/* Trigger a self-test on every alternate measurement if needed */
|
|
if (oor->enable_selftest)
|
|
{
|
|
oor->st_counter++;
|
|
|
|
switch (oor->st_counter)
|
|
{
|
|
case 1:
|
|
oor->st_cmd = BMM350_SELF_TEST_POS_X;
|
|
break;
|
|
case 2:
|
|
oor->st_cmd = BMM350_SELF_TEST_NEG_X;
|
|
break;
|
|
case 3:
|
|
oor->st_cmd = BMM350_SELF_TEST_POS_Y;
|
|
break;
|
|
case 4:
|
|
oor->st_cmd = BMM350_SELF_TEST_NEG_Y;
|
|
break;
|
|
|
|
case 5:
|
|
if (dev->enable_auto_br == BMM350_DISABLE)
|
|
{
|
|
/* Trigger the Bit Reset */
|
|
pmu_cmd = BMM350_PMU_CMD_BR;
|
|
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
/* Bit Reset delay*/
|
|
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
oor->st_counter = 0;
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
}
|
|
|
|
break;
|
|
default:
|
|
oor->st_counter = 0;
|
|
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
|
|
break;
|
|
}
|
|
|
|
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
|
|
}
|
|
else
|
|
{
|
|
if (oor->last_st_cmd != BMM350_SELF_TEST_DISABLE)
|
|
{
|
|
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
|
|
oor->st_counter = 0;
|
|
}
|
|
}
|
|
|
|
return rslt;
|
|
}
|
|
|
|
/*!
|
|
* @brief This internal API is used to validate self-test during the self-test window
|
|
* @param[in] data : Sensor data
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
*/
|
|
static void validate_selftest_window(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
|
|
{
|
|
switch (oor->last_st_cmd)
|
|
{
|
|
case BMM350_SELF_TEST_POS_X:
|
|
oor->mag_x_st_en = data->x;
|
|
break;
|
|
|
|
case BMM350_SELF_TEST_NEG_X:
|
|
oor->mag_x_st_dis = data->x;
|
|
oor->x_failed = (oor->mag_x_st_en - oor->mag_x_st_dis) < BMM350_FULL_ST_THRESHOLD ? true : false;
|
|
break;
|
|
|
|
case BMM350_SELF_TEST_POS_Y:
|
|
oor->mag_y_st_en = data->y;
|
|
break;
|
|
|
|
case BMM350_SELF_TEST_NEG_Y:
|
|
oor->mag_y_st_dis = data->y;
|
|
oor->y_failed = (oor->mag_y_st_en - oor->mag_y_st_dis) < BMM350_FULL_ST_THRESHOLD ? true : false;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
/********************** Global function definitions ************************/
|
|
|
|
/*!
|
|
* @brief This internal API is used to validate out of range.
|
|
* @param[in] data : Sensor data
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
*/
|
|
void bmm350_oor_validate_out_of_range(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
|
|
{
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
|
|
/* Threshold to start out of range detection */
|
|
fixed_t threshold = BMM350_OUT_OF_RANGE_THRESHOLD;
|
|
|
|
/* Threshold to start self-tests */
|
|
fixed_t st_threshold = BMM350_SELF_TEST_THRESHOLD;
|
|
|
|
/* Variable to compute the magnitude square */
|
|
fixed_t magnitude_square = 0;
|
|
#else
|
|
|
|
/* Threshold to start out of range detection */
|
|
float threshold = BMM350_OUT_OF_RANGE_THRESHOLD;
|
|
|
|
/* Threshold to start self-tests */
|
|
float st_threshold = BMM350_SELF_TEST_THRESHOLD;
|
|
#endif
|
|
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
|
|
/* Compute the Field Strength */
|
|
magnitude_square =
|
|
(uint32_t)((fixed_mul_A48_16(data->x,
|
|
data->x) +
|
|
fixed_mul_A48_16(data->y, data->y) + fixed_mul_A48_16(data->z, data->z)) >> F16_FRAC_BITS);
|
|
oor->field_strength = (uint32_t)bmm350_fixed_point_sqrt(magnitude_square);
|
|
#else
|
|
|
|
/* Compute the Field Strength */
|
|
oor->field_strength = sqrtf((data->x * data->x) + (data->y * data->y) + (data->z * data->z));
|
|
#endif
|
|
|
|
/* If either self-test failed, alert that the sensor is out of range and continue self-tests */
|
|
if (oor->x_failed || oor->y_failed)
|
|
{
|
|
oor->out_of_range = true;
|
|
oor->enable_selftest = true;
|
|
}
|
|
else
|
|
{
|
|
|
|
/* Check for the self-test threshold and perform self-tests to catch if the sensor is out of range */
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
if ((abs(data->x) >= st_threshold) || (abs(data->y) >= st_threshold) || (abs(data->z) >= st_threshold) ||
|
|
(oor->field_strength >= (uint32_t)(st_threshold >> F16_FRAC_BITS)))
|
|
#else
|
|
if ((fabsf(data->x) >= st_threshold) || (fabsf(data->y) >= st_threshold) || (fabsf(data->z) >= st_threshold) ||
|
|
(oor->field_strength >= st_threshold))
|
|
#endif
|
|
{
|
|
oor->enable_selftest = true;
|
|
}
|
|
else if (oor->st_counter == 0) /* If a self-test procedure has started, wait for it to complete */
|
|
{
|
|
oor->enable_selftest = false;
|
|
}
|
|
|
|
/* If out of range was previously detected, reduce the threshold to get back in range,
|
|
* effectively preventing hysteresis. Selecting 400uT */
|
|
if (oor->out_of_range)
|
|
{
|
|
threshold = BMM350_IN_RANGE_THRESHOLD;
|
|
}
|
|
|
|
/* Check if X or Y or Z > the threshold or the magnitude of all 3 is greater */
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
if ((abs(data->x) >= threshold) || (abs(data->y) >= threshold) || (abs(data->z) >= threshold) ||
|
|
(oor->field_strength >= (uint32_t)(threshold >> F16_FRAC_BITS)))
|
|
#else
|
|
if ((fabsf(data->x) >= threshold) || (fabsf(data->y) >= threshold) || (fabsf(data->z) >= threshold) ||
|
|
(oor->field_strength >= threshold))
|
|
#endif
|
|
{
|
|
oor->out_of_range = true;
|
|
}
|
|
else if (oor->st_counter == 0) /* If a self-test procedure has started, wait for it to complete */
|
|
{
|
|
if (oor->out_of_range)
|
|
{
|
|
oor->trigger_reset = true;
|
|
}
|
|
|
|
oor->out_of_range = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* @brief This API is used to perform magnetic reset sequence.
|
|
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
* @param[in,out] dev : Device structure of the BMM350
|
|
*
|
|
* @return Result of API execution status
|
|
* @retval = 0 -> Success
|
|
* @retval < 0 -> Error
|
|
*/
|
|
int8_t bmm350_oor_perform_reset_sequence(const struct bmm350_oor_reset_delay *rdelay,
|
|
struct bmm350_oor_params *oor,
|
|
struct bmm350_dev *dev)
|
|
{
|
|
int8_t rslt = 0;
|
|
uint8_t pmu_cmd = 0;
|
|
|
|
oor->reset_counter++;
|
|
|
|
switch (oor->reset_counter)
|
|
{
|
|
case 1: /* Trigger the Bit reset fast */
|
|
pmu_cmd = BMM350_PMU_CMD_BR_FAST;
|
|
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
/* Bit Reset Window synchronization delay*/
|
|
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
|
|
}
|
|
|
|
break;
|
|
|
|
case 2: /* Trigger Flux Guide reset */
|
|
pmu_cmd = BMM350_PMU_CMD_FGR;
|
|
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
/* Flux Guide Reset Window synchronization delay*/
|
|
(void)execute_delay_in_steps(rdelay->fgr_delay, rdelay->delay_step, dev);
|
|
}
|
|
|
|
break;
|
|
|
|
case 3: /* Flux Guide dummy */
|
|
break;
|
|
|
|
default: /* Default acts like the Flux guide reset dummy */
|
|
oor->reset_counter = 0;
|
|
oor->trigger_reset = false;
|
|
break;
|
|
}
|
|
|
|
return rslt;
|
|
}
|
|
|
|
/*!
|
|
* @brief This API is used to read out of range in during self-test.
|
|
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
|
|
* @param[out] data : Sensor data
|
|
* @param[out] oor : Structure that stores the state of the out of range detector
|
|
* @param[in,out] dev : Device structure of the BMM350
|
|
*
|
|
* @return Result of API execution status
|
|
* @retval = 0 -> Success
|
|
* @retval < 0 -> Error
|
|
*/
|
|
int8_t bmm350_oor_read(const struct bmm350_oor_reset_delay *rdelay,
|
|
struct bmm350_mag_temp_data *data,
|
|
struct bmm350_oor_params *oor,
|
|
struct bmm350_dev *dev)
|
|
{
|
|
int8_t rslt = 0;
|
|
uint8_t pmu_cmd = BMM350_PMU_CMD_SUS;
|
|
|
|
#ifdef BMM350_OOR_HALF_SELF_TEST
|
|
rslt = trigger_half_selftest(rdelay, oor, dev);
|
|
#else
|
|
rslt = trigger_selftest(rdelay, oor, dev);
|
|
#endif
|
|
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
pmu_cmd = BMM350_PMU_CMD_FM_FAST;
|
|
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
|
|
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
rslt = bmm350_get_compensated_mag_xyz_temp_data_fixed(data, dev);
|
|
#else
|
|
rslt = bmm350_get_compensated_mag_xyz_temp_data(data, dev);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#ifdef BMM350_OOR_HALF_SELF_TEST
|
|
validate_half_selftest(data, oor);
|
|
#else
|
|
validate_selftest_window(data, oor);
|
|
#endif
|
|
|
|
bmm350_oor_validate_out_of_range(data, oor);
|
|
|
|
oor->last_st_cmd = oor->st_cmd;
|
|
|
|
return rslt;
|
|
}
|
|
|
|
/*!
|
|
* @brief This API is used to computing the reset delay settings for Magnetic Reset Sequence.
|
|
* @param[in] odr_config : ODR Configuration
|
|
* @param[out] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
|
|
*/
|
|
void bmm350_oor_compute_delay_setting(uint8_t odr_config, struct bmm350_oor_reset_delay *rdelay)
|
|
{
|
|
uint16_t min_dealy_unit = 2500; /* corresponds to 400Hz -> (1/400) * (10^6) us */
|
|
|
|
#ifdef BMM350_USE_FIXED_POINT
|
|
uint32_t power_scale = ((uint32_t)1 << (odr_config - BMM350_ODR_400HZ));
|
|
rdelay->delay_step = (uint32_t)(min_dealy_unit * power_scale);
|
|
#else
|
|
rdelay->delay_step = (uint32_t)(min_dealy_unit * (pow(2.0, (double)(odr_config - BMM350_ODR_400HZ))));
|
|
#endif
|
|
|
|
rdelay->br_delay = ((rdelay->delay_step * 3) - 1000);
|
|
rdelay->fgr_delay = ((rdelay->delay_step * 4) - 2000);
|
|
}
|
|
|
|
/********************** Static function definitions ************************/
|
|
|
|
/*!
|
|
* @brief This internal API is used to execute delay operation for the reset functions.
|
|
* @param[in] delay : ODR Window Delay
|
|
* @param[in] delay_step : Delay Time Resolution
|
|
* @param[in,out] dev : Device structure of the BMM350
|
|
*
|
|
* @return Result of API execution status
|
|
* @retval = 0 -> Success
|
|
* @retval < 0 -> Error
|
|
*/
|
|
static int8_t execute_delay_in_steps(uint32_t delay, uint32_t delay_step, const struct bmm350_dev *dev)
|
|
{
|
|
int8_t rslt = BMM350_OK;
|
|
|
|
if ((delay > 0) && (delay_step > 0) && (delay >= delay_step))
|
|
{
|
|
while (delay >= delay_step)
|
|
{
|
|
rslt = bmm350_delay_us(delay_step, dev);
|
|
if (rslt == BMM350_OK)
|
|
{
|
|
delay = delay - delay_step;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
rslt = BMM350_E_INVALID_INPUT;
|
|
}
|
|
|
|
return rslt;
|
|
}
|