Mirrors_Framework/nuttx
1
0
Fork 0
mirror of https://github.com/apache/nuttx.git synced 2026-05-27 19:36:35 +08:00
Code Issues Actions 1 Packages Projects Releases Wiki Activity

sensors/bmm150_uorb: support for fixed-point data

Browse Source 
support for fixed-point data for bmm150_uorb

Signed-off-by: raiden00pl <raiden00@railab.me>
This commit is contained in:
raiden00pl
2026-03-29 11:16:34 +02:00
committed by Xiang Xiao
parent cf9aaba449
commit c3190cd04b
1 changed files with 156 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drivers/sensors/bmm150_uorb.c
+156 -58
View File
@@ -43,12 +43,6 @@
* Pre-processor Definitions * Pre-processor Definitions
****************************************************************************/ ****************************************************************************/
/* Only float data type supported now */
#ifdef CONFIG_SENSORS_USE_B16
# error fixed-point data type not supported yet
#endif
#define BMM150_CHIPID_VAL 0x32 #define BMM150_CHIPID_VAL 0x32
#define BMM150_CHIPID 0x40 #define BMM150_CHIPID 0x40
@@ -166,12 +160,12 @@ static int bmm150_set_interval(FAR struct sensor_lowerhalf_s *lower,
/* Helpers */ /* Helpers */
static float bmm150_getx(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_getx(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data); FAR int16_t *data);
static float bmm150_gety(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_gety(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data); FAR int16_t *data);
static float bmm150_getz(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_getz(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data); FAR int16_t *data);
/**************************************************************************** /****************************************************************************
* Private Data * Private Data
@@ -444,25 +438,68 @@ static int bmm150_activate(FAR struct sensor_lowerhalf_s *lower,
* Name: bmm150_getx * Name: bmm150_getx
****************************************************************************/ ****************************************************************************/
static float bmm150_getx(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_getx(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data) FAR int16_t *data)
{ {
float tmp = 0.0f; sensor_data_t tmp;
float x0 = 0.0f; sensor_data_t x0;
float x1 = 0.0f; sensor_data_t x1;
float x2 = 0.0f; sensor_data_t x2;
float x3 = 0.0f; sensor_data_t x3;
float x4 = 0.0f; sensor_data_t x4;
/* TODO: check overflow */ /* TODO: check overflow */
x0 = dev->trim.xyz1 * 16384.0f / data[3]; /* x0 = xyz1 * 16384.0 / data[3]
tmp = x0 - 16384.0f; * = (xyz1 / data[3]) * 16384
x1 = dev->trim.xy2 * (tmp * tmp / 268435456.0f); */
x2 = x1 + tmp * dev->trim.xy1 / 16384.0f;
x3 = dev->trim.x2 + 160.0f; x0 = sensor_data_muli(
x4 = data[0] * (x2 + 256.0f) * x3; sensor_data_div(sensor_data_itof(dev->trim.xyz1),
tmp = ((x4 / 8192.0f) + dev->trim.x1 * 8.0f) / 16.0f; sensor_data_itof(data[3])),
16384);
/* tmp = x0 - 16384.0 */
tmp = sensor_data_subi(x0, 16384);
/* x1 = xy2 * (tmp * tmp / 268435456.0)
* = xy2 * (tmp / 16384.0) * (tmp / 16384.0)
*/
x1 = sensor_data_mul(sensor_data_itof(dev->trim.xy2),
sensor_data_mul(sensor_data_divi(tmp, 16384),
sensor_data_divi(tmp, 16384)));
/* x2 = x1 + tmp * xy1 / 16384.0
* = x1 + (tmp / 16384.0) * xy1
*/
x2 = sensor_data_add(x1,
sensor_data_muli(sensor_data_divi(tmp, 16384), dev->trim.xy1));
/* x3 = x2 + 160.0 */
x3 = sensor_data_add(sensor_data_itof(dev->trim.x2),
sensor_data_itof(160));
/* x4 = data[0] * (x2 + 256.0) * x3 / 8192.0
* = ((x2 + 256.0) / 8192.0) * x3 * data[0]
*/
x4 = sensor_data_mul(
sensor_data_mul(
sensor_data_divi(sensor_data_add(x2, sensor_data_itof(256)),
8192),
x3),
sensor_data_itof(data[0]));
/* tmp = (x4 + x1 * 8.0) / 16.0 */
tmp = sensor_data_divi(
sensor_data_add(x4,
sensor_data_muli(sensor_data_itof(dev->trim.x1), 8)),
16);
return tmp; return tmp;
} }
@@ -471,25 +508,68 @@ static float bmm150_getx(FAR struct bmm150_sensor_dev_s *dev,
* Name: bmm150_gety * Name: bmm150_gety
****************************************************************************/ ****************************************************************************/
static float bmm150_gety(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_gety(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data) FAR int16_t *data)
{ {
float tmp = 0.0f; sensor_data_t tmp;
float y0 = 0.0f; sensor_data_t y0;
float y1 = 0.0f; sensor_data_t y1;
float y2 = 0.0f; sensor_data_t y2;
float y3 = 0.0f; sensor_data_t y3;
float y4 = 0.0f; sensor_data_t y4;
/* TODO: check overflow */ /* TODO: check overflow */
y0 = dev->trim.xyz1 * 16384.0f / data[3]; /* y0 = xyz1 * 16384.0 / data[3]
tmp = y0 - 16384.0f; * = (xyz1 / data[3]) * 16384
y1 = dev->trim.xy2 * (tmp * tmp / 268435456.0f); */
y2 = y1 + tmp * dev->trim.xy1 / 16384.0f;
y3 = dev->trim.y2 + 160.0f; y0 = sensor_data_muli(
y4 = data[1] * (y2 + 256.0f) * y3; sensor_data_div(sensor_data_itof(dev->trim.xyz1),
tmp = ((y4 / 8192.0f) + dev->trim.y1 * 8.0f) / 16.0f; sensor_data_itof(data[3])),
16384);
/* tmp = y0 - 16384.0 */
tmp = sensor_data_subi(y0, 16384);
/* y1 = xy2 * (tmp * tmp / 268435456.0)
* = xy2 * (tmp / 16384.0) * (tmp / 16384.0)
*/
y1 = sensor_data_mul(sensor_data_itof(dev->trim.xy2),
sensor_data_mul(sensor_data_divi(tmp, 16384),
sensor_data_divi(tmp, 16384)));
/* y2 = y1 + tmp * xy1 / 16384.0
* = y1 + (tmp / 16384.0) * xy1
*/
y2 = sensor_data_add(y1,
sensor_data_muli(sensor_data_divi(tmp, 16384), dev->trim.xy1));
/* y3 = y2 + 160.0 */
y3 = sensor_data_add(sensor_data_itof(dev->trim.y2),
sensor_data_itof(160));
/* y4 = data[1] * (y2 + 256.0) * y3 / 8192.0
* = ((y2 + 256.0) / 8192.0) * y3 * data[1]
*/
y4 = sensor_data_mul(
sensor_data_mul(
sensor_data_divi(sensor_data_add(y2, sensor_data_itof(256)),
8192),
y3),
sensor_data_itof(data[1]));
/* tmp = (y4 + y1 * 8.0) / 16.0 */
tmp = sensor_data_divi(
sensor_data_add(y4,
sensor_data_muli(sensor_data_itof(dev->trim.y1), 8)),
16);
return tmp; return tmp;
} }
@@ -498,26 +578,44 @@ static float bmm150_gety(FAR struct bmm150_sensor_dev_s *dev,
* Name: bmm150_getz * Name: bmm150_getz
****************************************************************************/ ****************************************************************************/
static float bmm150_getz(FAR struct bmm150_sensor_dev_s *dev, static sensor_data_t bmm150_getz(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data) FAR int16_t *data)
{ {
float tmp = 0.0f; sensor_data_t tmp;
float z0 = 0.0f; sensor_data_t z0;
float z1 = 0.0f; sensor_data_t z1;
float z2 = 0.0f; sensor_data_t z3;
float z3 = 0.0f; sensor_data_t z4;
float z4 = 0.0f;
float z5 = 0.0f;
/* TODO: check overflow */ /* TODO: check overflow */
z0 = data[2] - dev->trim.z4; /* z0 = data[2] - z4_trim */
z1 = data[3] - dev->trim.xyz1;
z2 = dev->trim.z3 * z1; z0 = sensor_data_itof(data[2] - dev->trim.z4);
z3 = dev->trim.z1 * data[3] / 32768.0f;
z4 = dev->trim.z2 + z3; /* z1 = data[3] - xyz1 */
z5 = z0 * 121072.0f - z2;
tmp = (z5 / (z4 * 4.0f)) / 16.0f; z1 = sensor_data_itof(data[3] - dev->trim.xyz1);
/* z3 = z1_trim * (data[3] / 32768.0) */
z3 = sensor_data_mul(sensor_data_itof(dev->trim.z1),
sensor_data_divi(sensor_data_itof(data[3]), 32768));
/* z4 = z2_trim + z3 */
z4 = sensor_data_add(sensor_data_itof(dev->trim.z2), z3);
/* tmp = (z0 * 121072.0 - z3_trim * z1) / (z4 * 4.0) / 16.0
* = (z0 / z4) * 1891.75 - z3_trim * (z1 / z4) / 64.0
*/
tmp = sensor_data_sub(
sensor_data_mul(sensor_data_div(z0, z4),
sensor_data_ftof(1891.75f)),
sensor_data_divi(
sensor_data_muli(sensor_data_div(z1, z4), dev->trim.z3),
64));
return tmp; return tmp;
} }