sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

sensors: mx56xx: Fix calculation of second order compensation

Browse Source 
The temperature won't be compensated correctly without this patch.

Signed-off-by: Takumi Ando <t-ando@advaly.co.jp>
This commit is contained in:
Takumi Ando 2023-12-20 11:11:55 +09:00 committed by Xiang Xiao
parent d9455ab420
commit 19ac909eea
1 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
drivers/sensors/ms56xx_uorb.c
View File

@ -117,7 +117,7 @@ static int32_t ms56xx_compensate_temp(FAR struct ms56xx_dev_s *priv,
uint32_t temp, int32_t *deltat);
static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
uint32_t press, uint32_t dt,
int32_t temp);
int32_t *temp);
static unsigned long ms56xx_curtime(void);
@ -351,7 +351,7 @@ static inline void baro_measure_read(FAR struct ms56xx_dev_s *priv,
/* Compensate the temp/press with calibration data */
temp = ms56xx_compensate_temp(priv, temp, &deltat);
press = ms56xx_compensate_press(priv, press, deltat, temp);
press = ms56xx_compensate_press(priv, press, deltat, &temp);
baro->timestamp = ms56xx_curtime();
baro->pressure = press / 100.0f;
@ -527,7 +527,7 @@ static int32_t ms56xx_compensate_temp(FAR struct ms56xx_dev_s *priv,
static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
uint32_t press, uint32_t dt,
int32_t temp)
int32_t *temp)
{
struct ms56xx_calib_s *c = &priv->calib;
int64_t off = 0;
@ -535,6 +535,7 @@ static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
#if defined(CONFIG_MS56XX_SECOND_ORDER_COMPENSATE)
int64_t off2 = 0;
int64_t sens2 = 0;
int64_t t2 = 0;
uint64_t delta;
#endif
@ -544,20 +545,21 @@ static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
off = ((int64_t) c->c2 << 17) + ((int64_t) (c->c4 * dt) >> 6);
sens = ((int64_t) c->c1 << 16) + ((int64_t) (c->c3 * dt) >> 7);
#if defined(CONFIG_MS56XX_SECOND_ORDER_COMPENSATE)
if (temp < 2000)
if (*temp < 2000)
{
/* Low temperature */
delta = temp - 2000;
t2 = ((dt * dt) >> 31);
delta = *temp - 2000;
delta *= delta;
off2 = (61 * delta) >> 4;
sens2 = 2 * delta;
if (temp < -1500)
if (*temp < -1500)
{
/* Very low temperature */
delta = temp + 1500;
delta = *temp + 1500;
delta *= delta;
off2 += 15 * delta;
sens2 += 8 * delta;
@ -570,20 +572,21 @@ static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
off = ((int64_t) c->c2 << 16) + ((int64_t) (c->c4 * dt) >> 7);
sens = ((int64_t) c->c1 << 15) + ((int64_t) (c->c3 * dt) >> 8);
#if defined(CONFIG_MS56XX_SECOND_ORDER_COMPENSATE)
if (temp < 2000)
if (*temp < 2000)
{
/* Low temperature */
delta = temp - 2000;
t2 = ((dt * dt) >> 31);
delta = *temp - 2000;
delta *= delta;
off2 = (5 * delta) >> 1;
sens2 = (5 * delta) >> 2;
if (temp < -1500)
if (*temp < -1500)
{
/* Very low temperature */
delta = temp + 1500;
delta = *temp + 1500;
delta *= delta;
off2 += 7 * delta;
sens2 += (11 * delta) >> 1;
@ -594,7 +597,7 @@ static uint32_t ms56xx_compensate_press(FAR struct ms56xx_dev_s *priv,
}
#if defined(CONFIG_MS56XX_SECOND_ORDER_COMPENSATE)
temp -= ((dt * dt) >> 31);
*temp -= t2;
off -= off2;
sens -= sens2;
#endif