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这时,流程已基本没有问题的,晃动手机,上层也读到了数据,至此移植功能完成。