前面3篇都是当时对该驱动的一个修改与提交，代码目前提交修改3次
第一次是sensor驱动的节点生成与能上报数据
第二次修改是为了满足hal层的基础要求
这次第三次提交是为啦通过高德地图的验证
验证目标：通过重帧率，不稳定率，以及丢帧率，
重帧率：
相邻帧不能是相同的数据
不稳定率：
时间戳需要稳定在一个周期，例如设置周期采样每秒10次，在5分钟的采样中需要是5*60*10=3000个数据，要求是千分的一之内，也就是5分钟不能丢弃或多出3个以上
丢帧率：
顾名思义就是丢失的帧，这个要求是千分之三
上述要求在平台的基础框架上无法解决，原来用工作队列上报的情况来看，帧的时间戳极差会在8ms左右，那这个根本无法去保证上述条件
还是需要高精度来完成对时间戳的限制
因为我是考虑在基础框架上改动，工作队列采样到内存，再用高精度上报到接口，前几篇文章也写过这个解决方法，但是其中加锁的情况下，后续测试低概率会出现硬锁，因为工作队列和高精度拥有一个锁的情况下，如果工作队列一直不执行就会出现高精度只能被卡在加spin锁的位置导致死锁，这个位置是不能加普通spin_lock的，但是加spin_irqsave之类的会有比较大的消耗，再考虑驱动属于单生产者和单消费者，就改成fifo来存取数据，去掉锁的机制
基本上面高精度与它的异常情况考虑做完就能把丢帧跟不稳定率搞定
重振率：重帧率需要跟hal层联调验证，因为在input框架，hal层都会有过滤机制，
sensor芯片->驱动->event->上传到hal层->高德地图测试log
在event->上传到hal层：数据都可能出现被过滤的情况
其他位置都是传递数据，那必须在驱动部分把数据处理好再做上传我前面遇到的问题有
重点说一下第2点：
1.sensor过来的是相同的数据，
2.因为input框架会过滤掉部分数据
3.驱动处理的数据还可能出现相同值
accel在rk平台的注册默认是abs类型，gyro是rel类型，这里两个的过滤机制不同
1.abs类型的会过滤跟上次相同的值
2.rel会过滤与当前为0的值
这个做部分的数据就能过 ，数据处理基本在report_value中
代码如下
/* drivers/input/sensors/access/qmi8658_acc.c
*
* Copyright (C) 2012-2015 ROCKCHIP.
* Author: oeh<oeh@rock-chips.com>
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
*/
#include <linux/interrupt.h>
#include <linux/i2c.h>
#include <linux/slab.h>
#include <linux/irq.h>
#include <linux/miscdevice.h>
#include <linux/gpio.h>
#include <linux/uaccess.h>
#include <asm/atomic.h>
#include <linux/delay.h>
#include <linux/input.h>
#include <linux/workqueue.h>
#include <linux/freezer.h>
#include <linux/of_gpio.h>
#ifdef CONFIG_HAS_EARLYSUSPEND
#include <linux/earlysuspend.h>
#endif
#include <linux/sensor-dev.h>
#include <linux/qmi8658.h>
#include <linux/module.h>
#include <linux/jiffies.h>
#include <linux/of_device.h>
#include <linux/hrtimer.h>
#include <linux/time64.h>
#include <linux/kfifo.h>
static bool hrtimer_is_initialized;
/* sensor set rate */
static int qmi8658_set_rate(struct i2c_client *client, int rate)
{
const short hz[] = {448, 224, 112, 56, 28};
const int d[] = {0x24, 0x25,0x26, 0x27, 0x28};
int i =0, data =0, result =0;
/* always use poll mode, no need to set rate */
return 0;
if ((rate < 1) || (rate > 480))
return -1;
while ((rate > hz[i]) && (i < 5))
i++;
data = d[i];
result = sensor_write_reg(client, QMI8658_REG_CTRL2, data);
if (result)
return -1;
return 0;
}
/* sensor report value */
static int gsensor_report_value(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int ret = 0;
struct kfifo_data kfifo_data;
if (sensor->status_cur == SENSOR_ON) {
if(kfifo_len(&sensor->sensor_data_fifo_array) <= 0){
printk(KERN_INFO "qmi8658 acc fifo array empty\n");
return ret;
}
while (kfifo_len(&sensor->sensor_data_fifo_array) > 0) {
ret = kfifo_out(&sensor->sensor_data_fifo_array, &kfifo_data, 1);
if (kfifo_len(&sensor->sensor_data_fifo_array) <= 2) {
break;
}
}
if(kfifo_data.x == sensor->sensor_cur_data.value[0]){
kfifo_data.x += 1;
}
if(kfifo_data.y == sensor->sensor_cur_data.value[1]){
kfifo_data.y += 1;
}
if(kfifo_data.z == sensor->sensor_cur_data.value[2]){
kfifo_data.z += 1;
}
sensor->sensor_cur_data.value[0] = kfifo_data.x;
sensor->sensor_cur_data.value[1] = kfifo_data.y;
sensor->sensor_cur_data.value[2] = kfifo_data.z;
// printk("qmi8658 input_report_abs accel x=%d,y=%d,z=%d\n",sensor->sensor_cur_data.value[0],sensor->sensor_cur_data.value[1],sensor->sensor_cur_data.value[2]);
input_report_abs(sensor->input_dev, ABS_X, sensor->sensor_cur_data.value[0]);
input_report_abs(sensor->input_dev, ABS_Y, sensor->sensor_cur_data.value[1]);
input_report_abs(sensor->input_dev, ABS_Z, sensor->sensor_cur_data.value[2]);
input_sync(sensor->input_dev);
}
return ret;
}
/*sensor hrtimer init and report data*/
static enum hrtimer_restart input_timer_accel_callback(struct hrtimer *timer)
{
struct sensor_private_data *sensor = container_of(timer, struct sensor_private_data, input_timer_accel);
hrtimer_forward_now(timer, ktime_set(0, TIMER_INTERVAL));
gsensor_report_value(sensor->client);
return HRTIMER_RESTART;
}
/* init accel timer */
static int sensor_init_accel_timer(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int ret = 0;
INIT_KFIFO(sensor->sensor_data_fifo_array);
hrtimer_init(&sensor->input_timer_accel, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED);
sensor->input_timer_accel.function = input_timer_accel_callback;
pr_info("%s: sensor_init_accel_timer 1.\n", __func__);
return ret;
}
static int sensor_active(struct i2c_client *client, int enable, int rate)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int result = 0;
u8 databuf[2] = {0};
databuf[0] = sensor_read_reg(client, QMI8658_REG_CTRL7);
// dev_err(&client->dev, "%s:QMI8658_REG_CTRL7 = 0x%02x\n", __func__,databuf[0]);
if(hrtimer_is_initialized == false){
result = sensor_init_accel_timer(client);
if(result){
pr_err("%s: sensor init timer failed.\n", __func__);
return result;
}
hrtimer_is_initialized = true;
}
if (!enable) {
if(hrtimer_active(&sensor->input_timer_accel)){
hrtimer_cancel(&sensor->input_timer_accel);
dev_info(&client->dev, "%s:hrtimer_active -> hrtimer_cancel \n", __func__);
}else{
dev_info(&client->dev, "%s: hrtimer not active \n", __func__);
}
databuf[0] = (databuf[0] & 0xFE) | 0x80; //accel disable;
} else {
databuf[0] = (databuf[0] & 0xFE) | 0x81; //accel enable;
qmi8658_set_rate(client, rate);
if(hrtimer_active(&sensor->input_timer_accel)){
dev_info(&client->dev, "%s:hrtimer is active \n", __func__);
}else{
hrtimer_start(&sensor->input_timer_accel, ktime_set(0, TIMER_START_ACCEL), HRTIMER_MODE_ABS_PINNED);
dev_info(&client->dev, "%s: hrtimer_active -> hrtimer_start\n", __func__);
}
}
result = sensor_write_reg(client, QMI8658_REG_CTRL7, databuf[0]);
if (result) {
dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__);
return -1;
}
dev_err(&client->dev, "zhoucs %s:activer OK, enable=%d, result=%d\n", __func__, enable, result);
return result;
}
static int sensor_init(struct i2c_client *client)
{
int res = 0;
u8 read_data = 0,reg_value = 0;
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
read_data = sensor_read_reg(client, sensor->ops->id_reg);
if (read_data != sensor->ops->id_data) {
dev_err(&client->dev, "%s:check id err,read_data:%d,ops->id_data:%d\n",
__func__, read_data, sensor->ops->id_data);
return -1;
}else{
dev_err(&client->dev, "%s:check id success -->qmi8658\n", __func__);
}
res = sensor_write_reg(client, QMI8658_REG_RESET, QMI8658_SW_RESET);
if (res) {
dev_err(&client->dev, "set QMI8658_SW_RESET error,res: %d!\n", res);
return res;
}
msleep(15);
res = sensor_write_reg(client, QMI8658_REG_CTRL1, QMI8658_ADDR_AI_BE);
if (res) {
dev_err(&client->dev, "set QMI8658_REG_CTRL1 error,res: %d!\n", res);
return res;
}
reg_value = QMI8658_ACC_RANGE_DEF|QMI8658_ODR_118HZ_REG_VALUE;// ±8g,odr=59hz
res = sensor_write_reg(client, QMI8658_REG_CTRL2, reg_value);
if (res) {
dev_err(&client->dev, "set QMI8658_REG_CTRL2 error,res: %d!\n", res);
return res;
}
reg_value = QMI8658_GYR_RANGE_2048DPS|QMI8658_ODR_118HZ_REG_VALUE;//2048dps,odr=59hz
res = sensor_write_reg(client, QMI8658_REG_CTRL3, reg_value);
if (res) {
dev_err(&client->dev, "set QMI8658_REG_CTRL3 error,res: %d!\n", res);
return res;
}
res = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms);
if (res) {
dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
return res;
}
res = sensor->ops->active(client, 1, sensor->pdata->poll_delay_ms);
if (res) {
dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
return res;
}
return res;
}
static int sensor_report_value(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
struct sensor_platform_data *pdata = sensor->pdata;
int ret = 0;
short x, y, z;
struct kfifo_data kfifo_data;
u8 buffer[6] = {0};
// char value = 0,count = 0;
/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
// while(((value & 0x03)!= 0x03)&&(count++ < 3)){
// value = sensor_read_reg(client, QMI8658_REG_STATUSINT);
// }
*buffer = QMI8658_REG_GYR_XOUT_LSB;
ret = sensor_rx_data(client, buffer, sensor->ops->read_len);
*buffer = sensor->ops->read_reg;
ret |= sensor_rx_data(client, buffer, sensor->ops->read_len);
if (ret < 0)
return ret;
x = ((buffer[1] << 8) & 0xFF00) + (buffer[0] & 0xFF);
y = ((buffer[3] << 8) & 0xFF00) + (buffer[2] & 0xFF);
z = ((buffer[5] << 8) & 0xFF00) + (buffer[4] & 0xFF);
kfifo_data.x = (pdata->orientation[0]) * x + (pdata->orientation[1]) * y + (pdata->orientation[2]) * z;
kfifo_data.y = (pdata->orientation[3]) * x + (pdata->orientation[4]) * y + (pdata->orientation[5]) * z;
kfifo_data.z = (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z;
/* get sensor value to fifo array*/
if (!kfifo_in(&sensor->sensor_data_fifo_array,&kfifo_data,1)) {
dev_err(&client->dev,"Failed to put data into the FIFO queue.\n");
}
// dev_info(&client->dev,"workqueue input data x=%d.y=%d,z=%d\n",kfifo_data.x,kfifo_data.y,kfifo_data.z);
return ret;
}
static int sensor_suspend(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int ret;
dev_info(&client->dev, "%s: start\n", __func__);
ret = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms);
if (ret) {
dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
return ret;
}
return ret;
}
static int sensor_resume(struct i2c_client *client)
{
hrtimer_is_initialized = false;
return 0;
}
struct sensor_operate gsensor_qmi8658_ops = {
.name = "qmi8658_acc",
.type = SENSOR_TYPE_ACCEL,
.id_i2c = ACCEL_ID_QMI8658,
.read_reg = QMI8658_REG_ACC_XOUT_LSB,
.read_len = 6,
.id_reg = QMI8658_REG_WHO_AM_I,
.id_data = SENSOR_CHIP_ID_QMI8658,
.precision = QMI8658_PRECISION,
.ctrl_reg = QMI8658_REG_CTRL7,
.int_status_reg = QMI8658_REG_STATUS0,
.range = {-32768, 32768},
.trig = IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
.active = sensor_active,
.init = sensor_init,
.report = sensor_report_value,
.suspend = sensor_suspend,
.resume = sensor_resume,
};
/****************operate according to sensor chip:end************/
static int gsensor_qmi8658_probe(struct i2c_client *client,
const struct i2c_device_id *devid)
{
hrtimer_is_initialized = false;
return sensor_register_device(client, NULL, devid, &gsensor_qmi8658_ops);
}
static int gsensor_qmi8658_remove(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
hrtimer_cancel(&sensor->input_timer_accel);
return sensor_unregister_device(client, NULL, &gsensor_qmi8658_ops);
}
static const struct i2c_device_id gsensor_qmi8658_id[] = {
{"qmi8658_acc", ACCEL_ID_QMI8658},
{}
};
static struct i2c_driver gsensor_qmi8658_driver = {
.probe = gsensor_qmi8658_probe,
.remove = gsensor_qmi8658_remove,
.shutdown = sensor_shutdown,
.id_table = gsensor_qmi8658_id,
.driver = {
.name = "gsensor_qmi8658",
#ifdef CONFIG_PM
.pm = &sensor_pm_ops,
#endif
},
};
module_i2c_driver(gsensor_qmi8658_driver);
MODULE_AUTHOR("ouenhui <oeh@rock-chips.com");
MODULE_DESCRIPTION("qmi8658_acc 3-kfifo_data accelerometer driver");
MODULE_LICENSE("GPL");
/* drivers/input/sensors/access/qmi8658_gyro.c
*
* Copyright (C) 2012-2015 ROCKCHIP.
* Author: ouenhui <oeh@rock-chips.com>
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
*/
#include <linux/interrupt.h>
#include <linux/i2c.h>
#include <linux/slab.h>
#include <linux/irq.h>
#include <linux/miscdevice.h>
#include <linux/gpio.h>
#include <linux/uaccess.h>
#include <asm/atomic.h>
#include <linux/delay.h>
#include <linux/input.h>
#include <linux/workqueue.h>
#include <linux/freezer.h>
#include <linux/of_gpio.h>
#ifdef CONFIG_HAS_EARLYSUSPEND
#include <linux/earlysuspend.h>
#endif
#include <linux/sensor-dev.h>
#include <linux/qmi8658.h>
#include <linux/module.h>
#include <linux/jiffies.h>
#include <linux/of_device.h>
#include <linux/hrtimer.h>
#include <linux/time64.h>
#include <linux/kfifo.h>
static bool hrtimer_is_initialized;
static int qmi8658_set_rate(struct i2c_client *client, int rate)
{
const short hz[] = {448, 224, 112, 56, 28};
const int d[] = {0x24, 0x25,0x26, 0x27, 0x28};
int i =0, data =0, result =0;
/* always use poll mode, no need to set rate */
return 0;
if ((rate < 1) || (rate > 480))
return -1;
while ((rate > hz[i]) && (i < 5))
i++;
data = d[i];
result = sensor_write_reg(client, QMI8658_REG_CTRL2, data);
if (result)
return -1;
return 0;
}
/* sensor report value */
static int gyro_report_value(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int ret = 0;
struct kfifo_data kfifo_data;
if (sensor->status_cur == SENSOR_ON) {
if(kfifo_len(&sensor->sensor_data_fifo_array) <= 0){
printk(KERN_INFO "qmi8658 gyro fifo array empty\n");
return ret;
}
while (kfifo_len(&sensor->sensor_data_fifo_array) > 0) {
ret = kfifo_out(&sensor->sensor_data_fifo_array, &kfifo_data, 1);
if (kfifo_len(&sensor->sensor_data_fifo_array) <= 2) {
break;
}
}
/* Report gyroeration sensor information */
if ((sensor->sensor_cur_data.value[0] == kfifo_data.x) && (sensor->sensor_cur_data.value[1] == kfifo_data.y) &&
(sensor->sensor_cur_data.value[2] == kfifo_data.z)) {
kfifo_data.x += 1;
kfifo_data.y += 1;
kfifo_data.z += 1;
// dev_err(&client->dev, "%s:same data1 line=%d,change to kfifo_data.x = [%d],y = {%d},z = [%d]\n", __func__, __LINE__,kfifo_data.x,kfifo_data.y,kfifo_data.z);
}
if(kfifo_data.x == 0){
kfifo_data.x += 1;
if(sensor->sensor_cur_data.value[0] == kfifo_data.x){
kfifo_data.x += 1;
}
}
if(kfifo_data.y == 0){
kfifo_data.y += 1;
if(sensor->sensor_cur_data.value[1] == kfifo_data.y){
kfifo_data.y += 1;
}
}
if(kfifo_data.z == 0){
kfifo_data.z += 1;
if(sensor->sensor_cur_data.value[2] == kfifo_data.z){
kfifo_data.z += 1;
}
}
sensor->sensor_cur_data.value[0] = kfifo_data.x;
sensor->sensor_cur_data.value[1] = kfifo_data.y;
sensor->sensor_cur_data.value[2] = kfifo_data.z;
// printk("qmi8658 input_report_abs gyro x=%d,y=%d,z=%d\n",sensor->sensor_cur_data.value[0],sensor->sensor_cur_data.value[1],sensor->sensor_cur_data.value[2]);
input_report_rel(sensor->input_dev, ABS_RX, sensor->sensor_cur_data.value[0]);
input_report_rel(sensor->input_dev, ABS_RY, sensor->sensor_cur_data.value[1]);
input_report_rel(sensor->input_dev, ABS_RZ, sensor->sensor_cur_data.value[2]);
input_sync(sensor->input_dev);
}
return ret;
}
/*sensor hrtimer init and report data*/
static enum hrtimer_restart input_timer_gyro_callback(struct hrtimer *timer)
{
struct sensor_private_data *sensor = container_of(timer, struct sensor_private_data, input_timer_gyro);
hrtimer_forward_now(timer, ktime_set(0, TIMER_INTERVAL));
gyro_report_value(sensor->client);
return HRTIMER_RESTART;
}
/* init gyro timer */
static int sensor_init_gyro_timer(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int ret = 0;
INIT_KFIFO(sensor->sensor_data_fifo_array);
hrtimer_init(&sensor->input_timer_gyro, CLOCK_MONOTONIC, HRTIMER_MODE_ABS_PINNED);
sensor->input_timer_gyro.function = input_timer_gyro_callback;
pr_info("%s: sensor_init_gyro_timer 1.\n", __func__);
return ret;
}
static int sensor_active(struct i2c_client *client, int enable, int rate)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
int result = 0;
u8 databuf[2] = {0};
databuf[0] = sensor_read_reg(client, QMI8658_REG_CTRL7);
if(hrtimer_is_initialized == false){
result = sensor_init_gyro_timer(client);
if(result){
pr_err("%s: sensor init timer failed.\n", __func__);
return result;
}
hrtimer_is_initialized = true;
}
if (!enable) {
if(hrtimer_active(&sensor->input_timer_gyro)){
hrtimer_cancel(&sensor->input_timer_gyro);
dev_info(&client->dev, "%s:hrtimer_active -> hrtimer_cancel\n", __func__);
}else{
dev_info(&client->dev, "%s: hrtimer not active \n", __func__);
}
databuf[0] = (databuf[0] & 0xFD) | 0x80; //gyro disable;
} else {
databuf[0] = (databuf[0] & 0xFD) | 0x82; //gyro enable;
qmi8658_set_rate(client, rate);
// dev_info(&client->dev, "%s: qmi8658 user set rate %dms\n", __func__,sensor->pdata->poll_delay_ms);
if(hrtimer_active(&sensor->input_timer_gyro)){
dev_info(&client->dev, "%s:hrtimer is active \n", __func__);
}else{
hrtimer_start(&sensor->input_timer_gyro, ktime_set(0, TIMER_START_GYRO), HRTIMER_MODE_ABS_PINNED);
dev_info(&client->dev, "%s: hrtimer_active -> hrtimer_start\n", __func__);
}
}
result = sensor_write_reg(client, QMI8658_REG_CTRL7, databuf[0]);
if (result) {
dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__);
return -1;
}
dev_err(&client->dev, "zhoucs %s:activer OK, enable=%d, result=%d\n", __func__, enable, result);
return result;
}
static int sensor_init(struct i2c_client *client)
{
int ret;
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
/* init on qmi8658_acc.c */
ret = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms);
if (ret) {
dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
return ret;
}
ret = sensor->ops->active(client, 1, sensor->pdata->poll_delay_ms);
if (ret) {
dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__);
return ret;
}
return ret;
}
static int sensor_report_value(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
struct sensor_platform_data *pdata = sensor->pdata;
int ret = 0;
short x, y, z;
u8 buffer[6] = {0};
struct kfifo_data kfifo_data;
// char value = 0,count = 0;
/* Data bytes from hardware xL, xH, yL, yH, zL, zH */
// while(((value & 0x03)!= 0x03)&&(count++ < 3)){
// value = sensor_read_reg(client, QMI8658_REG_STATUSINT);
// }
*buffer = QMI8658_REG_ACC_XOUT_LSB;
ret = sensor_rx_data(client, buffer, sensor->ops->read_len);
*buffer = sensor->ops->read_reg;
ret |= sensor_rx_data(client, buffer, sensor->ops->read_len);
if (ret < 0)
return ret;
x = ((buffer[1] << 8) & 0xFF00) + (buffer[0] & 0xFF);
y = ((buffer[3] << 8) & 0xFF00) + (buffer[2] & 0xFF);
z = ((buffer[5] << 8) & 0xFF00) + (buffer[4] & 0xFF);
kfifo_data.x = (pdata->orientation[0]) * x + (pdata->orientation[1]) * y + (pdata->orientation[2]) * z;
kfifo_data.y = (pdata->orientation[3]) * x + (pdata->orientation[4]) * y + (pdata->orientation[5]) * z;
kfifo_data.z = (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z;
/* get sensor value to fifo array*/
if (!kfifo_in(&sensor->sensor_data_fifo_array, &kfifo_data, 1)) {
dev_err(&client->dev,"Failed to put data into the FIFO queue.\n");
}
// dev_info(&client->dev,"workqueue input data x=%d.y=%d,z=%d\n",kfifo_data.x,kfifo_data.y,kfifo_data.z);
return ret;
}
static int sensor_suspend(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
dev_info(&client->dev, "%s: start\n", __func__);
if(hrtimer_active(&sensor->input_timer_gyro)){
hrtimer_cancel(&sensor->input_timer_gyro);
dev_info(&client->dev, "%s:hrtimer_active yes -> hrtimer_cancel\n", __func__);
}else{
dev_info(&client->dev, "%s: hrtimer not active\n", __func__);
}
return 0;
}
static int sensor_resume(struct i2c_client *client)
{
hrtimer_is_initialized = false;
return 0;
}
struct sensor_operate gyro_qmi8658_ops = {
.name = "qmi8658_gyro",
.type = SENSOR_TYPE_GYROSCOPE,
.id_i2c = GYRO_ID_QMI8658,
.read_reg = QMI8658_REG_GYR_XOUT_LSB,
.read_len = 6,
.id_reg = QMI8658_REG_WHO_AM_I,
.id_data = SENSOR_CHIP_ID_QMI8658,
.precision = QMI8658_PRECISION,
.ctrl_reg = QMI8658_REG_CTRL7,
.int_status_reg = QMI8658_REG_STATUS0,
.range = {-32768, 32768},
.trig = IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
.active = sensor_active,
.init = sensor_init,
.report = sensor_report_value,
.suspend = sensor_suspend,
.resume = sensor_resume,
};
/****************operate according to sensor chip:end************/
static int gyro_qmi8658_probe(struct i2c_client *client,
const struct i2c_device_id *devid)
{
hrtimer_is_initialized = false;
return sensor_register_device(client, NULL, devid, &gyro_qmi8658_ops);
}
static int gyro_qmi8658_remove(struct i2c_client *client)
{
struct sensor_private_data *sensor =
(struct sensor_private_data *) i2c_get_clientdata(client);
hrtimer_cancel(&sensor->input_timer_gyro);
return sensor_unregister_device(client, NULL, &gyro_qmi8658_ops);
}
static const struct i2c_device_id gyro_qmi8658_id[] = {
{"qmi8658_gyro", GYRO_ID_QMI8658},
{}
};
static struct i2c_driver gyro_qmi8658_driver = {
.probe = gyro_qmi8658_probe,
.remove = gyro_qmi8658_remove,
.shutdown = sensor_shutdown,
.id_table = gyro_qmi8658_id,
.driver = {
.name = "gyro_qmi8658",
#ifdef CONFIG_PM
.pm = &sensor_pm_ops,
#endif
},
};
static int __init gyro_qmi8658_init(void)
{
return i2c_add_driver(&gyro_qmi8658_driver);
}
static void __exit gyro_qmi8658_exit(void)
{
i2c_del_driver(&gyro_qmi8658_driver);
}
/* must register after qmi8658_acc */
device_initcall_sync(gyro_qmi8658_init);
module_exit(gyro_qmi8658_exit);
MODULE_AUTHOR("ouenhui <oeh@rock-chips.com");
MODULE_DESCRIPTION("qmi8658_gyro 3-kfifo_data Gyroscope driver");
MODULE_LICENSE("GPL");
扫一扫
952
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
