OrgApache/incubator-nuttx
OrgApache
/
incubator-nuttx
mirror of https://github.com/apache/incubator-nuttx.git
1
0
Fork 0
Code Issues Releases Wiki Activity
incubator-nuttx/drivers/sensors/bmm150_uorb.c

770 lines
20 KiB
C
Raw Blame History

/****************************************************************************
* drivers/sensors/bmm150_uorb.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <debug.h>
#include <stdio.h>
#include <sys/param.h>
#include <nuttx/mutex.h>
#include <nuttx/signal.h>
#include <nuttx/kthread.h>
#include <nuttx/kmalloc.h>
#include <nuttx/sensors/bmm150.h>
#include <nuttx/sensors/sensor.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define BMM150_CHIPID_VAL 0x32
#define BMM150_CHIPID 0x40
#define BMM150_DATA_X_LSB 0x42
#define BMM150_DATA_X_MSB 0x43
#define BMM150_DATA_Y_LSB 0x44
#define BMM150_DATA_Y_MSB 0x45
#define BMM150_DATA_Z_LSB 0x46
#define BMM150_DATA_Z_MSB 0x47
#define BMM150_RHALL_LSB 0x48
#define BMM150_RHALL_MSB 0x49
#define BMM150_IRQSTAT 0x4a
#define BMM150_POWER 0x4b
#define BMM150_MODE 0x4c
#define BMM150_INTEN 0x4d
#define BMM150_INTCFG 0x4e
#define BMM150_LOW_THR 0x4f
#define BMM150_HIGH_THR 0x50
#define BMM150_REPXY 0x51
#define BMM150_REPZ 0x52
/* TRIM registers - NVM */
#define BMM150_TRIM_X1 0x5d /* 0 */
#define BMM150_TRIM_Y1 0x5e /* 1 */
#define BMM150_TRIM_Z4_LSB 0x62 /* 5 */
#define BMM150_TRIM_Z4_MSB 0x63 /* 6 */
#define BMM150_TRIM_X2 0x64 /* 7 */
#define BMM150_TRIM_Y2 0x65 /* 8 */
#define BMM150_TRIM_Z2_LSB 0x68 /* 11 */
#define BMM150_TRIM_Z2_MSB 0x69 /* 12 */
#define BMM150_TRIM_Z1_LSB 0x6a /* 13 */
#define BMM150_TRIM_Z1_MSB 0x6b /* 14 */
#define BMM150_TRIM_XYZ1_LSB 0x6c /* 15 */
#define BMM150_TRIM_XYZ1_MSB 0x6d /* 16 */
#define BMM150_TRIM_Z3_LSB 0x6e /* 17 */
#define BMM150_TRIM_Z3_MSB 0x6f /* 18 */
#define BMM150_TRIM_XY2 0x70 /* 19 */
#define BMM150_TRIM_XY1 0x71 /* 20 */
#define POWER_CONTROL (1 << 0)
#define MODE_SELFTEST 0x01
#define MODE_OPMODE_NORMAL (0 << 1)
#define MODE_OPMODE_FORCED (1 << 1)
#define MODE_OPMODE_SLEEP (2 << 1)
#define MODE_OPMODE_MASK (3 << 1)
#define MODE_DATARATE_10HZ (0 << 3)
#define MODE_DATARATE_2HZ (1 << 3)
#define MODE_DATARATE_6HZ (2 << 3)
#define MODE_DATARATE_8HZ (3 << 3)
#define MODE_DATARATE_15HZ (4 << 3)
#define MODE_DATARATE_20HZ (5 << 3)
#define MODE_DATARATE_25HZ (6 << 3)
#define MODE_DATARATE_30HZ (7 << 3)
#define MODE_ADV_SELFTEST0 (1 << 6)
#define MODE_ADV_SELFTEST1 (1 << 7)
#define INTCFG_EN_X (1 << 3)
#define INTCFG_EN_Y (1 << 4)
#define INTCFG_EN_Z (1 << 5)
/****************************************************************************
* Private Type Definitions
****************************************************************************/
struct bmm150_trim_s
{
int8_t x1;
int8_t y1;
int8_t x2;
int8_t y2;
uint16_t z1;
int16_t z2;
int16_t z3;
int16_t z4;
int8_t xy1;
int8_t xy2;
uint16_t xyz1;
};
/* Used by the driver to manage the device */
struct bmm150_sensor_dev_s
{
struct sensor_lowerhalf_s lower;
struct bmm150_trim_s trim;
bool enabled;
struct bmm150_config_s config;
uint32_t freq;
mutex_t lock;
#ifdef CONFIG_SENSORS_BMM150_POLL
uint32_t interval;
uint64_t last_update;
sem_t run;
#endif
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int bmm150_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable);
#ifndef CONFIG_SENSORS_BMM150_POLL
static int bmm150_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR char *buffer, size_t buflen);
#endif
static int bmm150_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us);
/* Helpers */
static float bmm150_getx(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data);
static float bmm150_gety(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data);
static float bmm150_getz(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct sensor_ops_s g_bmm150_sensor_ops =
{
NULL, /* open */
NULL, /* close */
bmm150_activate,
bmm150_set_interval,
NULL, /* batch */
#ifdef CONFIG_SENSORS_BMM150_POLL
NULL, /* fetch */
#else
bmm150_fetch,
#endif
NULL, /* flush */
NULL, /* selftest */
NULL, /* set_calibvalue */
NULL, /* calibrate */
NULL, /* get_info */
NULL, /* control */
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: bmm150_getreg8
*
* Description:
* Read from an 8-bit BMM150 register
*
****************************************************************************/
static uint8_t bmm150_getreg8(FAR struct bmm150_sensor_dev_s *priv,
uint8_t regaddr)
{
struct i2c_msg_s msg[2];
uint8_t regval = 0;
int ret;
msg[0].frequency = priv->freq;
msg[0].addr = priv->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &regaddr;
msg[0].length = 1;
msg[1].frequency = priv->freq;
msg[1].addr = priv->config.addr;
msg[1].flags = I2C_M_READ;
msg[1].buffer = &regval;
msg[1].length = 1;
ret = I2C_TRANSFER(priv->config.i2c, msg, 2);
if (ret < 0)
{
snerr("I2C_TRANSFER failed: %d\n", ret);
}
return regval;
}
/****************************************************************************
* Name: bmm150_getregs
*
* Description:
* Read cnt bytes from specified dev_addr and reg_addr
*
****************************************************************************/
void bmm150_getregs(FAR struct bmm150_sensor_dev_s *priv, uint8_t regaddr,
FAR uint8_t *regval, int len)
{
struct i2c_msg_s msg[2];
int ret;
msg[0].frequency = priv->freq;
msg[0].addr = priv->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &regaddr;
msg[0].length = 1;
msg[1].frequency = priv->freq;
msg[1].addr = priv->config.addr;
msg[1].flags = I2C_M_READ;
msg[1].buffer = regval;
msg[1].length = len;
ret = I2C_TRANSFER(priv->config.i2c, msg, 2);
if (ret < 0)
{
snerr("I2C_TRANSFER failed: %d\n", ret);
}
}
/****************************************************************************
* Name: bmm150_putreg8
*
* Description:
* Write to an 8-bit BMM150 register
*
****************************************************************************/
static void bmm150_putreg8(FAR struct bmm150_sensor_dev_s *priv,
uint8_t regaddr, uint8_t regval)
{
struct i2c_msg_s msg[2];
uint8_t txbuffer[2];
int ret;
txbuffer[0] = regaddr;
txbuffer[1] = regval;
msg[0].frequency = priv->freq;
msg[0].addr = priv->config.addr;
msg[0].flags = 0;
msg[0].buffer = txbuffer;
msg[0].length = 2;
ret = I2C_TRANSFER(priv->config.i2c, msg, 1);
if (ret < 0)
{
snerr("I2C_TRANSFER failed: %d\n", ret);
}
}
/****************************************************************************
* Name: bmm150_initialize
*
* Description:
* Intialzie BMM150 driver
*
****************************************************************************/
static int bmm150_initialzie(FAR struct bmm150_sensor_dev_s *dev)
{
uint8_t data[21];
uint16_t u16;
int16_t i16;
uint8_t id = 0;
int ret = OK;
/* Set Power Control */
bmm150_putreg8(dev, BMM150_POWER, POWER_CONTROL);
/* Start-Up Time */
up_mdelay(3);
/* Read Chip ID */
id = bmm150_getreg8(dev, BMM150_CHIPID);
if (id != BMM150_CHIPID_VAL)
{
/* Chip ID is not Correct */
snerr("Wrong chip ID! %02x\n", id);
ret = -ENODEV;
goto errout;
}
/* Get TRIM registers */
bmm150_getregs(dev, BMM150_TRIM_X1, (FAR uint8_t *)data, 20);
dev->trim.x1 = (int8_t)data[0];
dev->trim.y1 = (int8_t)data[1];
dev->trim.x2 = (int8_t)data[7];
dev->trim.y2 = (int8_t)data[8];
u16 = ((uint16_t)(data[14] << 8) + data[11]);
dev->trim.z1 = u16;
i16 = ((int16_t)(data[12] << 8) + data[11]);
dev->trim.z2 = i16;
i16 = ((int16_t)(data[18] << 8) + data[17]);
dev->trim.z3 = i16;
i16 = ((int16_t)(data[6] << 8) + data[7]);
dev->trim.z4 = i16;
dev->trim.xy1 = (int8_t)data[20];
dev->trim.xy2 = (int8_t)data[19];
u16 = ((uint16_t)(data[16] << 8) + data[15]);
dev->trim.xyz1 = u16;
errout:
/* Clear Power Control */
bmm150_putreg8(dev, BMM150_POWER, 0);
return ret;
}
/****************************************************************************
* Name: bmm150_activate
****************************************************************************/
static int bmm150_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable)
{
FAR struct bmm150_sensor_dev_s *dev = NULL;
#ifdef CONFIG_SENSORS_BMM150_POLL
bool start_thread = false;
#endif
uint8_t val = 0;
dev = (FAR struct bmm150_sensor_dev_s *)lower;
nxmutex_lock(&dev->lock);
if (enable)
{
#ifdef CONFIG_SENSORS_BMM150_POLL
if (!dev->enabled)
{
start_thread = true;
dev->last_update = sensor_get_timestamp();
}
#endif
/* Set Power Control */
bmm150_putreg8(dev, BMM150_POWER, POWER_CONTROL);
/* Start-Up Time */
up_mdelay(3);
/* Set Normal Mode */
val = bmm150_getreg8(dev, BMM150_MODE);
val &= ~MODE_OPMODE_MASK;
val |= MODE_OPMODE_NORMAL;
bmm150_putreg8(dev, BMM150_MODE, val);
/* Enable XYZ - active low */
val = 0;
bmm150_putreg8(dev, BMM150_INTCFG, val);
}
else
{
/* Set Sleep Mode */
val = bmm150_getreg8(dev, BMM150_MODE);
val |= MODE_OPMODE_SLEEP;
bmm150_putreg8(dev, BMM150_MODE, val);
}
dev->enabled = enable;
#ifdef CONFIG_SENSORS_BMM150_POLL
if (start_thread)
{
/* Wake up the thread */
nxsem_post(&dev->run);
}
#endif
nxmutex_unlock(&dev->lock);
return OK;
}
/****************************************************************************
* Name: bmm150_getx
****************************************************************************/
static float bmm150_getx(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data)
{
float tmp = 0.0f;
float x0 = 0.0f;
float x1 = 0.0f;
float x2 = 0.0f;
float x3 = 0.0f;
float x4 = 0.0f;
/* TODO: check overflow */
x0 = dev->trim.xyz1 * 16384.0f / data[3];
tmp = x0 - 16384.0f;
x1 = dev->trim.xy2 * (tmp * tmp / 268435456.0f);
x2 = x1 + tmp * dev->trim.xy1 / 16384.0f;
x3 = dev->trim.x2 + 160.0f;
x4 = data[0] * (x2 + 256.0f) * x3;
tmp = ((x4 / 8192.0f) + dev->trim.x1 * 8.0f) / 16.0f;
return tmp;
}
/****************************************************************************
* Name: bmm150_gety
****************************************************************************/
static float bmm150_gety(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data)
{
float tmp = 0.0f;
float y0 = 0.0f;
float y1 = 0.0f;
float y2 = 0.0f;
float y3 = 0.0f;
float y4 = 0.0f;
/* TODO: check overflow */
y0 = dev->trim.xyz1 * 16384.0f / data[3];
tmp = y0 - 16384.0f;
y1 = dev->trim.xy2 * (tmp * tmp / 268435456.0f);
y2 = y1 + tmp * dev->trim.xy1 / 16384.0f;
y3 = dev->trim.y2 + 160.0f;
y4 = data[1] * (y2 + 256.0f) * y3;
tmp = ((y4 / 8192.0f) + dev->trim.y1 * 8.0f) / 16.0f;
return tmp;
}
/****************************************************************************
* Name: bmm150_getz
****************************************************************************/
static float bmm150_getz(FAR struct bmm150_sensor_dev_s *dev,
FAR int16_t *data)
{
float tmp = 0.0f;
float z0 = 0.0f;
float z1 = 0.0f;
float z2 = 0.0f;
float z3 = 0.0f;
float z4 = 0.0f;
float z5 = 0.0f;
/* TODO: check overflow */
z0 = data[2] - dev->trim.z4;
z1 = data[3] - dev->trim.xyz1;
z2 = dev->trim.z3 * z1;
z3 = dev->trim.z1 * data[3] / 32768.0f;
z4 = dev->trim.z2 + z3;
z5 = z0 * 121072.0f - z2;
tmp = (z5 / (z4 * 4.0f)) / 16.0f;
return tmp;
}
#ifndef CONFIG_SENSORS_BMM150_POLL
/****************************************************************************
* Name: bmm150_fetch
****************************************************************************/
static int bmm150_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR char *buffer, size_t buflen)
{
FAR struct bmm150_sensor_dev_s *dev =
(FAR struct bmm150_sensor_dev_s *)lower;
struct sensor_mag mag_data;
int16_t data[4];
uint64_t now = sensor_get_timestamp();
int ret = 0;
nxmutex_lock(&dev->lock);
if (!dev->enabled)
{
ret = -EACCES;
goto errout;
}
if (buflen != sizeof(mag_data))
{
ret = -EINVAL;
goto errout;
}
/* Get data */
do
{
bmm150_getregs(dev, BMM150_DATA_X_LSB, (FAR uint8_t *)data, 8);
}
while (!(data[3] & 1));
/* Correct data */
data[0] >>= 3;
data[1] >>= 3;
data[2] >>= 1;
data[3] >>= 2;
/* Get compensated data */
mag_data.timestamp = now;
mag_data.x = bmm150_getx(dev, data);
mag_data.y = bmm150_gety(dev, data);
mag_data.z = bmm150_getz(dev, data);
memcpy(buffer, &mag_data, sizeof(mag_data));
ret = sizeof(mag_data);
errout:
nxmutex_unlock(&dev->lock);
return ret;
}
#endif
/****************************************************************************
* Name: bmm150_set_interval
****************************************************************************/
static int bmm150_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *interval)
{
#ifdef CONFIG_SENSORS_BMM150_POLL
FAR struct bmm150_sensor_dev_s *dev =
(FAR struct bmm150_sensor_dev_s *)lower;
dev->interval = *interval;
#endif
return OK;
}
#ifdef CONFIG_SENSORS_BMM150_POLL
/****************************************************************************
* Name: bmm150_thread
*
* Description: Thread for performing interval measurement cycle and data
* read.
*
* Parameter:
* argc - Number opf arguments
* argv - Pointer to argument list
*
****************************************************************************/
static int bmm150_thread(int argc, FAR char **argv)
{
FAR struct bmm150_sensor_dev_s *dev =
(FAR struct bmm150_sensor_dev_s *)((uintptr_t)strtoul(argv[1], NULL,
16));
struct sensor_mag mag_data;
int16_t data[4];
int ret = 0;
while (true)
{
if (!dev->enabled)
{
/* Waiting to be woken up */
ret = nxsem_wait(&dev->run);
if (ret < 0)
{
continue;
}
}
if (dev->enabled)
{
/* Get data */
bmm150_getregs(dev, BMM150_DATA_X_LSB, (FAR uint8_t *)data, 8);
/* Correct data */
data[0] >>= 3;
data[1] >>= 3;
data[2] >>= 1;
data[3] >>= 2;
/* Get compensated data */
mag_data.timestamp = sensor_get_timestamp();
mag_data.x = bmm150_getx(dev, data);
mag_data.y = bmm150_gety(dev, data);
mag_data.z = bmm150_getz(dev, data);
dev->lower.push_event(dev->lower.priv,
&mag_data, sizeof(mag_data));
}
/* Sleeping thread before fetching the next sensor data */
nxsig_usleep(dev->interval);
}
return OK;
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: bmm150_register_uorb
*
* Description:
* Register the BMM150 uorb device as 'devpath'
*
* Input Parameters:
* devno - The user specifies device number, from 0.
* config - device configuration
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int bmm150_register_uorb(int devno, FAR struct bmm150_config_s *config)
{
FAR struct bmm150_sensor_dev_s *dev = NULL;
#ifdef CONFIG_SENSORS_BMM150_POLL
FAR char *argv[2];
char arg1[32];
#endif
int ret = OK;
/* Without config info, we can't do anything. */
if (config == NULL)
{
return -EINVAL;
}
/* Initialize the device structure. */
dev = kmm_malloc(sizeof(struct bmm150_sensor_dev_s));
if (dev == NULL)
{
snerr("ERROR: Failed to allocate bmm150 device instance\n");
return -ENOMEM;
}
memset(dev, 0, sizeof(struct bmm150_sensor_dev_s));
nxmutex_init(&dev->lock);
#ifdef CONFIG_SENSORS_BMM150_POLL
nxsem_init(&dev->run, 0, 0);
#endif
/* Configure dev */
dev->config.i2c = config->i2c;
dev->config.addr = config->addr;
dev->freq = CONFIG_BMM150_I2C_FREQUENCY;
/* Register sensor */
dev->lower.type = SENSOR_TYPE_MAGNETIC_FIELD;
dev->lower.ops = &g_bmm150_sensor_ops;
#ifdef CONFIG_SENSORS_BMM150_POLL
dev->enabled = false;
dev->interval = CONFIG_SENSORS_BMM150_POLL_INTERVAL;
#endif
/* Check Device ID */
ret = bmm150_initialzie(dev);
if (ret < 0)
{
snerr("Failed to initialize driver: %d\n", ret);
return ret;
}
/* Regsiter driver */
ret = sensor_register(&dev->lower, devno);
if (ret < 0)
{
goto mag_err;
}
#ifdef CONFIG_SENSORS_BMM150_POLL
/* Create thread for polling sensor data */
snprintf(arg1, 16, "%p", dev);
argv[0] = arg1;
argv[1] = NULL;
ret = kthread_create("bmm150_thread", SCHED_PRIORITY_DEFAULT,
CONFIG_SENSORS_BMM150_THREAD_STACKSIZE,
bmm150_thread,
argv);
if (ret < 0)
{
goto thr_err;
}
#endif
return ret;
#ifdef CONFIG_SENSORS_BMM150_POLL
thr_err:
#endif
sensor_unregister(&dev->lower, devno);
mag_err:
kmm_free(dev);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink