657 lines
17 KiB
C
657 lines
17 KiB
C
|
/**************************************************************************//**
|
||
|
|
*
|
||
|
|
* @copyright (C) 2019 Nuvoton Technology Corp. All rights reserved.
|
||
|
|
*
|
||
|
|
* SPDX-License-Identifier: Apache-2.0
|
||
|
|
*
|
||
|
|
* Change Logs:
|
||
|
|
* Date Author Notes
|
||
|
|
* 2020-1-16 Wayne First version
|
||
|
|
*
|
||
|
|
******************************************************************************/
|
||
|
|
|
||
|
|
#include <rtconfig.h>
|
||
|
|
|
||
|
|
#if defined(NU_PKG_USING_BMX055)
|
||
|
|
|
||
|
|
#include <string.h>
|
||
|
|
#include "sensor_bmx055.h"
|
||
|
|
|
||
|
|
#define DBG_ENABLE
|
||
|
|
#define DBG_LEVEL DBG_LOG
|
||
|
|
#define DBG_SECTION_NAME "sensor.bmx055"
|
||
|
|
#define DBG_COLOR
|
||
|
|
#include <rtdbg.h>
|
||
|
|
|
||
|
|
struct bmx055
|
||
|
|
{
|
||
|
|
/* Structure used for Magnetometer */
|
||
|
|
struct bmm050_t mag;
|
||
|
|
|
||
|
|
/* Structure used for read the mag xyz data*/
|
||
|
|
struct bmm050_mag_data_s16_t mag_data;
|
||
|
|
|
||
|
|
/* Structure used for Accelerometer */
|
||
|
|
struct bma2x2_t accel;
|
||
|
|
|
||
|
|
/* Structure used to read accel xyz and temperature data*/
|
||
|
|
struct bma2x2_accel_data_temp accel_xyzt;
|
||
|
|
|
||
|
|
/* Structure used for Gyroscope */
|
||
|
|
struct bmg160_t gyro;
|
||
|
|
|
||
|
|
/* structure used to read gyro xyz and interrupt status*/
|
||
|
|
struct bmg160_data_t gyro_xyzi;
|
||
|
|
|
||
|
|
} ;
|
||
|
|
typedef struct bmx055 *bmx055_t;
|
||
|
|
|
||
|
|
struct bmx055_param_pair
|
||
|
|
{
|
||
|
|
int val;
|
||
|
|
int reg;
|
||
|
|
};
|
||
|
|
typedef struct bmx055_param_pair *bmx055_param_pair_t;
|
||
|
|
|
||
|
|
static const struct bmx055_param_pair accel_ranges[] =
|
||
|
|
{
|
||
|
|
{2000, BMA2x2_RANGE_2G},
|
||
|
|
{4000, BMA2x2_RANGE_4G},
|
||
|
|
{8000, BMA2x2_RANGE_8G},
|
||
|
|
{16000, BMA2x2_RANGE_16G},
|
||
|
|
};
|
||
|
|
|
||
|
|
static const struct bmx055_param_pair gyro_ranges[] =
|
||
|
|
{
|
||
|
|
{125, BMG160_RANGE_125},
|
||
|
|
{250, BMG160_RANGE_250},
|
||
|
|
{500, BMG160_RANGE_500},
|
||
|
|
{1000, BMG160_RANGE_1000},
|
||
|
|
{2000, BMG160_RANGE_2000},
|
||
|
|
};
|
||
|
|
|
||
|
|
static const struct bmx055_param_pair accel_odr[] =
|
||
|
|
{
|
||
|
|
{8, BMA2x2_BW_7_81HZ},
|
||
|
|
{16, BMA2x2_BW_15_63HZ},
|
||
|
|
{32, BMA2x2_BW_31_25HZ},
|
||
|
|
{63, BMA2x2_BW_62_50HZ},
|
||
|
|
{125, BMA2x2_BW_125HZ},
|
||
|
|
{250, BMA2x2_BW_250HZ},
|
||
|
|
{500, BMA2x2_BW_500HZ},
|
||
|
|
{1000, BMA2x2_BW_1000HZ},
|
||
|
|
};
|
||
|
|
|
||
|
|
static const struct bmx055_param_pair gyro_odr[] =
|
||
|
|
{
|
||
|
|
{12, BMG160_BW_12_HZ},
|
||
|
|
{23, BMG160_BW_23_HZ},
|
||
|
|
{32, BMG160_BW_32_HZ},
|
||
|
|
{47, BMG160_BW_47_HZ},
|
||
|
|
{64, BMG160_BW_64_HZ},
|
||
|
|
{116, BMG160_BW_116_HZ},
|
||
|
|
{230, BMG160_BW_230_HZ},
|
||
|
|
{500, BMG160_BW_500_HZ},
|
||
|
|
};
|
||
|
|
|
||
|
|
static const struct bmx055_param_pair mag_odr[] =
|
||
|
|
{
|
||
|
|
{2, BMM050_DR_02HZ},
|
||
|
|
{6, BMM050_DR_06HZ},
|
||
|
|
{8, BMM050_DR_08HZ},
|
||
|
|
{15, BMM050_DR_15HZ},
|
||
|
|
{20, BMM050_DR_20HZ},
|
||
|
|
{25, BMM050_DR_25HZ},
|
||
|
|
{30, BMM050_DR_30HZ},
|
||
|
|
};
|
||
|
|
|
||
|
|
static struct bmx055 g_sbmx055;
|
||
|
|
static struct rt_i2c_bus_device *i2c_bus_dev;
|
||
|
|
static uint8_t g_u8Range_accel = 0;
|
||
|
|
static uint8_t g_u8Range_gyro = 0;
|
||
|
|
|
||
|
|
static int8_t bmx055_i2c_write_reg(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_data, uint8_t len)
|
||
|
|
{
|
||
|
|
struct rt_i2c_msg msgs[2];
|
||
|
|
|
||
|
|
msgs[0].addr = dev_id; /* Slave address */
|
||
|
|
msgs[0].flags = RT_I2C_WR; /* Write flag */
|
||
|
|
msgs[0].buf = (rt_uint8_t *)®_addr; /* Slave register address */
|
||
|
|
msgs[0].len = 1; /* Number of bytes sent */
|
||
|
|
|
||
|
|
msgs[1].addr = dev_id; /* Slave address */
|
||
|
|
msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; /* Read flag without NO_START */
|
||
|
|
msgs[1].buf = (rt_uint8_t *)reg_data; /* Read data pointer */
|
||
|
|
msgs[1].len = len; /* Number of bytes read */
|
||
|
|
|
||
|
|
if (rt_i2c_transfer(i2c_bus_dev, &msgs[0], 2) != 2)
|
||
|
|
{
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int8_t bmx055_i2c_read_reg(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_data, uint8_t len)
|
||
|
|
{
|
||
|
|
struct rt_i2c_msg msgs[3];
|
||
|
|
|
||
|
|
msgs[0].addr = dev_id; /* Slave address */
|
||
|
|
msgs[0].flags = RT_I2C_WR; /* Write flag */
|
||
|
|
msgs[0].buf = (rt_uint8_t *)®_addr; /* Slave register address */
|
||
|
|
msgs[0].len = 1; /* Number of bytes sent */
|
||
|
|
|
||
|
|
|
||
|
|
msgs[1].addr = dev_id; /* Slave address */
|
||
|
|
msgs[1].flags = RT_I2C_RD ; /* Read flag */
|
||
|
|
msgs[1].buf = (rt_uint8_t *)®_data[0]; /* Read data pointer */
|
||
|
|
msgs[1].len = len;
|
||
|
|
|
||
|
|
if (rt_i2c_transfer(i2c_bus_dev, &msgs[0], 2) != 2)
|
||
|
|
{
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
void bmx055_delay_ms(BMM050_MDELAY_DATA_TYPE ms)
|
||
|
|
{
|
||
|
|
rt_thread_mdelay(ms);
|
||
|
|
}
|
||
|
|
|
||
|
|
static int8_t bmx055_init_mag(struct bmm050_t *mag)
|
||
|
|
{
|
||
|
|
int8_t com_rslt = 0;
|
||
|
|
u8 v_data_rate_u8 = BMM050_INIT_VALUE;
|
||
|
|
|
||
|
|
mag->dev_addr = BMM050_I2C_ADDRESS;
|
||
|
|
mag->bus_write = bmx055_i2c_write_reg;
|
||
|
|
mag->bus_read = bmx055_i2c_read_reg;
|
||
|
|
mag->delay_msec = bmx055_delay_ms;
|
||
|
|
|
||
|
|
com_rslt = bmm050_init(mag);
|
||
|
|
com_rslt += bmm050_set_functional_state(BMM050_NORMAL_MODE);
|
||
|
|
com_rslt += bmm050_set_data_rate(BMM050_DATA_RATE_30HZ);
|
||
|
|
com_rslt += bmm050_get_data_rate(&v_data_rate_u8);
|
||
|
|
|
||
|
|
return com_rslt;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int8_t bmx055_init_accel(struct bma2x2_t *accel)
|
||
|
|
{
|
||
|
|
int8_t com_rslt = 0;
|
||
|
|
uint8_t bandwidth = 0x08; // bandwidth of 7.81Hz
|
||
|
|
|
||
|
|
accel->dev_addr = BMA2x2_I2C_ADDR1;
|
||
|
|
accel->bus_write = bmx055_i2c_write_reg;
|
||
|
|
accel->bus_read = bmx055_i2c_read_reg;
|
||
|
|
accel->delay_msec = bmx055_delay_ms;
|
||
|
|
|
||
|
|
com_rslt = bma2x2_init(accel);
|
||
|
|
com_rslt += bma2x2_set_power_mode(BMA2x2_MODE_NORMAL);
|
||
|
|
com_rslt += bma2x2_set_bw(bandwidth);
|
||
|
|
com_rslt += bma2x2_get_range(&g_u8Range_accel);
|
||
|
|
|
||
|
|
return com_rslt;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int8_t bmx055_init_gyro(struct bmg160_t *gyro)
|
||
|
|
{
|
||
|
|
int8_t com_rslt = 0;
|
||
|
|
|
||
|
|
gyro->dev_addr = BMG160_I2C_ADDR1;
|
||
|
|
gyro->bus_write = bmx055_i2c_write_reg;
|
||
|
|
gyro->bus_read = bmx055_i2c_read_reg;
|
||
|
|
gyro->delay_msec = bmx055_delay_ms;
|
||
|
|
|
||
|
|
com_rslt = bmg160_init(gyro);
|
||
|
|
com_rslt += bmg160_set_power_mode(BMG160_MODE_NORMAL);
|
||
|
|
com_rslt += bmg160_set_bw(C_BMG160_BW_230HZ_U8X);
|
||
|
|
com_rslt += bmg160_get_range_reg(&g_u8Range_gyro);
|
||
|
|
|
||
|
|
return com_rslt;
|
||
|
|
}
|
||
|
|
|
||
|
|
static double get_mg_value(int32_t i32AccelVal)
|
||
|
|
{
|
||
|
|
switch (g_u8Range_accel)
|
||
|
|
{
|
||
|
|
case BMA2x2_RANGE_2G:
|
||
|
|
return (double)i32AccelVal * 0.98; //res: 0.98,
|
||
|
|
case BMA2x2_RANGE_4G:
|
||
|
|
return (double)i32AccelVal * 1.95;
|
||
|
|
case BMA2x2_RANGE_8G:
|
||
|
|
return (double)i32AccelVal * 3.91;
|
||
|
|
case BMA2x2_RANGE_16G:
|
||
|
|
return (double)i32AccelVal * 7.81;
|
||
|
|
}
|
||
|
|
return 0.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
static double get_mdps_value(int32_t i32AccelVal)
|
||
|
|
{
|
||
|
|
switch (g_u8Range_gyro)
|
||
|
|
{
|
||
|
|
case BMG160_RANGE_125:
|
||
|
|
return (double)i32AccelVal * 3.8;
|
||
|
|
case BMG160_RANGE_250:
|
||
|
|
return (double)i32AccelVal * 7.6;
|
||
|
|
case BMG160_RANGE_500:
|
||
|
|
return (double)i32AccelVal * 15.3;
|
||
|
|
case BMG160_RANGE_1000:
|
||
|
|
return (double)i32AccelVal * 30.5;
|
||
|
|
case BMG160_RANGE_2000:
|
||
|
|
return (double)i32AccelVal * 61;
|
||
|
|
}
|
||
|
|
return 0.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
static rt_size_t bmx055_fetch_data(rt_sensor_t sensor, void *buf, rt_size_t len)
|
||
|
|
{
|
||
|
|
struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
|
||
|
|
switch (sensor->info.type)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CLASS_ACCE:
|
||
|
|
bma2x2_read_accel_xyzt(&g_sbmx055.accel_xyzt);
|
||
|
|
data->type = RT_SENSOR_CLASS_ACCE;
|
||
|
|
|
||
|
|
/* Report mg */
|
||
|
|
data->data.acce.x = (int32_t)get_mg_value(g_sbmx055.accel_xyzt.x);
|
||
|
|
data->data.acce.y = (int32_t)get_mg_value(g_sbmx055.accel_xyzt.y);
|
||
|
|
data->data.acce.z = (int32_t)get_mg_value(g_sbmx055.accel_xyzt.z);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_GYRO:
|
||
|
|
bmg160_get_data_XYZI(&g_sbmx055.gyro_xyzi);
|
||
|
|
data->type = RT_SENSOR_CLASS_GYRO;
|
||
|
|
|
||
|
|
/* Report mdps */
|
||
|
|
data->data.gyro.x = (int32_t)get_mdps_value(g_sbmx055.gyro_xyzi.datax);
|
||
|
|
data->data.gyro.y = (int32_t)get_mdps_value(g_sbmx055.gyro_xyzi.datay);
|
||
|
|
data->data.gyro.z = (int32_t)get_mdps_value(g_sbmx055.gyro_xyzi.dataz);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_MAG:
|
||
|
|
bmm050_read_mag_data_XYZ(&g_sbmx055.mag_data);
|
||
|
|
data->type = RT_SENSOR_CLASS_MAG;
|
||
|
|
|
||
|
|
/* Report mquass */
|
||
|
|
data->data.mag.x = g_sbmx055.mag_data.datax;
|
||
|
|
data->data.mag.y = g_sbmx055.mag_data.datay;
|
||
|
|
data->data.mag.z = g_sbmx055.mag_data.dataz;
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
data->timestamp = rt_sensor_get_ts();
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int find_param_index(const int eng_val, const struct bmx055_param_pair *pairs, const int size)
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
|
||
|
|
while (i < size)
|
||
|
|
{
|
||
|
|
if (eng_val <= pairs[i].val)
|
||
|
|
return i;
|
||
|
|
i++;
|
||
|
|
}
|
||
|
|
return i - 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
static rt_err_t bmx055_getid(rt_sensor_t sensor, rt_uint8_t *pu8)
|
||
|
|
{
|
||
|
|
switch (sensor->info.type)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CLASS_ACCE:
|
||
|
|
*pu8 = g_sbmx055.accel.chip_id;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_GYRO:
|
||
|
|
*pu8 = g_sbmx055.gyro.chip_id;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_MAG:
|
||
|
|
*pu8 = g_sbmx055.mag.company_id;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static rt_err_t bmx055_set_power(rt_sensor_t sensor, rt_uint8_t power_mode)
|
||
|
|
{
|
||
|
|
uint8_t power_ctr;
|
||
|
|
|
||
|
|
switch (sensor->info.type)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CLASS_ACCE:
|
||
|
|
{
|
||
|
|
switch (power_mode)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_POWER_DOWN:
|
||
|
|
power_ctr = BMA2x2_MODE_STANDBY;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_POWER_NORMAL:
|
||
|
|
power_ctr = BMA2x2_MODE_NORMAL;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_POWER_LOW:
|
||
|
|
power_ctr = BMA2x2_MODE_LOWPOWER1;
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bma2x2_set_power_mode(power_ctr) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_GYRO:
|
||
|
|
{
|
||
|
|
switch (power_mode)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_POWER_DOWN:
|
||
|
|
power_ctr = BMG160_MODE_DEEPSUSPEND;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_POWER_NORMAL:
|
||
|
|
power_ctr = BMG160_MODE_NORMAL;
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bmg160_set_power_mode(power_ctr) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_MAG:
|
||
|
|
{
|
||
|
|
switch (power_mode)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_POWER_DOWN:
|
||
|
|
power_ctr = 0;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_POWER_NORMAL:
|
||
|
|
power_ctr = 1;
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
if (bmm050_set_power_mode(power_ctr) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
exit_bmx055_set_power:
|
||
|
|
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
static rt_err_t bmx055_set_range(rt_sensor_t sensor, rt_uint16_t range)
|
||
|
|
{
|
||
|
|
int idx;
|
||
|
|
|
||
|
|
switch (sensor->info.type)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CLASS_ACCE:
|
||
|
|
{
|
||
|
|
idx = find_param_index(range, accel_ranges, sizeof(accel_ranges));
|
||
|
|
if (bma2x2_set_range(accel_ranges[idx].reg) != 0)
|
||
|
|
goto exit_bmx055_set_range;
|
||
|
|
else if (bma2x2_get_range(&g_u8Range_accel) != 0)
|
||
|
|
goto exit_bmx055_set_range;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_GYRO:
|
||
|
|
{
|
||
|
|
idx = find_param_index(range, gyro_ranges, sizeof(gyro_ranges));
|
||
|
|
if (bmg160_set_range_reg(gyro_ranges[idx].reg) != 0)
|
||
|
|
goto exit_bmx055_set_range;
|
||
|
|
else if (bmg160_get_range_reg(&g_u8Range_gyro) != 0)
|
||
|
|
goto exit_bmx055_set_range;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
exit_bmx055_set_range:
|
||
|
|
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
static rt_err_t bmx055_set_odr(rt_sensor_t sensor, rt_uint16_t odr_hz)
|
||
|
|
{
|
||
|
|
int idx;
|
||
|
|
switch (sensor->info.type)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CLASS_ACCE:
|
||
|
|
{
|
||
|
|
idx = find_param_index(odr_hz, accel_odr, sizeof(accel_odr));
|
||
|
|
if (bma2x2_set_bw(accel_odr[idx].reg) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_GYRO:
|
||
|
|
{
|
||
|
|
idx = find_param_index(odr_hz, gyro_odr, sizeof(gyro_odr));
|
||
|
|
if (bmg160_set_bw(gyro_odr[idx].reg) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case RT_SENSOR_CLASS_MAG:
|
||
|
|
{
|
||
|
|
idx = find_param_index(odr_hz, mag_odr, sizeof(mag_odr));
|
||
|
|
if (bmm050_set_data_rate(mag_odr[idx].reg) != 0)
|
||
|
|
goto exit_bmx055_set_power;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return RT_EOK;
|
||
|
|
|
||
|
|
exit_bmx055_set_power:
|
||
|
|
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
static rt_err_t bmx055_control(rt_sensor_t sensor, int cmd, void *args)
|
||
|
|
{
|
||
|
|
RT_ASSERT(sensor != RT_NULL);
|
||
|
|
RT_ASSERT(args != RT_NULL);
|
||
|
|
|
||
|
|
switch (cmd)
|
||
|
|
{
|
||
|
|
case RT_SENSOR_CTRL_GET_ID:
|
||
|
|
{
|
||
|
|
rt_uint8_t *pu8id = (rt_uint8_t *)args;
|
||
|
|
return bmx055_getid(sensor, pu8id);
|
||
|
|
}
|
||
|
|
case RT_SENSOR_CTRL_SET_RANGE:
|
||
|
|
return bmx055_set_range(sensor, (rt_uint32_t)args);
|
||
|
|
|
||
|
|
case RT_SENSOR_CTRL_SET_POWER:
|
||
|
|
return bmx055_set_power(sensor, ((rt_uint32_t)args & 0xff));
|
||
|
|
|
||
|
|
case RT_SENSOR_CTRL_SET_ODR:
|
||
|
|
return bmx055_set_odr(sensor, ((rt_uint32_t)args & 0xff));
|
||
|
|
|
||
|
|
}
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
static struct rt_sensor_ops sensor_ops =
|
||
|
|
{
|
||
|
|
bmx055_fetch_data,
|
||
|
|
bmx055_control
|
||
|
|
};
|
||
|
|
|
||
|
|
static int rt_hw_bmx055_accel_init(const char *name, struct rt_sensor_config *cfg)
|
||
|
|
{
|
||
|
|
rt_int8_t result;
|
||
|
|
rt_sensor_t sensor = RT_NULL;
|
||
|
|
|
||
|
|
sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
|
||
|
|
if (sensor == RT_NULL)
|
||
|
|
return -(RT_ENOMEM);
|
||
|
|
|
||
|
|
sensor->info.type = RT_SENSOR_CLASS_ACCE;
|
||
|
|
sensor->info.vendor = RT_SENSOR_VENDOR_BOSCH;
|
||
|
|
sensor->info.model = "bmx055_acce";
|
||
|
|
sensor->info.unit = RT_SENSOR_UNIT_MG;
|
||
|
|
sensor->info.intf_type = RT_SENSOR_INTF_I2C;
|
||
|
|
sensor->info.range_max = 16000;
|
||
|
|
sensor->info.range_min = 2000;
|
||
|
|
sensor->info.period_min = 100;
|
||
|
|
|
||
|
|
rt_memcpy(&sensor->config, cfg, sizeof(struct rt_sensor_config));
|
||
|
|
sensor->ops = &sensor_ops;
|
||
|
|
|
||
|
|
result = rt_hw_sensor_register(sensor, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
|
||
|
|
if (result != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("device register: %d", result);
|
||
|
|
rt_free(sensor);
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int rt_hw_bmx055_gyro_init(const char *name, struct rt_sensor_config *cfg)
|
||
|
|
{
|
||
|
|
rt_int8_t result;
|
||
|
|
rt_sensor_t sensor = RT_NULL;
|
||
|
|
|
||
|
|
sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
|
||
|
|
if (sensor == RT_NULL)
|
||
|
|
return -(RT_ENOMEM);
|
||
|
|
|
||
|
|
sensor->info.type = RT_SENSOR_CLASS_GYRO;
|
||
|
|
sensor->info.vendor = RT_SENSOR_VENDOR_BOSCH;
|
||
|
|
sensor->info.model = "bmx055_gyro";
|
||
|
|
sensor->info.unit = RT_SENSOR_UNIT_MDPS;
|
||
|
|
sensor->info.intf_type = RT_SENSOR_INTF_I2C;
|
||
|
|
sensor->info.range_max = 2000;
|
||
|
|
sensor->info.range_min = 125;
|
||
|
|
sensor->info.period_min = 100;
|
||
|
|
|
||
|
|
rt_memcpy(&sensor->config, cfg, sizeof(struct rt_sensor_config));
|
||
|
|
sensor->ops = &sensor_ops;
|
||
|
|
|
||
|
|
result = rt_hw_sensor_register(sensor, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
|
||
|
|
if (result != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("device register: %d", result);
|
||
|
|
rt_free(sensor);
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int rt_hw_bmx055_mag_init(const char *name, struct rt_sensor_config *cfg)
|
||
|
|
{
|
||
|
|
rt_int8_t result;
|
||
|
|
rt_sensor_t sensor = RT_NULL;
|
||
|
|
|
||
|
|
sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
|
||
|
|
if (sensor == RT_NULL)
|
||
|
|
return -(RT_ENOMEM);
|
||
|
|
|
||
|
|
sensor->info.type = RT_SENSOR_CLASS_MAG;
|
||
|
|
sensor->info.vendor = RT_SENSOR_VENDOR_BOSCH;
|
||
|
|
sensor->info.model = "bmx055_mag";
|
||
|
|
sensor->info.unit = RT_SENSOR_UNIT_MGAUSS;
|
||
|
|
sensor->info.intf_type = RT_SENSOR_INTF_I2C;
|
||
|
|
sensor->info.range_max = 25000; // 1uT = 10*mGauss, X/Y: 1300uT=13000mGauss, Z: 2500uT=25000mG
|
||
|
|
sensor->info.range_min = 0;
|
||
|
|
sensor->info.period_min = 100;
|
||
|
|
|
||
|
|
rt_memcpy(&sensor->config, cfg, sizeof(struct rt_sensor_config));
|
||
|
|
sensor->ops = &sensor_ops;
|
||
|
|
|
||
|
|
result = rt_hw_sensor_register(sensor, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
|
||
|
|
if (result != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("device register: %d", result);
|
||
|
|
rt_free(sensor);
|
||
|
|
return -RT_ERROR;
|
||
|
|
}
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
int rt_hw_bmx055_init(const char *name, struct rt_sensor_config *cfg)
|
||
|
|
{
|
||
|
|
struct rt_sensor_intf *intf;
|
||
|
|
rt_err_t ret = -RT_ERROR;
|
||
|
|
|
||
|
|
RT_ASSERT(name != NULL);
|
||
|
|
RT_ASSERT(cfg != NULL);
|
||
|
|
|
||
|
|
intf = &cfg->intf;
|
||
|
|
|
||
|
|
/* Find I2C bus */
|
||
|
|
i2c_bus_dev = (struct rt_i2c_bus_device *)rt_device_find(intf->dev_name);
|
||
|
|
if (i2c_bus_dev == RT_NULL)
|
||
|
|
{
|
||
|
|
LOG_E("Can't found I2C bus..!\n");
|
||
|
|
goto exit_rt_hw_bmx055_init;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bmx055_init_mag(&g_sbmx055.mag) != 0)
|
||
|
|
{
|
||
|
|
LOG_E("Init mag..!\n");
|
||
|
|
}
|
||
|
|
else if ((ret = rt_hw_bmx055_mag_init(name, cfg)) != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("Register mag..!\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bmx055_init_accel(&g_sbmx055.accel) != 0)
|
||
|
|
{
|
||
|
|
LOG_E("Init accel..!\n");
|
||
|
|
}
|
||
|
|
else if ((ret = rt_hw_bmx055_accel_init(name, cfg)) != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("Register accel..!\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
if (bmx055_init_gyro(&g_sbmx055.gyro) != 0)
|
||
|
|
{
|
||
|
|
LOG_E("Init gyro..!\n");
|
||
|
|
}
|
||
|
|
else if ((ret = rt_hw_bmx055_gyro_init(name, cfg)) != RT_EOK)
|
||
|
|
{
|
||
|
|
LOG_E("Register gyro..!\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
exit_rt_hw_bmx055_init:
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif //#if defined(NU_PKG_USING_BMX055)
|