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

1158 lines
30 KiB
C
Raw Blame History

/****************************************************************************
* drivers/sensors/vl53l1x.c
* Character driver for the ST vl53l1x distance.
*
* Copyright (C) 2019 Acutronics Robotics
* Author: Acutronics Robotics (Juan Flores Muñoz) <juan@erlerobotics.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdlib.h>
#include <fixedmath.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/kmalloc.h>
#include <nuttx/signal.h>
#include <nuttx/fs/fs.h>
#include <nuttx/i2c/i2c_master.h>
#include <nuttx/sensors/vl53l1x.h>
#include <nuttx/sensors/ioctl.h>
#include <nuttx/random.h>
#if defined(CONFIG_I2C) && defined(CONFIG_SENSORS_VL53L1X)
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define VL53L1X_FREQ 100000
#define VL53L1X_ADDR 0x29
/****************************************************************************
* Private Type Definitions
****************************************************************************/
struct vl53l1x_dev_s
{
FAR struct i2c_master_s *i2c; /* I2C interface */
uint8_t addr; /* VL53L0X I2C address */
int freq; /* VL53L0X Frequency */
};
/****************************************************************************
* Private Data
****************************************************************************/
static const uint8_t g_vl51l1x_default_config[] =
{
0x00,
0x00,
0x00,
0x01,
0x02,
0x00,
0x02,
0x08,
0x00,
0x08,
0x10,
0x01,
0x01,
0x00,
0x00,
0x00,
0x00,
0xff,
0x00,
0x0f,
0x00,
0x00,
0x00,
0x00,
0x00,
0x20,
0x0b,
0x00,
0x00,
0x02,
0x0a,
0x21,
0x00,
0x00,
0x05,
0x00,
0x00,
0x00,
0x00,
0xc8,
0x00,
0x00,
0x38,
0xff,
0x01,
0x00,
0x08,
0x00,
0x00,
0x01,
0xdb,
0x0f,
0x01,
0xf1,
0x0d,
0x01,
0x68,
0x00,
0x80,
0x08,
0xb8,
0x00,
0x00,
0x00,
0x00,
0x0f,
0x89,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
0x01,
0x0f,
0x0d,
0x0e,
0x0e,
0x00,
0x00,
0x02,
0xc7,
0xff,
0x9b,
0x00,
0x00,
0x00,
0x01,
0x00,
0x00
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* I2C operations */
static uint8_t vl53l1x_getreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static uint16_t vl53l1x_getreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static uint32_t vl53l1x_getreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static void vl53l1x_putreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint8_t regval);
static void vl53l1x_putreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint16_t regval);
static void vl53l1x_putreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint32_t regval);
/* Basic driver operations */
static void vl53l1x_startranging(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_stopranging(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_sensorinit(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_getdistance(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *distance);
/* Set data operations */
static void vl53l1x_settimingbudget(FAR struct vl53l1x_dev_s *priv,
uint16_t ms_value);
static void vl53l1x_setdistancemode(FAR struct vl53l1x_dev_s *priv,
uint16_t mode);
/* Get data operations */
static void vl53l1x_gettimingbudget(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *ms_value);
static void vl53l1x_getdistancemode(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *dm);
static void vl53l1x_getid(FAR struct vl53l1x_dev_s *priv, FAR uint16_t *id);
static void vl53l1x_getoffset(FAR struct vl53l1x_dev_s *priv,
FAR int16_t *offset);
static void vl53l1x_getintstate(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *state);
/* Other operations */
static void vl53l1x_clearint(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_dataready(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *dataready);
static void vl53l1x_tempupdate(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_calibrateoffset(FAR struct vl53l1x_dev_s *priv,
uint16_t target_distance,
FAR int16_t *offset);
/* Character driver methods */
static void vl53l1x_read(FAR struct file *filep, FAR char *buffer,
size_t buflen);
static ssize_t vl53l1x_write(FAR struct file *filep, FAR const char *buffer,
size_t buflen);
static void vl53l1x_ioctl(FAR struct file *filep, int cmd, uint16_t arg);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct file_operations g_vl53l1xfops =
{
NULL, /* open */
NULL, /* close */
vl53l1x_read, /* read */
vl53l1x_write, /* write */
NULL, /* seek */
vl53l1x_ioctl, /* ioctl */
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: vl53l1x_SensorInit
*
* Description:
* This function loads the 135 bytes default values to initialize the
* sensor.
*
****************************************************************************/
static void vl53l1x_sensorinit(FAR struct vl53l1x_dev_s *priv)
{
uint8_t addr = 0x00;
uint8_t tmp = 0;
for (addr = 0x2d; addr <= 0x87; addr++)
{
vl53l1x_putreg8(priv, addr, g_vl51l1x_default_config[addr - 0x2d]);
}
vl53l1x_startranging(priv);
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_clearint(priv);
vl53l1x_stopranging(priv);
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x09);
vl53l1x_putreg8(priv, 0x0b, 0);
}
/****************************************************************************
* Name: vl53l1x_clearint
*
* Description:
* This function clears the interrupt, to be called after a ranging data
* reading to arm the interrupt for the next data ready event.
*
****************************************************************************/
static void vl53l1x_clearint(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, INTERRUPT_CLEAR, 0x01);
}
/****************************************************************************
* Name: vl53l1x_startranging
*
* Description:
* This function starts the ranging distance operation. The ranging ops
* is continuous. The clear interrupt has to be done after each get data to
* allow the interrupt to raise when the next data is ready.
*
****************************************************************************/
static void vl53l1x_startranging(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, SYSTEM_MODE, 0x40);
}
/****************************************************************************
* Name: vl53l1x_StopRanging
*
* Description:
* This function stops the ranging.
*
****************************************************************************/
static void vl53l1x_stopranging(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, SYSTEM_MODE, 0x00);
}
/****************************************************************************
* Name: vl53l1x_dataready
*
* Description:
* This function checks if the new ranging data is available by polling the
* dedicated register.
*
****************************************************************************/
static void vl53l1x_dataready(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *dataready)
{
uint8_t temp;
uint8_t intpol;
temp = vl53l1x_getreg8(priv, GPIO_STATUS);
vl53l1x_getintstate(priv, &intpol);
/* Read in the register to check if a new value is available */
if ((temp & 1) == intpol)
{
*dataready = 1;
}
else
{
*dataready = 0;
}
}
/****************************************************************************
* Name: vl53l1x_getinterruptstate
*
* Description:
* This function returns the current interrupt polarity.
*
****************************************************************************/
static void vl53l1x_getintstate(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *state)
{
uint8_t temp;
temp = vl53l1x_getreg8(priv, GPIO_MUX_CTRL);
temp = temp & 0x10;
*state = !(temp >> 4);
}
/****************************************************************************
* Name: VL53L1X_GetTimingBudgetInMs
*
* Description:
* This function returns the timing budget in ms.
*
****************************************************************************/
static void vl53l1x_gettimingbudget(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *ms_value)
{
uint16_t temp;
temp = vl53l1x_getreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI);
switch (temp)
{
case 0x001d:
*ms_value = 15;
break;
case 0x0051:
case 0x001e:
*ms_value = 20;
break;
case 0x00d6:
case 0x0060:
*ms_value = 33;
break;
case 0x1ae:
case 0x00ad:
*ms_value = 50;
break;
case 0x02e1:
case 0x01cc:
*ms_value = 100;
break;
case 0x03e1:
case 0x02d9:
*ms_value = 200;
break;
case 0x0591:
case 0x048f:
*ms_value = 500;
break;
default:
*ms_value = 0;
break;
}
}
/****************************************************************************
* Name: vl53l1x_settimingbudget
*
* Description:
* This function programs the timing budget in ms.
*
****************************************************************************/
static void vl53l1x_settimingbudget(FAR struct vl53l1x_dev_s *priv,
uint16_t ms_value)
{
uint16_t dm;
vl53l1x_getdistancemode(priv, &dm);
/* Short Distance */
if (dm == 1)
{
switch (ms_value)
{
case 15:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x01d);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0027);
}
break;
case 20:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0051);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 33:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x00d6);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 50:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x1ae);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x01e8);
}
break;
case 100:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x02e1);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0388);
}
break;
case 200:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x03e1);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0496);
}
break;
case 500:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0591);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x05c1);
}
break;
default:
break;
}
}
else
{
switch (ms_value)
{
case 20:
{
vl53l1x_putreg16(priv, PHASECAL_TIMEOUT_MACRO, 0x001e);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0022);
}
break;
case 33:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0060);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 50:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x00ad);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x00c6);
}
break;
case 100:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x01cc);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x01ea);
}
break;
case 200:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x02d9);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x02f8);
}
break;
case 500:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x048f);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x04a4);
}
break;
default:
break;
}
}
}
/****************************************************************************
* Name: vl53l1x_setdistancemode
*
* Description:
* This function programs the distance mode (1=short, 2=long(default)).
* Short mode max distance is limited to 1.3 m but better ambient immunity.
* Long mode can range up to 4 m in the dark with 200 ms timing budget.
*
****************************************************************************/
static void vl53l1x_setdistancemode(FAR struct vl53l1x_dev_s *priv,
uint16_t dm)
{
uint16_t tb;
vl53l1x_gettimingbudget(priv, &tb);
switch (dm)
{
case 1:
{
vl53l1x_putreg8(priv, PHASECAL_TIMEOUT_MACRO, 0x14);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_A, 0x07);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_B, 0x05);
vl53l1x_putreg8(priv, RANGE_CFG_VALID_PHASE, 0x38);
vl53l1x_putreg16(priv, SD_CFG_WOI_SD0, 0x0705);
vl53l1x_putreg16(priv, SD_CFG_INIT_PHASE, 0x0606);
}
break;
case 2:
{
vl53l1x_putreg8(priv, PHASECAL_TIMEOUT_MACRO, 0x0a);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_A, 0x0f);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_B, 0x0d);
vl53l1x_putreg8(priv, RANGE_CFG_VALID_PHASE, 0xb8);
vl53l1x_putreg16(priv, SD_CFG_WOI_SD0, 0x0f0d);
vl53l1x_putreg16(priv, SD_CFG_INIT_PHASE, 0x0e0e);
}
break;
default:
break;
}
vl53l1x_settimingbudget(priv, tb);
}
/****************************************************************************
* Name: vl53l1x_getdistancemode
*
* Description:
* This function returns the current distance mode (1=short, 2=long).
*
****************************************************************************/
static void vl53l1x_getdistancemode(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *dm)
{
uint8_t mode_read;
mode_read = vl53l1x_getreg8(priv, PHASECAL_TIMEOUT_MACRO);
if (mode_read == 0x14)
{
*dm = 1;
}
if (mode_read == 0x0a)
{
*dm = 2;
}
}
/****************************************************************************
* Name: vl53l1x_getid
*
* Description:
* This function returns the sensor id, sensor Id must be 0xeeac.
*
****************************************************************************/
static void vl53l1x_getid(FAR struct vl53l1x_dev_s *priv, FAR uint16_t *id)
{
uint16_t tmp = 0;
tmp = vl53l1x_getreg16(priv, VL53L1_GET_ID);
*id = tmp;
}
/****************************************************************************
* Name: vl53l1x_GetDistance
*
* Description:
* This function returns the distance measured by the sensor in mm.
*
****************************************************************************/
static void vl53l1x_getdistance(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *distance)
{
uint16_t tmp;
tmp = vl53l1x_getreg16(priv, VL53L1_GET_DISTANCE);
*distance = tmp;
}
/****************************************************************************
* Name: vl53l1x_getoffset
*
* Description:
* This function returns the programmed offset correction value in mm.
*
****************************************************************************/
static void vl53l1x_getoffset(FAR struct vl53l1x_dev_s *priv,
FAR int16_t *offset)
{
uint16_t temp;
temp = vl53l1x_getreg16(priv, RANGE_OFFSET_MM);
temp = temp << 3;
temp = temp >> 5;
*offset = (int16_t) (temp);
}
/****************************************************************************
* Name: vl53l1x_tempupdate
*
* Description:
* This function performs the temperature calibration. It is recommended to
* call this function any time the temperature might have changed by more
* than 8 deg C without sensor ranging activity for an extended period.
*
****************************************************************************/
static void vl53l1x_tempupdate(FAR struct vl53l1x_dev_s *priv)
{
uint8_t tmp = 0;
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x81);
vl53l1x_putreg8(priv, 0x0b, 0x92);
vl53l1x_startranging(priv);
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_clearint(priv);
vl53l1x_stopranging(priv);
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x09);
vl53l1x_putreg8(priv, 0x0b, 0);
}
/****************************************************************************
* Name: vl53l1x_calibrateoffset
*
* Description:
* The function returns the offset value found and programs the offset
* compensation into the device.
*
****************************************************************************/
static void vl53l1x_calibrateoffset(FAR struct vl53l1x_dev_s *priv,
uint16_t target_distance,
FAR int16_t *offset)
{
uint8_t i = 0;
uint8_t tmp;
int16_t average_distance = 0;
uint16_t distance;
vl53l1x_putreg16(priv, RANGE_OFFSET_MM, 0x0);
vl53l1x_putreg16(priv, INNER_OFFSET_MM, 0x0);
vl53l1x_putreg16(priv, OUTER_OFFSET_MM, 0x0);
vl53l1x_startranging(priv);
for (i = 0; i < 50; i++)
{
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_getdistance(priv, &distance);
vl53l1x_clearint(priv);
average_distance = average_distance + distance;
}
vl53l1x_stopranging(priv);
average_distance = average_distance / 50;
*offset = target_distance - average_distance;
vl53l1x_putreg16(priv, RANGE_OFFSET_MM, *offset *4);
}
/****************************************************************************
* Name: vl53l1x_getreg8
*
* Description:
* Read from an 8-bit vl53l1x register
*
****************************************************************************/
static uint8_t vl53l1x_getreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint8_t regval = 0;
uint16_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Write the register address */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&regaddr, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read the register value */
ret = i2c_read(priv->i2c, &config, &regval, 1);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
return regval;
}
/****************************************************************************
* Name: bmp180_getreg16
*
* Description:
* Read two 8-bit from a BMP180 register
*
****************************************************************************/
static uint16_t vl53l1x_getreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint16_t msb;
uint16_t lsb;
uint16_t regval = 0;
uint8_t reg_addr_aux[2];
uint8_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Split the I2C direction */
reg_addr_aux[0] = (regaddr >> 8) & 0xff;
reg_addr_aux[1] = regaddr;
/* Register to read */
sninfo("Reg %02x %\n", reg_addr_aux[0], reg_addr_aux[1]);
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&reg_addr_aux, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read register */
ret = i2c_read(priv->i2c, &config, (FAR uint8_t *) & regval, 2);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
/* MSB and LSB are inverted */
msb = (regval & 0xff);
lsb = (regval & 0xff00) >> 8;
regval = (msb << 8) | lsb;
return regval;
}
/****************************************************************************
* Name: vl53l1x_getreg32
*
* Description:
* Read 4 8-bit from a vl53l1x register
*
****************************************************************************/
static uint32_t vl53l1x_getreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint16_t byte[4];
uint32_t regval = 0;
uint8_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Register to read */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&regaddr, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read register */
ret = i2c_read(priv->i2c, &config, (FAR uint8_t *) & regval, 4);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
/* The bytes are in the opposite order of importance */
byte[0] = (regval & 0xff);
byte[1] = (regval & 0xff00) >> 8;
byte[2] = (regval & 0xffff00) >> 16;
byte[3] = (regval & 0xffffff00) >> 24;
regval = (byte[0] << 24) | (byte[1] << 16) | (byte[2] << 8) | byte[3];
return regval;
}
/****************************************************************************
* Name: vl53l1x_putreg8
*
* Description:
* Write to an 8-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg8(FAR struct vl53l1x_dev_s *priv, uint16_t regaddr,
uint8_t regval)
{
struct i2c_config_s config;
uint8_t data[3];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 3);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_putreg16
*
* Description:
* Write to an 16-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint16_t regval)
{
struct i2c_config_s config;
uint8_t data[4];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = (regval >> 8) & 0xff;
data[3] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 4);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_putreg32
*
* Description:
* Write to an 32-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint32_t regval)
{
struct i2c_config_s config;
uint8_t data[7];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = (regval >> 24) & 0xff;
data[4] = (regval >> 16) & 0xff;
data[5] = (regval >> 8) & 0xff;
data[6] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 7);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_read
****************************************************************************/
static void vl53l1x_read(FAR struct file *filep, FAR char *buffer,
size_t buflen)
{
FAR struct inode *inode = filep->f_inode;
FAR struct vl53l1x_dev_s *priv = inode->i_private;
FAR uint16_t *aux = (FAR uint16_t *) buffer;
vl53l1x_startranging(priv);
vl53l1x_getdistance(priv, aux);
vl53l1x_stopranging(priv);
}
/****************************************************************************
* Name: vl53l1x_write
****************************************************************************/
static ssize_t vl53l1x_write(FAR struct file *filep, FAR const char *buffer,
size_t buflen)
{
return -ENOSYS;
}
/****************************************************************************
* Name: vl53l1x_ioctl
****************************************************************************/
static void vl53l1x_ioctl(FAR struct file *filep, int cmd, uint16_t arg)
{
FAR struct inode *inode = filep->f_inode;
FAR struct vl53l1x_dev_s *priv = inode->i_private;
switch (cmd)
{
case SNIOC_DISTANCESHORT:
{
sninfo("Set distance up to 1.3M\n");
vl53l1x_setdistancemode(priv, 1);
}
break;
case SNIOC_DISTANCELONG:
{
sninfo("Set distance up to 4M\n");
vl53l1x_setdistancemode(priv, 2);
}
break;
case SNIOC_CALIBRATE:
{
sninfo("Calibrating distance\n");
int16_t offset;
vl53l1x_getoffset(priv, (FAR int16_t *)&offset);
vl53l1x_calibrateoffset(priv, arg, (FAR int16_t *)&offset);
}
break;
case SNIOC_TEMPUPDATE:
{
sninfo("Recalculating due to temperature change\n");
vl53l1x_tempupdate(priv);
}
break;
}
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: vl53l1x_register
*
* Description:
* Register the vl53l1x character device as 'devpath'
*
* Input Parameters:
* devpath - The full path to the driver to register. E.g., "/dev/tof"
* i2c - An instance of the I2C interface to use to communicate with
* vl53l1x TOF
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int vl53l1x_register(FAR const char *devpath, FAR struct i2c_master_s *i2c)
{
FAR struct vl53l1x_dev_s *priv;
int ret = 0;
uint16_t id;
/* Initialize the vl53l1x device structure */
priv = kmm_malloc(sizeof(struct vl53l1x_dev_s));
if (!priv)
{
snerr("ERROR: Failed to allocate instance\n");
return -ENOMEM;
}
priv->i2c = i2c;
priv->addr = VL53L1X_ADDR;
priv->freq = VL53L1X_FREQ;
vl53l1x_sensorinit(priv);
vl53l1x_getid(priv, &id);
if (id != 0xeacc)
{
snerr("ERROR: Failed sensor ID %04x\n", id);
kmm_free(priv);
return 0;
}
register_driver(devpath, &g_vl53l1xfops, 0666, priv);
if (ret < 0)
{
snerr("ERROR: Failed to register driver: %d\n", ret);
kmm_free(priv);
return 0;
}
sninfo("vl53l1x driver loaded successfully!\n");
return 1;
}
#endif /* CONFIG_I2C && CONFIG_SENSORS_VL53L1X */
Reference in New Issue View Git Blame Copy Permalink