LSM6DS33是一款加速度计和陀螺仪,还支持计步功能。刚好手上有这款传感器,便移植下。
vendor/mediatek/proprietary/hardware/sensor/StepCounter.cpp
#undef NDEBUG
kernel-3.18/arch/arm64/configs/len6737t_66_n_defconfig
CONFIG_CUSTOM_KERNEL_STEP_COUNTER=y
这是因为加了这个才编译step_counter这个文件夹。
kernel-3.18/drivers/misc/mediatek/Makefile
obj-$(CONFIG_CUSTOM_KERNEL_STEP_COUNTER) += step_counter/
源码路径kernel-3.18/drivers/misc/mediatek/accelerometer/lsm6ds3,相关计步代码摘录如下
#define LSM6DS3_CONFIG_PEDO_THS_MIN 0x0F
#define LSM6DS3_SUCCESS 0
#define LSM6DS3_ERR_I2C -1
#define LSM6DS3_STEP_COUNTER_L 0x4B
#define LSM6DS3_STEP_COUNTER_H 0x4C
#define LSM6DS3_RAM_ACCESS 0X01
#define LSM6DS3_RAM_PAGE_MASK 0x80
#define LSM6DS3_CTRL10_C 0x19
#define LSM6DS3_PEDO_RST_STEP_MASK 0x02
#define LSM6DS3_ERR_STATUS -3
#define LSM6DS3_CTRL1_XL 0x10
#define LSM6DS3_ACC_ODR_MASK 0xF0
#define LSM6DS3_ACC_ODR_POWER_DOWN 0x00
#define LSM6DS3_TAP_CFG 0x58
#define LSM6DS3_PEDO_EN_MASK 0x40
#define LSM6DS3_ACC_GYRO_FUNC_EN_MASK 0x04
typedef enum {
LSM6DS3_ACC_GYRO_PEDO_RST_STEP_DISABLED = 0x00,
LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED = 0x02,
} LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t;
typedef enum {
LSM6DS3_ACC_GYRO_FUNC_EN_DISABLED = 0x00,
LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED = 0x04,
} LSM6DS3_ACC_GYRO_FUNC_EN_t;
typedef enum {
LSM6DS3_ACC = 1,
LSM6DS3_STEP_C = 2,
LSM6DS3_TILT = 3,
} LSM6DS3_INIT_TYPE;
typedef enum {
LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED = 0x00,
LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED = 0x80,
} LSM6DS3_ACC_GYRO_RAM_PAGE_t;
typedef enum {
LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED = 0x00,
LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED = 0x40,
} LSM6DS3_ACC_GYRO_PEDO_EN_t;
static DEFINE_MUTEX(lsm6ds3_init_mutex);
static int lsm6ds3_acc_init_flag; /*initial in module_init = -1;*/
static bool tilt_enable_status; /*initial in module_init = false;*/
static unsigned long lsm6ds3_init_flag_test;
static bool pedo_enable_status; /*initial in module_init = false;*/
static int LSM6DS3_acc_SetPowerMode(struct i2c_client *client, bool enable)
{
u8 databuf[2] = {0};
int res = 0;
struct lsm6ds3h_i2c_data *obj = i2c_get_clientdata(client);
if (enable == sensor_power) {
GSE_LOG("Sensor power status is newest!\n");
return LSM6DS3_SUCCESS;
}
if (hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf)) {
GSE_ERR("read lsm6ds3 power ctl register err!\n");
return LSM6DS3_ERR_I2C;
}
GSE_LOG("LSM6DS3_CTRL1_XL:databuf[0] = %x!\n", databuf[0]);
if (true == enable) {
databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;/*clear lsm6ds3 gyro ODR bits*/
databuf[0] |= obj->sample_rate;/*LSM6DS3_ACC_ODR_104HZ; //default set 100HZ for LSM6DS3 acc*/
} else {
databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;/*clear lsm6ds3 acc ODR bits*/
databuf[0] |= LSM6DS3_ACC_ODR_POWER_DOWN;
}
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_CTRL1_XL;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_LOG("LSM6DS3 set power mode: ODR 100hz failed!\n");
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("set LSM6DS3 gyro power mode:ODR 100HZ ok %d!\n", enable);
}
sensor_power = enable;
return LSM6DS3_SUCCESS;
}
static int lsm6ds3_step_c_enable_significant(int en)
{
int res = 0;
return res;
}
static int LSM6DS3_acc_Enable_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_FUNC_EN_t newValue)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
if (hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf)) {
GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
}
databuf[0] &= ~LSM6DS3_ACC_GYRO_FUNC_EN_MASK;/*clear */
databuf[0] |= newValue;
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_CTRL10_C;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_acc_SetSampleRate(struct i2c_client *client, u8 sample_rate)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
res = LSM6DS3_acc_SetPowerMode(client, true); /*set Sample Rate will enable power and should changed power status*/
if (res != LSM6DS3_SUCCESS) {
return res;
}
if (hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf)) {
GSE_ERR("read acc data format register err!\n");
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("read acc data format register: 0x%x\n", databuf[0]);
}
databuf[0] &= ~LSM6DS3_ACC_ODR_MASK; /*clear*/
databuf[0] |= sample_rate;
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_CTRL1_XL;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("write sample rate register err!\n");
return LSM6DS3_ERR_I2C;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_acc_Enable_Pedometer_Func(struct i2c_client *client, bool enable)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
if (hwmsen_read_byte(client, LSM6DS3_TAP_CFG, databuf)) {
GSE_ERR("read acc data format register err!\n");
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("read acc data format register: 0x%x\n", databuf[0]);
}
if (enable) {
databuf[0] &= ~LSM6DS3_PEDO_EN_MASK; /*clear */
databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED;
} else {
databuf[0] &= ~LSM6DS3_PEDO_EN_MASK; /*clear */
databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED;
}
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_TAP_CFG;
res = i2c_master_send(client, databuf, 0x2);
if (res < 0) {
GSE_ERR("write enable pedometer func register err!\n");
return LSM6DS3_ERR_I2C;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_W_Open_RAM_Page(struct i2c_client *client, LSM6DS3_ACC_GYRO_RAM_PAGE_t newValue)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
if (hwmsen_read_byte(client, LSM6DS3_RAM_ACCESS, databuf)) {
GSE_ERR("%s read LSM6DS3_RAM_ACCESS register err!\n", __func__);
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
}
databuf[0] &= ~LSM6DS3_RAM_PAGE_MASK;/*clear */
databuf[0] |= newValue;
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_RAM_ACCESS;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("%s write LSM6DS3_RAM_ACCESS register err!\n", __func__);
return LSM6DS3_ERR_I2C;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_Reset_Pedo_Data(struct i2c_client *client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t newValue)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
if (hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf)) {
GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("%s read acc LSM6DS3_CTRL10_C data format register: 0x%x\n", __func__, databuf[0]);
}
databuf[0] &= ~LSM6DS3_PEDO_RST_STEP_MASK;/*clear */
databuf[0] |= newValue;
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_CTRL10_C;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_Write_PedoThreshold(struct i2c_client *client, u8 newValue)
{
u8 databuf[2] = {0};
int res = 0;
GSE_FUN();
res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED);
if (LSM6DS3_SUCCESS != res) {
return res;
}
if (hwmsen_read_byte(client, LSM6DS3_CONFIG_PEDO_THS_MIN, databuf)) {
GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
} else {
GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
}
databuf[0] &= ~0x1F;
databuf[0] |= (newValue & 0x1F);
databuf[1] = databuf[0];
databuf[0] = LSM6DS3_CONFIG_PEDO_THS_MIN;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
}
databuf[0] = 0x14;
databuf[1] = 0x6e;
res = i2c_master_send(client, databuf, 0x2);
if (res <= 0) {
GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
return LSM6DS3_ERR_I2C;
}
res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED);
if (LSM6DS3_SUCCESS != res) {
GSE_ERR("%s write LSM6DS3_W_Open_RAM_Page failed!\n", __func__);
return res;
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_enable_pedo(struct i2c_client *client, bool enable)
{
int res = 0;
struct lsm6ds3h_i2c_data *obj = i2c_get_clientdata(client);
if (true == enable) {
/*/software reset
//set ODR to 26 hz
//res = LSM6DS3_acc_SetSampleRate(client, LSM6DS3_ACC_ODR_26HZ);*/
res = LSM6DS3_acc_SetSampleRate(client, obj->sample_rate);
if (LSM6DS3_SUCCESS == res) {
GSE_LOG(" %s set 26hz odr to acc\n", __func__);
}
/*enable tilt feature and pedometer feature*/
res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_acc_Enable_Pedometer_Func failed!\n");
return LSM6DS3_ERR_STATUS;
}
res = LSM6DS3_acc_Enable_Func(client, LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED);
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
return LSM6DS3_ERR_STATUS;
}
res = LSM6DS3_Write_PedoThreshold(client, 0x11);/* set threshold to a certain value here*/
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_Write_PedoThreshold failed!\n");
return LSM6DS3_ERR_STATUS;
}
res = LSM6DS3_Reset_Pedo_Data(client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED);
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_Reset_Pedo_Data failed!\n");
return LSM6DS3_ERR_STATUS;
}
} else {
res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_acc_Enable_Func failed at disable pedo!\n");
return LSM6DS3_ERR_STATUS;
}
/*do not turn off the func*/
if (!enable_status && !tilt_enable_status) {
res = LSM6DS3_acc_SetPowerMode(client, false);
if (res != LSM6DS3_SUCCESS) {
GSE_LOG(" LSM6DS3_acc_SetPowerMode failed at disable pedo!\n");
return LSM6DS3_ERR_STATUS;
}
}
}
return LSM6DS3_SUCCESS;
}
static int LSM6DS3_Get_Pedo_DataReg(struct i2c_client *client, u16 *Value)
{
u8 databuf[2] = {0};
GSE_FUN();
if (hwmsen_read_block(client, LSM6DS3_STEP_COUNTER_L, databuf, 2)) {
GSE_ERR("LSM6DS3 read acc data error\n");
return -2;
}
*Value = (databuf[1]<<8)|databuf[0];
return LSM6DS3_SUCCESS;
}
static int lsm6ds3_step_c_open_report_data(int open)
{
return LSM6DS3_SUCCESS;
}
static int lsm6ds3_step_c_enable_nodata(int en)
{
int res = 0;
int value = en;
int err = 0;
struct lsm6ds3h_i2c_data *priv = obj_i2c_data;
if (priv == NULL) {
GSE_ERR("%s obj_i2c_data is NULL!\n", __func__);
return -1;
}
if (value == 1) {
pedo_enable_status = true;
res = LSM6DS3_enable_pedo(priv->client, true);
if (LSM6DS3_SUCCESS != res) {
GSE_LOG("LSM6DS3_enable_pedo failed at open action!\n");
return res;
}
} else {
pedo_enable_status = false;
res = LSM6DS3_enable_pedo(priv->client, false);
if (LSM6DS3_SUCCESS != res) {
GSE_LOG("LSM6DS3_enable_pedo failed at close action!\n");
return res;
}
}
GSE_LOG("lsm6ds3_step_c_enable_nodata OK!\n");
return err;
}
static int lsm6ds3_step_c_enable_step_detect(int en)
{
return lsm6ds3_step_c_enable_nodata(en);
}
static int lsm6ds3_step_c_set_delay(u64 delay)
{
return 0;
}
static int lsm6ds3_step_c_get_data(uint32_t *value, int *status)
{
int err = 0;
u16 pedo_data = 0;
struct lsm6ds3h_i2c_data *priv = obj_i2c_data;
err = LSM6DS3_Get_Pedo_DataReg(priv->client, &pedo_data);
*value = (u32)pedo_data;
*status = SENSOR_STATUS_ACCURACY_MEDIUM;
return err;
}
static int lsm6ds3_step_c_get_data_step_d(uint32_t *value, int *status)
{
return 0;
}
static int lsm6ds3_step_c_get_data_significant(uint32_t *value, int *status)
{
return 0;
}
static int lsm6ds3_step_c_local_init(void)
{
int res = 0;
struct step_c_control_path step_ctl = {0};
struct step_c_data_path step_data = {0};
mutex_lock(&lsm6ds3_init_mutex);
set_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);
if ((0 == test_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test)) \
&& (0 == test_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test))) {
//res = lsm6ds3_local_init_common();
if (res < 0) {
goto lsm6ds3_step_c_local_init_failed;
}
}
if (lsm6ds3_acc_init_flag == -1) {
mutex_unlock(&lsm6ds3_init_mutex);
GSE_ERR("%s init failed!\n", __FUNCTION__);
return -1;
} else {
step_ctl.open_report_data = lsm6ds3_step_c_open_report_data;
step_ctl.enable_nodata = lsm6ds3_step_c_enable_nodata;
step_ctl.enable_step_detect = lsm6ds3_step_c_enable_step_detect;
step_ctl.step_c_set_delay = lsm6ds3_step_c_set_delay;
step_ctl.step_d_set_delay = lsm6ds3_step_c_set_delay;
step_ctl.is_report_input_direct = false;
step_ctl.is_support_batch = false;
//#ifdef LSM6DS3_SIGNIFICANT_MOTION
step_ctl.enable_significant = lsm6ds3_step_c_enable_significant;
//#endif
res = step_c_register_control_path(&step_ctl);
if (res) {
GSE_ERR("register step counter control path err\n");
goto lsm6ds3_step_c_local_init_failed;
}
step_data.get_data = lsm6ds3_step_c_get_data;
step_data.get_data_step_d = lsm6ds3_step_c_get_data_step_d;
step_data.get_data_significant = lsm6ds3_step_c_get_data_significant;
step_data.vender_div = 1;
res = step_c_register_data_path(&step_data);
if (res) {
GSE_ERR("register step counter data path err= %d\n", res);
goto lsm6ds3_step_c_local_init_failed;
}
}
mutex_unlock(&lsm6ds3_init_mutex);
return 0;
lsm6ds3_step_c_local_init_failed:
mutex_unlock(&lsm6ds3_init_mutex);
GSE_ERR("%s init failed!\n", __FUNCTION__);
return res;
}
static int lsm6ds3_step_c_local_uninit(void)
{
clear_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);
return 0;
}
/*----------------------------------------------------------------------------*/
static int lsm6ds3_step_c_local_init(void);
static int lsm6ds3_step_c_local_uninit(void);
static struct step_c_init_info lsm6ds3_step_c_init_info = {
.name = "LSM6DS3_STEP_C",
.init = lsm6ds3_step_c_local_init,
.uninit = lsm6ds3_step_c_local_uninit,
};
step_c_driver_add(&lsm6ds3_step_c_init_info); /*step counter*/
先让上层知道底层支持计步功能,修改如下
device/lentek/len6737t_66_n/ProjectConfig.mk
CUSTOM_KERNEL_STEP_COUNTER= yes
这是因为定义CUSTOM_KERNEL_STEP_COUNTER,数组sSensorList才保存计步传感器信息。
vendor/mediatek/proprietary/hardware/sensor/sensors.c
struct sensor_t sSensorList[] ={
#ifdef CUSTOM_KERNEL_STEP_COUNTER
{
.name = STEP_COUNTER,
.vendor = STEP_COUNTER_VENDER,
.version = 1,
.handle = ID_STEP_COUNTER+ID_OFFSET,
.type = SENSOR_TYPE_STEP_COUNTER,
.maxRange = STEP_COUNTER_RANGE,//600.0f,
.resolution = STEP_COUNTER_RESOLUTION,//0.0016667f,
.power = STEP_COUNTER_POWER,//0.25f,
.minDelay = STEP_COUNTER_MINDELAY,
.flags = SENSOR_FLAG_ON_CHANGE_MODE,
.reserved = {}
},
{
.name = STEP_DETECTOR,
.vendor = STEP_DETECTOR_VENDER,
.version = 1,
.handle = ID_STEP_DETECTOR+ID_OFFSET,
.type = SENSOR_TYPE_STEP_DETECTOR,
.maxRange = STEP_DETECTOR_RANGE,//600.0f,
.resolution = STEP_DETECTOR_RESOLUTION,//0.0016667f,
.power = STEP_DETECTOR_POWER,//0.25f,
.minDelay = STEP_DETECTOR_MINDELAY,
.flags = SENSOR_FLAG_SPECIAL_REPORTING_MODE,
.reserved = {}
},
#endif
}
mmm vendor/mediatek/proprietary/hardware/sensor编译生成相应的hal库,并push
到机器了,如sensors.mt6737t.so,重启后用安兔兔检测能看到手机支持了计步功能。
这时发现驱动并没有使能,找到如下代码,让ALOGD输入调试信息vendor/mediatek/proprietary/hardware/sensor/StepCounter.cpp
#undef NDEBUG
adb logcat -s STEP_COUNTER能找到出错原因,原来是权限不够导致不能使能,在device/mediatek/mt6735/init.mt6735.rc加入
chmod 0660 /sys/class/misc/m_step_c_misc/step_cactive
chmod 0660 /sys/class/misc/m_step_c_misc/step_cbatch
chmod 0660 /sys/class/misc/m_step_c_misc/step_cdelay
chmod 0660 /sys/class/misc/m_step_c_misc/step_cdevnum
chmod 0660 /sys/class/misc/m_step_c_misc/step_cenablenodata
chmod 0660 /sys/class/misc/m_step_c_misc/step_cflush
chown system system /sys/class/misc/m_step_c_misc/step_cactive
chown system system /sys/class/misc/m_step_c_misc/step_cbatch
chown system system /sys/class/misc/m_step_c_misc/step_cdelay
chown system system /sys/class/misc/m_step_c_misc/step_cdevnum
chown system system /sys/class/misc/m_step_c_misc/step_cenablenodata
chown system system /sys/class/misc/m_step_c_misc/step_cflush
这时,流程已基本没有问题的,晃动手机,上层也读到了数据,至此移植功能完成。