Commit 2b43b4a4 authored by Jussi Kivilinna's avatar Jussi Kivilinna Committed by Pekka Niemimaa
Browse files

sensors/lis2dh: fix scaling of raw axis values



Change-Id: I7018744d2d7eb17882fb1a1f9b1e4b1c255e2300
Signed-off-by: default avatarJussi Kivilinna <jussi.kivilinna@haltian.com>
parent 0d95f9c3
......@@ -99,6 +99,7 @@ struct lis2dh_dev_s
FAR struct lis2dh_config_s *config; /* Platform specific configuration */
struct lis2dh_setup *setup; /* User defined device operation mode setup */
struct lis2dh_vector_s vector_data;/* Latest read data read from lis2dh */
int scale; /* Full scale in milliG */
sem_t devsem; /* Manages exclusive access to this structure */
bool fifo_used; /* LIS2DH configured to use FIFO */
bool fifo_stopped;/* FIFO got full and has stopped. */
......@@ -133,7 +134,7 @@ static int lis2dh_poll(FAR struct file *filep, FAR struct pollfd *
static void lis2dh_notify(FAR struct lis2dh_dev_s *priv);
static int lis2dh_int_handler(int irq, FAR void *context);
static int lis2dh_setup(FAR struct lis2dh_dev_s * dev, struct lis2dh_setup *new_setup);
static int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte);
static inline int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte, int scale);
static int lis2dh_read_temp(FAR struct lis2dh_dev_s *dev, int16_t *temper);
static int lis2dh_clear_interrupts(FAR struct lis2dh_dev_s *priv, uint8_t interrupts);
#ifdef LIS2DH_SELFTEST
......@@ -1072,6 +1073,7 @@ static int lis2dh_clear_interrupts(FAR struct lis2dh_dev_s *priv, uint8_t interr
****************************************************************************/
static FAR const struct lis2dh_vector_s * lis2dh_get_readings(FAR struct lis2dh_dev_s * dev, bool force_read, int *err)
{
int scale = dev->scale;
uint8_t retval[7];
DEBUGASSERT(dev != NULL);
......@@ -1089,9 +1091,9 @@ static FAR const struct lis2dh_vector_s * lis2dh_get_readings(FAR struct lis2dh_
return NULL;
}
dev->vector_data.x = lis2dh_raw_to_mg(retval[2], retval[1]);
dev->vector_data.y = lis2dh_raw_to_mg(retval[4], retval[3]);
dev->vector_data.z = lis2dh_raw_to_mg(retval[6], retval[5]);
dev->vector_data.x = lis2dh_raw_to_mg(retval[2], retval[1], scale);
dev->vector_data.y = lis2dh_raw_to_mg(retval[4], retval[3], scale);
dev->vector_data.z = lis2dh_raw_to_mg(retval[6], retval[5], scale);
/* Add something to entropy pool. */
......@@ -1113,42 +1115,23 @@ static FAR const struct lis2dh_vector_s * lis2dh_get_readings(FAR struct lis2dh_
* Input Parameters:
* raw_hibyte - Hi byte of raw data
* raw_lobyte - Lo byte of raw data
* scale - full scale in milliG
*
* Returned Value:
* Returns acceleration value in mg
****************************************************************************/
static int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte)
static inline int16_t lis2dh_raw_to_mg(uint8_t raw_hibyte, uint8_t raw_lobyte,
int scale)
{
int16_t value;
/* Value is signed integer, range INT16_MIN..INT16_MAX. */
value = (raw_hibyte << 8) | raw_lobyte;
//value = value >> 4;
switch(lis2dh_data->setup->fullscale)
{
case ST_LIS2DH_CR4_FULL_SCALE_2G:
default:
value = value >> 4;
break;
case ST_LIS2DH_CR4_FULL_SCALE_4G:
value = value >> 3;
break;
case ST_LIS2DH_CR4_FULL_SCALE_8G:
value = value / 62; // FIXME
break;
case ST_LIS2DH_CR4_FULL_SCALE_16G:
value = value / 186; // FIXME
break;
}
value &= 0xfff;
if (value & 0x800)
{
value = ~value;
value &= 0xfff;
value +=1;
value = -value;
}
return value;
/* Scale to mg, INT16_MIN..INT16_MAX => -scale..scale */
return (int32_t)value * scale / INT16_MAX;
}
static int lis2dh_read_temp(FAR struct lis2dh_dev_s *dev, int16_t *temper)
......@@ -1528,6 +1511,23 @@ static int lis2dh_setup(FAR struct lis2dh_dev_s * dev, struct lis2dh_setup *new_
goto error;
}
switch (dev->setup->fullscale & 0x30)
{
default:
case ST_LIS2DH_CR4_FULL_SCALE_2G:
dev->scale = 2000;
break;
case ST_LIS2DH_CR4_FULL_SCALE_4G:
dev->scale = 4000;
break;
case ST_LIS2DH_CR4_FULL_SCALE_8G:
dev->scale = 8000;
break;
case ST_LIS2DH_CR4_FULL_SCALE_16G:
dev->scale = 16000;
break;
}
if (dev->setup->fifo_enable)
{
dev->fifo_used = true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment