//加入需要的宏定义

#define  ID_BASE           SENSORS_HANDLE_BASE

#define  ID_ACCELERATION   (ID_BASE+0)

#define  ID_MAGNETIC_FIELD (ID_BASE+1)

#define  ID_ORIENTATION (ID_BASE+2)

#define S_HANDLE_ACCELEROMETER      (1<

#define S_HANDLE_MAGNETIC_FIELD           (1<

#define S_HANDLE_ORIENTATION                 (1<

#define SENSORS_NUM 4

#define SUPPORTED_SENSORS  ((1<

//在 sensor_t sensors_list[] 中添加两个sensor的信息,

//这些只是一些Sensor的信息，应用程序可以获取到。

#ifdef MAGNETIC_FIELD

{

name       : "XXX 3-axis Magnetic field sensor",

vendor    : "XXX company",

version    : 1,

handle     : S_HANDLE_MAGNETIC_FIELD,

type       : SENSOR_TYPE_MAGNETIC_FIELD,

maxRange   : 600.0f,//最大范围

resolution : 30.0f,//最小分辨率

power      : 6.7f,//这个不太懂

},

#endif

#ifdef ORIENTATION

{

name: "XXX Orientation sensor",

vendor: "XXX company",

version: 1,

handle: S_HANDLE_ORIENTATION,

type: SENSOR_TYPE_ORIENTATION,

maxRange: 360,

resolution: 0.1,

power: 20,

},

#endif

//定义一个结构来保存orientation的信息

static struct orientation{

float azimuth;

float pitch;

float roll;

}orientation;

//在 control__open_data_source()函数中打开设备

static native_handle_t*

control__open_data_source(struct sensors_control_device_t *dev)

{

SensorControl*  ctl = (void*)dev;

native_handle_t* handle;

int fd_m = open (MAGNETIC_DATA_DEVICE, O_RDONLY);

LOGD ("Open Magnetic Data source: %d, %d/n", fd_m, errno);

if (fd_m>= 0)

{

dev->fd[ID_MAGNETIC_FIELD] = dup(fd_m);

}

return handle;

}

//实现数据的打开和关闭函数

static int

data__data_open(struct sensors_data_device_t *dev, native_handle_t* handle)

{

struct sensors_data_context_t *dev;

dev = (struct sensors_data_context_t *)device;

for(int i=0 ;i

{

dev->fd[i] = dup(handle->data[i]);

}

native_handle_close(handle);

native_handle_delete(handle);

return 0;

}

static int

data__data_close(struct sensors_data_device_t *dev)

{

struct sensors_data_context_t *dev;

dev = (struct sensors_data_context_t *)device;

for(int i=0 ;i

{

if (dev->fd[i] >= 0)

{

close(dev->fd[i]);

}

dev->fd[i] = -1;

}

return 0;

}

//最关键的poll函数

static int

data__poll(struct sensors_data_device_t *dev, sensors_data_t* values)

{

SensorData*  data = (void*)dev;

int fd = data->events_fd;

//判断设备是否打开

if(dev->fd[ID_MAGNETIC_FIELD] < 0)

{

LOGD("In %s dev[%d] is not open!/n",__FUNCTION__ ,ID_MAGNETIC_FIELD);

return -1;

}

pollfd pfd[SENSORS_NUM] =

{

//省略其他sensor代码

{

fd: dev->fd[ID_MAGNETIC_FIELD],

events: POLLIN,

revents: 0

},

//省略其他sensor代码

};

int err = poll (pfd, SENSORS_NUM, s_timeout);

unsigned int  mask = SUPPORTED_SENSORS;

static unsigned int poll_flag=0;

if(poll_flag==0)

{

poll_flag = mask;

}

//省略其他sensor

if(poll_flag&(1<

{

if((pfd[ID_MAGNETIC_FIELD].revents&POLLIN) == POLLIN)

{

char rawData[6];

err = read (dev->fd[ID_MAGNETIC_FIELD], &rawData, sizeof(rawData));

if(err<0)

{

LOGE("read magnetic field ret:%d errno:%d/n", err, errno);

return err;

}

struct timespec t;

clock_gettime(CLOCK_REALTIME, &t);

data->time = timespec_to_ns(&t);

data->sensor = SENSOR_TYPE_MAGNETIC_FIELD;

data->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;

//上报的数据单位要转换成 uTesla

data->magnetic.x = ( (rawData[1] << 8 ) | rawData[0])/ MAGNETIC_CONVERT;

data->magnetic.y = ( (rawData[3] << 8 ) | rawData[2])/ MAGNETIC_CONVERT;

data->magnetic.z = ( (rawData[5] << 8 ) | rawData[4])/ MAGNETIC_CONVERT;

//把陀螺仪需要的数据计算出来，用atan2()，头文件要加上#include

float azimuth = atan2(  (float)(data->magnetic.x ),(float)(data->magnetic.y) );

if(azimuth<0)

{

azimuth = 360 - fabs(azimuth*180/PI);

}

else

{

azimuth = azimuth*180/PI;

}

orientation.azimuth = 360-azimuth;

//rotation around the X axis.+180~-180 degree

orientation.pitch = atan2( (float)(data->magnetic.y ),(float)(data->magnetic.z)

)*180/PI;

//rotation around the Y axis +90~-90 degree

float roll = atan2( (float)(data->magnetic.x ),(float)(data->magnetic.z) )

*180/PI;

if (roll > 90)

{

roll = -(180.0-roll);

}

else if (roll < -90)

{

roll = 180 + roll;

}

orientation.roll =  roll;

}

return S_HANDLE_MAGNETIC_FIELD;

}

if(poll_flag&(1<

{

//数据已经计算好了直接上报就行

struct timespec t;

clock_gettime(CLOCK_REALTIME, &t);

data->time = timespec_to_ns(&t);

data->sensor = SENSOR_TYPE_ORIENTATION;

data->orientation.azimuth = orientation.azimuth;

data->orientation.pitch = orientation.pitch;

data->orientation.roll = orientation.roll;

poll_flag &= ~(1<

return S_HANDLE_ORIENTATION;

}

}