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/bmi270_uorb.c

710 lines
19 KiB
C
Raw Blame History

/****************************************************************************
* drivers/sensors/bmi270_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 "bmi270_base.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define CONSTANTS_ONE_G 9.8f
/****************************************************************************
* Private Types
****************************************************************************/
enum bmi270_idx_e
{
BMI270_ACCEL_IDX = 0,
BMI270_GYRO_IDX,
BMI270_MAX_IDX
};
struct bmi270_sensor_s
{
struct sensor_lowerhalf_s lower;
uint64_t last_update;
float scale;
FAR void *dev;
bool enabled;
#ifdef CONFIG_SENSORS_BMI270_POLL
uint32_t interval;
#endif
struct bmi270_dev_s base;
};
struct bmi270_sensor_dev_s
{
struct bmi270_sensor_s priv[BMI270_MAX_IDX];
mutex_t lock;
#ifdef CONFIG_SENSORS_BMI270_POLL
sem_t run;
#endif
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* Sensor methods */
static int bmi270_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
bool enable);
static int bmi270_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us);
#ifndef CONFIG_SENSORS_BMI270_POLL
static int bmi270_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR char *buffer, size_t buflen);
#endif
static int bmi270_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
int cmd, unsigned long arg);
/* Helpers */
static int bmi270_accel_scale(FAR struct bmi270_sensor_s *priv,
uint8_t scale);
static int bmi270_gyro_scale(FAR struct bmi270_sensor_s *priv,
uint16_t scale);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct sensor_ops_s g_sensor_ops =
{
NULL, /* open */
NULL, /* close */
bmi270_activate,
bmi270_set_interval,
NULL, /* batch */
#ifdef CONFIG_SENSORS_BMI270_POLL
NULL, /* fetch */
#else
bmi270_fetch,
#endif
NULL, /* flush */
NULL, /* selftest */
NULL, /* set_calibvalue */
NULL, /* calibrate */
NULL, /* get_info */
bmi270_control
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: bmi270_activate
****************************************************************************/
static int bmi270_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable)
{
FAR struct bmi270_sensor_s *priv = NULL;
FAR struct bmi270_sensor_dev_s *dev = NULL;
bool start = false;
bool stop = false;
int ret = OK;
int tmp = 0;
priv = (FAR struct bmi270_sensor_s *)lower;
dev = priv->dev;
nxmutex_lock(&dev->lock);
tmp = (dev->priv[BMI270_ACCEL_IDX].enabled +
dev->priv[BMI270_GYRO_IDX].enabled);
if (enable && tmp == 0)
{
/* One time start */
start = true;
}
else if (!enable && tmp == 1)
{
/* One time stop */
stop = true;
}
priv->enabled = enable;
nxmutex_unlock(&dev->lock);
if (start)
{
/* Set normal mode */
bmi270_set_normal_imu(&priv->base);
#ifdef CONFIG_SENSORS_BMI270_POLL
priv->last_update = sensor_get_timestamp();
/* Wake up the thread */
nxsem_post(&dev->run);
#endif
}
else if (stop)
{
/* Disable acquisition of acc and gyro */
bmi270_putreg8(&priv->base, BMI270_PWR_CTRL, 0);
up_mdelay(30);
}
return ret;
}
/****************************************************************************
* Name: bmi270_set_interval
****************************************************************************/
static int bmi270_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *interval)
{
#ifdef CONFIG_SENSORS_BMI270_POLL
FAR struct bmi270_sensor_s *priv = NULL;
priv = (FAR struct bmi270_sensor_s *)lower;
priv->interval = *interval;
#endif
return OK;
}
#ifndef CONFIG_SENSORS_BMI270_POLL
/****************************************************************************
* Name: bmi270_fetch
****************************************************************************/
static int bmi270_fetch(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, FAR char *buffer,
size_t buflen)
{
FAR struct bmi270_sensor_s *priv = NULL;
int16_t data[3];
int ret = OK;
priv = (FAR struct bmi270_sensor_s *)lower;
switch (lower->type)
{
case SENSOR_TYPE_ACCELEROMETER:
{
struct sensor_accel accel;
bmi270_getregs(&priv->base, BMI270_DATA_8,
(FAR uint8_t *)data, 6);
accel.timestamp = sensor_get_timestamp();
accel.x = data[0] * priv->scale;
accel.y = data[1] * priv->scale;
accel.z = data[2] * priv->scale;
memcpy(buffer, &accel, sizeof(accel));
ret = sizeof(accel);
break;
}
case SENSOR_TYPE_GYROSCOPE:
{
struct sensor_gyro gyro;
bmi270_getregs(&priv->base, BMI270_DATA_14,
(FAR uint8_t *)data, 6);
gyro.timestamp = sensor_get_timestamp();
gyro.x = data[0] * priv->scale;
gyro.y = data[1] * priv->scale;
gyro.z = data[2] * priv->scale;
memcpy(buffer, &gyro, sizeof(gyro));
ret = sizeof(gyro);
break;
}
default:
{
ret = -EINVAL;
break;
}
}
return ret;
}
#endif
/****************************************************************************
* Name: bmi270_cotrol
****************************************************************************/
static int bmi270_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, int cmd,
unsigned long arg)
{
FAR struct bmi270_sensor_s *priv = NULL;
int ret = OK;
priv = (FAR struct bmi270_sensor_s *)lower;
switch (cmd)
{
/* Set full scale command */
case SNIOC_SET_SCALE_XL:
{
if (priv->lower.type == SENSOR_TYPE_GYROSCOPE)
{
ret = bmi270_gyro_scale(priv, arg);
}
else if (priv->lower.type == SENSOR_TYPE_ACCELEROMETER)
{
ret = bmi270_accel_scale(priv, arg);
}
break;
}
default:
{
snerr("ERROR: Unrecognized cmd: %d\n", cmd);
ret = -ENOTTY;
break;
}
}
return ret;
}
/****************************************************************************
* Name: bmi270_midpoint
*
* Description:
* Find the midpoint between two numbers.
*
****************************************************************************/
static uint32_t bmi270_midpoint(uint32_t a, uint32_t b)
{
return (uint32_t)(((uint64_t)a +
(uint64_t)b + (uint64_t)1) / (uint64_t)2);
}
/****************************************************************************
* Name: bmi270_accel_scale
****************************************************************************/
static int bmi270_accel_scale(FAR struct bmi270_sensor_s *priv,
uint8_t scale)
{
int ret = OK;
if (scale < bmi270_midpoint(2, 4))
{
bmi270_putreg8(&priv->base, BMI270_ACC_RANGE, ACCEL_RANGE_2G);
priv->scale = CONSTANTS_ONE_G / 16384.f;
}
else if (scale < bmi270_midpoint(4, 8))
{
bmi270_putreg8(&priv->base, BMI270_ACC_RANGE, ACCEL_RANGE_4G);
priv->scale = CONSTANTS_ONE_G / 8192.f;
}
else if (scale < bmi270_midpoint(8, 16))
{
bmi270_putreg8(&priv->base, BMI270_ACC_RANGE, ACCEL_RANGE_8G);
priv->scale = CONSTANTS_ONE_G / 4096.f;
}
else
{
bmi270_putreg8(&priv->base, BMI270_ACC_RANGE, ACCEL_RANGE_16G);
priv->scale = CONSTANTS_ONE_G / 2048.f;
}
return ret;
}
/****************************************************************************
* Name: bmi270_gyro_scale
****************************************************************************/
static int bmi270_gyro_scale(FAR struct bmi270_sensor_s *priv,
uint16_t scale)
{
int ret = OK;
if (scale < bmi270_midpoint(125, 250))
{
bmi270_putreg8(&priv->base, BMI270_GYR_RANGE, GYRO_RANGE_125);
priv->scale = (M_PI / 180.0f) * 125.f / 32768.f;
}
else if (scale < bmi270_midpoint(250, 500))
{
bmi270_putreg8(&priv->base, BMI270_GYR_RANGE, GYRO_RANGE_250);
priv->scale = (M_PI / 180.0f) * 250.f / 32768.f;
}
else if (scale < bmi270_midpoint(500, 1000))
{
bmi270_putreg8(&priv->base, BMI270_GYR_RANGE, GYRO_RANGE_500);
priv->scale = (M_PI / 180.0f) * 500.f / 32768.f;
}
else if (scale < bmi270_midpoint(1000, 2000))
{
bmi270_putreg8(&priv->base, BMI270_GYR_RANGE, GYRO_RANGE_1000);
priv->scale = (M_PI / 180.0f) * 1000.f / 32768.f;
}
else
{
bmi270_putreg8(&priv->base, BMI270_GYR_RANGE, GYRO_RANGE_2000);
priv->scale = (M_PI / 180.0f) * 2000.f / 32768.f;
}
return ret;
}
#ifdef CONFIG_SENSORS_BMI270_POLL
/****************************************************************************
* Name: bmi270_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 bmi270_accel_data(FAR struct bmi270_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 = buf[0] * priv->scale;
accel.y = buf[1] * priv->scale;
accel.z = buf[2] * priv->scale;
accel.temperature = 0;
lower->push_event(lower->priv, &accel, sizeof(accel));
}
/****************************************************************************
* Name: bmi270_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 bmi270_gyro_data(FAR struct bmi270_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 = buf[0] * priv->scale;
gyro.y = buf[1] * priv->scale;
gyro.z = buf[2] * priv->scale;
gyro.temperature = 0;
lower->push_event(lower->priv, &gyro, sizeof(gyro));
}
/****************************************************************************
* Name: bmi270_thread
*
* Description:
* Thread for performing interval measurement cycle and data read.
*
* Parameter:
* argc - Number opf arguments
* argv - Pointer to argument list
*
****************************************************************************/
static int bmi270_thread(int argc, FAR char **argv)
{
FAR struct bmi270_sensor_dev_s *dev
= (FAR struct bmi270_sensor_dev_s *)((uintptr_t)strtoul(argv[1], NULL,
16));
FAR struct bmi270_sensor_s *accel = &dev->priv[BMI270_ACCEL_IDX];
FAR struct bmi270_sensor_s *gyro = &dev->priv[BMI270_GYRO_IDX];
unsigned long min_interval;
int16_t data[6];
int ret;
while (true)
{
if ((!accel->enabled) && (!gyro->enabled))
{
/* Waiting to be woken up */
ret = nxsem_wait(&dev->run);
if (ret < 0)
{
continue;
}
}
/* Get data */
bmi270_getregs(&gyro->base, BMI270_DATA_8, (FAR uint8_t *)data, 12);
/* Read accel */
if (accel->enabled)
{
bmi270_accel_data(accel, data);
}
/* Read gyro */
if (gyro->enabled)
{
bmi270_gyro_data(gyro, &data[3]);
}
/* Sleeping thread before fetching the next sensor data */
min_interval = MIN(accel->interval, gyro->interval);
nxsig_usleep(min_interval);
}
return OK;
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: bmi270_register_uorb
*
* Description:
* Register the BMI270 IMU as sensor device
*
* Input Parameters:
* devno - Instance number for driver
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
#ifdef CONFIG_SENSORS_BMI270_I2C
int bmi270_register_uorb(int devno, FAR struct i2c_master_s *i2c,
uint8_t addr)
#else /* CONFIG_SENSORS_BMI270_SPI */
int bmi270_register_uorb(int devno, FAR struct spi_dev_s *spi)
#endif
{
FAR struct bmi270_sensor_dev_s *dev = NULL;
FAR struct bmi270_sensor_s *tmp = NULL;
#ifdef CONFIG_SENSORS_BMI270_POLL
FAR char *argv[2];
char arg1[32];
#endif
int ret = OK;
/* Initialize the device structure. */
dev = (FAR struct bmi270_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_BMI270_POLL
nxsem_init(&dev->run, 0, 0);
#endif
/* Accelerometer register */
tmp = &dev->priv[BMI270_ACCEL_IDX];
tmp->dev = dev;
#ifdef CONFIG_SENSORS_BMI270_I2C
tmp->base.i2c = i2c;
tmp->base.addr = addr;
#else
tmp->base.spi = spi;
#endif
tmp->lower.ops = &g_sensor_ops;
tmp->lower.type = SENSOR_TYPE_ACCELEROMETER;
tmp->lower.nbuffer = 1;
#ifdef CONFIG_SENSORS_BMI270_POLL
tmp->enabled = false;
tmp->interval = CONFIG_SENSORS_BMI270_POLL_INTERVAL;
#endif
ret = sensor_register(&tmp->lower, devno);
if (ret < 0)
{
snerr("sensor_register failed: %d\n", ret);
goto gyro_err;
}
/* Gyroscope register */
tmp = &dev->priv[BMI270_GYRO_IDX];
tmp->dev = dev;
#ifdef CONFIG_SENSORS_BMI270_I2C
tmp->base.i2c = i2c;
tmp->base.addr = addr;
#else
tmp->base.spi = spi;
#endif
tmp->lower.ops = &g_sensor_ops;
tmp->lower.type = SENSOR_TYPE_GYROSCOPE;
tmp->lower.nbuffer = 1;
#ifdef CONFIG_SENSORS_BMI270_POLL
tmp->enabled = false;
tmp->interval = CONFIG_SENSORS_BMI270_POLL_INTERVAL;
#endif
ret = sensor_register(&tmp->lower, devno);
if (ret < 0)
{
snerr("sensor_register failed: %d\n", ret);
goto gyro_err;
}
#ifdef CONFIG_SENSORS_BMI270_SPI
/* BMI270 detects communication bus is SPI by rising edge of CS. */
bmi270_getreg8(&tmp->base, 0x00);
bmi270_getreg8(&tmp->base, 0x00);
up_udelay(200);
#endif
/* Initialization sequence */
ret = bmi270_init_seq(&tmp->base);
if (ret != 0)
{
return ret;
}
/* Set default scale */
bmi270_accel_scale(&dev->priv[BMI270_ACCEL_IDX], 2);
bmi270_gyro_scale(&dev->priv[BMI270_GYRO_IDX], 2000);
#ifdef CONFIG_SENSORS_BMI270_POLL
/* Create thread for polling sensor data */
snprintf(arg1, 16, "%p", dev);
argv[0] = arg1;
argv[1] = NULL;
ret = kthread_create("bmi270_thread", SCHED_PRIORITY_DEFAULT,
CONFIG_SENSORS_BMI270_THREAD_STACKSIZE,
bmi270_thread,
argv);
if (ret < 0)
{
goto thr_err;
}
#endif
return ret;
#ifdef CONFIG_SENSORS_BMI270_POLL
thr_err:
#endif
#ifdef AUX_MAG_SUPPORTED
sensor_unregister(&dev->priv[BMI270_MAG_IDX].lower, devno);
mag_err:
#endif
sensor_unregister(&dev->priv[BMI270_GYRO_IDX].lower, devno);
gyro_err:
sensor_unregister(&dev->priv[BMI270_ACCEL_IDX].lower, devno);
kmm_free(dev);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink