sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
26999450cc
nuttx/drivers/sensors/lsm9ds1_uorb.c
raiden00pl 79e8aa5e21 sensors: add uorb support for lsm9ds1
2024-03-09 11:53:57 +08:00

804 lines
22 KiB
C
Raw Blame History

/****************************************************************************
* drivers/sensors/lsm9ds1_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 <math.h>
#include <stdio.h>
#include <string.h>
#include <limits.h>
#include <fcntl.h>
#include <inttypes.h>
#include <sys/param.h>
#include <nuttx/mutex.h>
#include <nuttx/signal.h>
#include <nuttx/compiler.h>
#include <nuttx/nuttx.h>
#include <nuttx/kthread.h>
#include <nuttx/sensors/sensor.h>
#include <nuttx/sensors/ioctl.h>
#include "lsm9ds1_base.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define CONSTANTS_ONE_G 9.8f
/****************************************************************************
* Private Types
****************************************************************************/
enum lsm9ds1_idx_e
{
LSM9DS1_ACCEL_IDX = 0,
LSM9DS1_GYRO_IDX,
LSM9DS1_MAG_IDX,
LSM9DS1_MAX_IDX
};
struct lsm9ds1_sensor_s
{
struct sensor_lowerhalf_s lower;
uint64_t last_update;
float scale;
FAR void *dev;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
bool enabled;
unsigned long interval;
#endif
struct lsm9ds1_dev_s base;
};
struct lsm9ds1_sensor_dev_s
{
struct lsm9ds1_sensor_s priv[3];
mutex_t lock;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
sem_t run;
#endif
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* Sensor methods */
static int lsm9ds1_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
bool enable);
static int lsm9ds1_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR unsigned long *period_us);
#ifndef CONFIG_SENSORS_LSM9DS1_POLL
static int lsm9ds1_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR char *buffer, size_t buflen);
#endif
static int lsm9ds1_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
int cmd, unsigned long arg);
/* Helpers */
static int lsm9ds1_mag_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale);
static int lsm9ds1_accel_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale);
static int lsm9ds1_gyro_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct sensor_ops_s g_sensor_ops =
{
NULL, /* open */
NULL, /* close */
.activate = lsm9ds1_activate,
.set_interval = lsm9ds1_set_interval,
NULL, /* batch */
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
NULL, /* fetch */
#else
.fetch = lsm9ds1_fetch,
#endif
NULL, /* selftest */
NULL, /* set_calibvalue */
NULL, /* calibrate */
.control = lsm9ds1_control
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: lsm9ds1_activate
****************************************************************************/
static int lsm9ds1_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable)
{
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
FAR struct lsm9ds1_sensor_s *priv = NULL;
FAR struct lsm9ds1_sensor_dev_s *dev = NULL;
bool start_thread = false;
int ret = OK;
priv = container_of(lower, struct lsm9ds1_sensor_s, lower);
dev = priv->dev;
if (enable)
{
if (!priv->enabled)
{
start_thread = true;
priv->last_update = sensor_get_timestamp();
}
ret = priv->base.ops->start(&priv->base);
}
else
{
ret = priv->base.ops->stop(&priv->base);
}
priv->enabled = enable;
if (start_thread)
{
/* Wake up the thread */
nxsem_post(&dev->run);
}
return ret;
#else
return OK;
#endif
}
/****************************************************************************
* Name: lsm9ds1_set_interval
****************************************************************************/
static int lsm9ds1_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR unsigned long *interval)
{
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
FAR struct lsm9ds1_sensor_s *priv = NULL;
priv = container_of(lower, struct lsm9ds1_sensor_s, lower);
priv->interval = *interval;
#endif
return OK;
}
/****************************************************************************
* Name: lsm9ds1_data
****************************************************************************/
static int16_t lsm9ds1_data(int16_t data)
{
/* The value is positive */
if (data < 0x8000)
{
data = data;
}
/* The value is negative, so find its absolute value by taking the
* two's complement
*/
else if (data > 0x8000)
{
data = -(~data + 1);
}
/* The value is negative and can't be represented as a positive
* int16_t value
*/
else
{
data = -32768;
}
return data;
}
#ifndef CONFIG_SENSORS_LSM9DS1_POLL
/****************************************************************************
* Name: lsm9ds1_set_interval
****************************************************************************/
static int lsm9ds1_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, FAR char *buffer,
size_t buflen)
{
FAR struct lsm9ds1_sensor_s *priv = NULL;
int16_t data[3];
int ret = OK;
priv = container_of(lower, struct lsm9ds1_sensor_s, lower);
switch (lower->type)
{
case SENSOR_TYPE_ACCELEROMETER:
{
struct sensor_accel accel;
ret = lsm9ds1_readreg(&priv->base, LSM9DS1_OUT_X_L_XL,
(FAR uint8_t *)data, 6);
accel.timestamp = sensor_get_timestamp();
accel.x = (int16_t)lsm9ds1_data(data[0]) * priv->scale;
accel.y = (int16_t)lsm9ds1_data(data[1]) * priv->scale;
accel.z = (int16_t)lsm9ds1_data(data[2]) * priv->scale;
memcpy(buffer, &accel, sizeof(accel));
break;
}
case SENSOR_TYPE_GYROSCOPE:
{
struct sensor_gyro gyro;
ret = lsm9ds1_readreg(&priv->base, LSM9DS1_OUT_X_L_G,
(FAR uint8_t *)data, 6);
gyro.timestamp = sensor_get_timestamp();
gyro.x = (int16_t)lsm9ds1_data(data[0]) * priv->scale;
gyro.y = (int16_t)lsm9ds1_data(data[1]) * priv->scale;
gyro.z = (int16_t)lsm9ds1_data(data[2]) * priv->scale;
memcpy(buffer, &gyro, sizeof(gyro));
break;
}
case SENSOR_TYPE_MAGNETIC_FIELD:
{
struct sensor_mag mag;
ret = lsm9ds1_readreg(&priv->base, LSM9DS1_OUT_X_L_M,
(FAR uint8_t *)data, 6);
mag.timestamp = sensor_get_timestamp();
mag.x = (int16_t)lsm9ds1_data(data[0]) * priv->scale;
mag.y = (int16_t)lsm9ds1_data(data[1]) * priv->scale;
mag.z = (int16_t)lsm9ds1_data(data[2]) * priv->scale;
memcpy(buffer, &mag, sizeof(mag));
break;
}
default:
{
ret = -EINVAL;
break;
}
}
return ret;
}
#endif
/****************************************************************************
* Name: lsm9ds1_cotrol
****************************************************************************/
static int lsm9ds1_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, int cmd,
unsigned long arg)
{
FAR struct lsm9ds1_sensor_s *priv = NULL;
int ret = OK;
priv = container_of(lower, struct lsm9ds1_sensor_s, lower);
switch (cmd)
{
/* Set full scale command */
case SNIOC_SET_SCALE_XL:
{
if (priv->lower.type == SENSOR_TYPE_GYROSCOPE)
{
ret = lsm9ds1_gyro_scale(priv, arg);
}
else if (priv->lower.type == SENSOR_TYPE_ACCELEROMETER)
{
ret = lsm9ds1_accel_scale(priv, arg);
}
else if (priv->lower.type == SENSOR_TYPE_MAGNETIC_FIELD)
{
ret = lsm9ds1_mag_scale(priv, arg);
}
break;
}
default:
{
snerr("ERROR: Unrecognized cmd: %d\n", cmd);
ret = -ENOTTY;
break;
}
}
return ret;
}
/****************************************************************************
* Name: lsm9ds1_mag_scale
****************************************************************************/
static int lsm9ds1_mag_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale)
{
int ret = OK;
ret = priv->base.ops->setfullscale(&priv->base, scale);
if (ret < 0)
{
return ret;
}
if (scale < lsm9ds1_midpoint(4, 8))
{
priv->scale = 8.f / 65536.f;
}
else if (scale < lsm9ds1_midpoint(8, 12))
{
priv->scale = 16.f / 65536.f;
}
else if (scale < lsm9ds1_midpoint(12, 16))
{
priv->scale = 24.f / 65536.f;
}
else
{
priv->scale = 32.f / 65536.f;
}
return ret;
}
/****************************************************************************
* Name: lsm9ds1_accel_scale
****************************************************************************/
static int lsm9ds1_accel_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale)
{
int ret = OK;
ret = priv->base.ops->setfullscale(&priv->base, scale);
if (ret < 0)
{
return ret;
}
if (scale < lsm9ds1_midpoint(2, 4))
{
priv->scale = CONSTANTS_ONE_G / 16384.f;
}
else if (scale < lsm9ds1_midpoint(4, 8))
{
priv->scale = CONSTANTS_ONE_G / 8192.f;
}
else if (scale < lsm9ds1_midpoint(8, 16))
{
priv->scale = CONSTANTS_ONE_G / 4096.f;
}
else
{
priv->scale = CONSTANTS_ONE_G / 2048.f;
}
return ret;
}
/****************************************************************************
* Name: lsm9ds1_gyro_scale
****************************************************************************/
static int lsm9ds1_gyro_scale(FAR struct lsm9ds1_sensor_s *priv,
uint8_t scale)
{
int ret = OK;
ret = priv->base.ops->setfullscale(&priv->base, scale);
if (ret < 0)
{
return ret;
}
if (scale < lsm9ds1_midpoint(245, 500))
{
priv->scale = (M_PI / 180.0f) * 245.f / 32768.f;
}
else if (scale < lsm9ds1_midpoint(500, 2000))
{
priv->scale = (M_PI / 180.0f) * 500.f / 32768.f;
}
else
{
priv->scale = (M_PI / 180.0f) * 2000.f / 32768.f;
}
return ret;
}
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
/****************************************************************************
* Name: lsm9ds1_accel_data
*
* Description: get and push accel data from struct sensor_data_s
*
* Parameter:
* priv - Internal private lower half driver instance
* buf - Point to data
*
* Return:
* OK - on success
*
****************************************************************************/
static void lsm9ds1_accel_data(FAR struct lsm9ds1_sensor_s *priv,
FAR int16_t *buf)
{
FAR struct sensor_lowerhalf_s *lower = &priv->lower;
struct sensor_accel accel;
uint64_t now = sensor_get_timestamp();
if (!priv->enabled || now - priv->last_update < priv->interval)
{
return;
}
priv->last_update = now;
accel.timestamp = now;
accel.x = (int16_t)lsm9ds1_data(buf[0]) * priv->scale;
accel.y = (int16_t)lsm9ds1_data(buf[1]) * priv->scale;
accel.z = (int16_t)lsm9ds1_data(buf[2]) * priv->scale;
accel.temperature = 0;
lower->push_event(lower->priv, &accel, sizeof(accel));
}
/****************************************************************************
* Name: lsm9ds1_gyro_data
*
* Description: get and push gyro data from struct sensor_data_s
*
* Parameter:
* priv - Internal private lower half driver instance
* buf - Point to data
*
* Return:
* OK - on success
*
****************************************************************************/
static void lsm9ds1_gyro_data(FAR struct lsm9ds1_sensor_s *priv,
FAR int16_t *buf)
{
FAR struct sensor_lowerhalf_s *lower = &priv->lower;
struct sensor_gyro gyro;
uint64_t now = sensor_get_timestamp();
if (!priv->enabled || now - priv->last_update < priv->interval)
{
return;
}
priv->last_update = now;
gyro.timestamp = now;
gyro.x = (int16_t)lsm9ds1_data(buf[0]) * priv->scale;
gyro.y = (int16_t)lsm9ds1_data(buf[1]) * priv->scale;
gyro.z = (int16_t)lsm9ds1_data(buf[2]) * priv->scale;
gyro.temperature = 0;
lower->push_event(lower->priv, &gyro, sizeof(gyro));
}
/****************************************************************************
* Name: lsm9ds1_mag_data
*
* Description: get and push magnetometer data from struct sensor_data_s
*
* Parameter:
* priv - Internal private lower half driver instance
* buf - Point to data
*
* Return:
* OK - on success
*
****************************************************************************/
static void lsm9ds1_mag_data(FAR struct lsm9ds1_sensor_s *priv,
FAR int16_t *buf)
{
FAR struct sensor_lowerhalf_s *lower = &priv->lower;
struct sensor_mag mag;
uint64_t now = sensor_get_timestamp();
if (!priv->enabled || now - priv->last_update < priv->interval)
{
return;
}
priv->last_update = now;
mag.timestamp = now;
mag.x = (int16_t)lsm9ds1_data(buf[0]) * priv->scale;
mag.y = (int16_t)lsm9ds1_data(buf[1]) * priv->scale;
mag.z = (int16_t)lsm9ds1_data(buf[2]) * priv->scale;
mag.temperature = 0;
lower->push_event(lower->priv, &mag, sizeof(mag));
}
/****************************************************************************
* Name: lsm9ds1_thread
*
* Description: Thread for performing interval measurement cycle and data
* read.
*
* Parameter:
* argc - Number opf arguments
* argv - Pointer to argument list
*
****************************************************************************/
static int lsm9ds1_thread(int argc, FAR char **argv)
{
FAR struct lsm9ds1_sensor_dev_s *dev
= (FAR struct lsm9ds1_sensor_dev_s *)((uintptr_t)strtoul(argv[1], NULL,
16));
FAR struct lsm9ds1_sensor_s *accel = &dev->priv[LSM9DS1_ACCEL_IDX];
FAR struct lsm9ds1_sensor_s *gyro = &dev->priv[LSM9DS1_GYRO_IDX];
FAR struct lsm9ds1_sensor_s *mag = &dev->priv[LSM9DS1_MAG_IDX];
unsigned long min_interval;
int16_t adata[3];
int16_t gdata[3];
int16_t mdata[3];
int ret;
while (true)
{
if ((!accel->enabled) && (!gyro->enabled) && (!mag->enabled))
{
/* Waiting to be woken up */
ret = nxsem_wait(&dev->run);
if (ret < 0)
{
continue;
}
}
/* Read accel */
if (accel->enabled)
{
ret = lsm9ds1_readreg(&accel->base,
LSM9DS1_OUT_X_L_XL, (uint8_t *)adata, 6);
lsm9ds1_accel_data(accel, adata);
}
/* Read gyro */
if (gyro->enabled)
{
ret = lsm9ds1_readreg(&gyro->base,
LSM9DS1_OUT_X_L_G, (uint8_t *)gdata, 6);
lsm9ds1_gyro_data(gyro, gdata);
}
/* Read mag */
if (mag->enabled)
{
ret = lsm9ds1_readreg(&mag->base,
LSM9DS1_OUT_X_L_M, (uint8_t *)mdata, 6);
lsm9ds1_mag_data(mag, mdata);
}
/* Sleeping thread before fetching the next sensor data */
min_interval = MIN(accel->interval, gyro->interval);
min_interval = MIN(min_interval, mag->interval);
nxsig_usleep(min_interval);
}
return OK;
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: lsm9ds1_register_uorb
*
* Description:
* Register the LSM9DS1 IMU as sensor device
*
* Input Parameters:
* devno - Instance number for driver
* config - configuratio
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int lsm9ds1_register_uorb(int devno, FAR struct lsm9ds1_config_s *config)
{
FAR struct lsm9ds1_sensor_dev_s *dev = NULL;
FAR struct lsm9ds1_sensor_s *tmp = NULL;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
FAR char *argv[2];
char arg1[32];
#endif
int ret = OK;
/* Initialize the device structure. */
dev = (FAR struct lsm9ds1_sensor_dev_s *)kmm_malloc(sizeof(*dev));
if (dev == NULL)
{
snerr("ERROR: Failed to allocate instance\n");
return -ENOMEM;
}
memset(dev, 0, sizeof(*dev));
nxmutex_init(&dev->lock);
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
nxsem_init(&dev->run, 0, 0);
#endif
/* Accelerometer register */
tmp = &dev->priv[LSM9DS1_ACCEL_IDX];
tmp->dev = dev;
tmp->base.ops = &g_lsm9ds1accel_ops;
tmp->base.i2c = config->i2c;
tmp->base.addr = config->addr_acc;
tmp->lower.ops = &g_sensor_ops;
tmp->lower.type = SENSOR_TYPE_ACCELEROMETER;
tmp->lower.nbuffer = 1;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
tmp->enabled = false;
tmp->interval = CONFIG_SENSORS_LSM9DS1_POLL_INTERVAL;
#endif
ret = sensor_register(&tmp->lower, devno);
if (ret < 0)
{
snerr("sensor_register failed: %d\n", ret);
goto gyro_err;
}
lsm9ds1_accel_scale(tmp, 2);
/* Gyroscope register */
tmp = &dev->priv[LSM9DS1_GYRO_IDX];
tmp->dev = dev;
tmp->base.ops = &g_lsm9ds1gyro_ops;
tmp->base.i2c = config->i2c;
tmp->base.addr = config->addr_gyro;
tmp->lower.ops = &g_sensor_ops;
tmp->lower.type = SENSOR_TYPE_GYROSCOPE;
tmp->lower.nbuffer = 1;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
tmp->enabled = false;
tmp->interval = CONFIG_SENSORS_LSM9DS1_POLL_INTERVAL;
#endif
ret = sensor_register(&tmp->lower, devno);
if (ret < 0)
{
snerr("sensor_register failed: %d\n", ret);
goto gyro_err;
}
lsm9ds1_gyro_scale(tmp, 245);
/* Magnetic register */
tmp = &dev->priv[LSM9DS1_MAG_IDX];
tmp->dev = dev;
tmp->base.ops = &g_lsm9ds1mag_ops;
tmp->base.i2c = config->i2c;
tmp->base.addr = config->addr_mag;
tmp->lower.ops = &g_sensor_ops;
tmp->lower.type = SENSOR_TYPE_MAGNETIC_FIELD;
tmp->lower.nbuffer = 1;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
tmp->enabled = false;
tmp->interval = CONFIG_SENSORS_LSM9DS1_POLL_INTERVAL;
#endif
ret = sensor_register(&tmp->lower, devno);
if (ret < 0)
{
snerr("sensor_register failed: %d\n", ret);
goto mag_err;
}
lsm9ds1_mag_scale(tmp, 4);
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
/* Create thread for polling sensor data */
snprintf(arg1, 16, "%p", dev);
argv[0] = arg1;
argv[1] = NULL;
ret = kthread_create("lsm9ds1_thread", SCHED_PRIORITY_DEFAULT,
CONFIG_SENSORS_LSM9DS1_THREAD_STACKSIZE,
lsm9ds1_thread,
argv);
if (ret < 0)
{
goto thr_err;
}
#endif
return ret;
#ifdef CONFIG_SENSORS_LSM9DS1_POLL
thr_err:
#endif
sensor_unregister(&dev->priv[LSM9DS1_MAG_IDX].lower, devno);
mag_err:
sensor_unregister(&dev->priv[LSM9DS1_GYRO_IDX].lower, devno);
gyro_err:
sensor_unregister(&dev->priv[LSM9DS1_ACCEL_IDX].lower, devno);
kmm_free(dev);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink