完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
扫一扫,分享给好友
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; int x,y,z; struct sensor_axis axis; #ifdef CONFIG_SENSORS_STK8312 char buffer[3] = {0}; #else char buffer[6] = {0}; #endif int acc_xyz[3] = {0}; int k_status = 0; int idx = 0; #ifdef STK_ZG_FILTER s16 zero_fir = 0; #endif k_status = atomic_read(&cali_status); // if(sensor->ops->read_len < 3) //sensor->ops->read_len = 3 // { // DBG("%s:lenth is error,len=%dn",__func__,sensor->ops->read_len); // return -1; // } #ifdef CONFIG_SENSORS_STK8312 memset(buffer, 0, 3); #else memset(buffer, 0, 6); #endif /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ do { *buffer = sensor->ops->read_reg; ret = sensor_rx_data(client, buffer, sensor->ops->read_len); if (ret < 0) return ret; } while (0); DBG("data: %d %d %dn", buffer[0], buffer[1], buffer[2]); #ifdef CONFIG_SENSORS_STK8312 if (buffer[0] & 0x80) acc_xyz[0] = buffer[0] - 256; else acc_xyz[0] = buffer[0]; if (buffer[1] & 0x80) acc_xyz[1] = buffer[1] - 256; else acc_xyz[1] = buffer[1]; if (buffer[2] & 0x80) acc_xyz[2] = buffer[2] - 256; else acc_xyz[2] = buffer[2]; #else if (buffer[0] & 0x80) acc_xyz[0] = ((int)buffer[0]<<4) + (buffer[1]>>4) - 4096; else acc_xyz[0] = ((int)buffer[0]<<4) + (buffer[1]>>4); if (buffer[2] & 0x80) acc_xyz[1] = ((int)buffer[2]<<4) + (buffer[3]>>4) - 4096; else acc_xyz[1] = ((int)buffer[2]<<4) + (buffer[3]>>4); if (buffer[4] & 0x80) acc_xyz[2] = ((int)buffer[4]<<4) + (buffer[5]>>4) - 4096; else acc_xyz[2] = ((int)buffer[4]<<4) + (buffer[5]>>4); #endif DBG("ACC data: %d %d %dn", acc_xyz[0], acc_xyz[1], acc_xyz[2]); if(event_since_en == 16 || event_since_en == 17) STK831x_CheckReading(acc_xyz, false); else if(event_since_en == 18) STK831x_CheckReading(acc_xyz, true); #ifdef CONFIG_SENSORS_STK8312 else if(event_since_en_limit == 21) STK831x_CheckCode(acc_xyz); #endif #ifdef CONFIG_SENSORS_STK8313 if(((getenable==1) && (event_since_en_limit==20)) || (k_status == STK_K_RUNNING)) { acc_xyz[0] += stk_driver_offset[0]; acc_xyz[1] += stk_driver_offset[1]; acc_xyz[2] += stk_driver_offset[2]; } #endif if(k_status == STK_K_RUNNING) { cali_raw_data[0] = acc_xyz[0]; cali_raw_data[1] = acc_xyz[1]; cali_raw_data[2] = acc_xyz[2]; return 0; } #ifdef STK_TUNE if((k_status&0xF0) != 0) STK831x_Tune(client, acc_xyz); #endif if(fir_en) { if(fir.num < firlength) { fir.raw[fir.num][0] = acc_xyz[0]; fir.raw[fir.num][1] = acc_xyz[1]; fir.raw[fir.num][2] = acc_xyz[2]; fir.sum[0] += acc_xyz[0]; fir.sum[1] += acc_xyz[1]; fir.sum[2] += acc_xyz[2]; fir.num++; fir.idx++; } else { idx = fir.idx % firlength; fir.sum[0] -= fir.raw[idx][0]; fir.sum[1] -= fir.raw[idx][1]; fir.sum[2] -= fir.raw[idx][2]; fir.raw[idx][0] = acc_xyz[0]; fir.raw[idx][1] = acc_xyz[1]; fir.raw[idx][2] = acc_xyz[2]; fir.sum[0] += acc_xyz[0]; fir.sum[1] += acc_xyz[1]; fir.sum[2] += acc_xyz[2]; fir.idx++; acc_xyz[0] = fir.sum[0]/firlength; acc_xyz[1] = fir.sum[1]/firlength; acc_xyz[2] = fir.sum[2]/firlength; } } #ifdef STK_DEBUG_RAWDATA DBG("%s:After FIR %4d,%4d,%4dn", __func__, acc_xyz[0], acc_xyz[1], acc_xyz[2]); #endif show_raw_data[0] = acc_xyz[0]; show_raw_data[1] = acc_xyz[1]; show_raw_data[2] = acc_xyz[2]; #ifdef STK_ZG_FILTER if( abs(acc_xyz[0]) <= STK_ZG_COUNT) acc_xyz[0] = (acc_xyz[0]*zero_fir); if( abs(acc_xyz[1]) <= STK_ZG_COUNT) acc_xyz[1] = (acc_xyz[1]*zero_fir); if( abs(acc_xyz[2]) <= STK_ZG_COUNT) acc_xyz[2] = (acc_xyz[2]*zero_fir); #endif /* #ifdef STK_ZG_FILTER */ x = acc_xyz[0] * STK831X_GRAVITY_STEP; y = acc_xyz[1] * STK831X_GRAVITY_STEP; z = acc_xyz[2] * STK831X_GRAVITY_STEP; axis.x = ((pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z); axis.y = ((pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z); axis.z = ((pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z); if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) { gsensor_report_value(client, &axis); mutex_lock(&(sensor->data_mutex) ); sensor->axis = axis; mutex_unlock(&(sensor->data_mutex) ); } //===========================DBG reg============== //show_all_reg(client); //================================================ return ret; }
|
|
相关推荐 |
|
只有小组成员才能发言,加入小组>>
9681 浏览 19 评论
9869 浏览 0 评论
3867 浏览 0 评论
8754 浏览 8 评论
2412 浏览 1 评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-10-12 23:20 , Processed in 0.613666 second(s), Total 98, Slave 79 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号