高通sensor坐标轴的转换

高通的sensor系统中，可以让我们在adsp中转换sensor的坐标系。由于sensor贴的位置，可能与我们的android要求的坐标系不同，那么需要转换，有的芯片可以向里面写寄存器，让芯片来帮我们转换，高通的也可以在dd driver里面来进行转换。

这里转换坐标系的信息，我们可以保存到nv里面，也就是那个sns_reg文件里面，在初始化的时候会通过init函数传递进来：

sns_ddf_status_e (*init)(
        sns_ddf_handle_t*         dd_handle_ptr,
        sns_ddf_handle_t         smgr_handle,
        sns_ddf_nv_params_s*   nv_params,
        sns_ddf_device_access_s   device_info[],
        uint32_t                 num_devices,
        sns_ddf_memhandler_s*     memhandler,
        sns_ddf_sensor_e**       sensors,
        uint32_t*                 num_sensors

);

我们先讲讲使用情况，是如何对应的。

传递进来的参数如下：{1， 2， 3}，那么raw data就按照x，y，z 传递上去；

如果传递进来的参数是：{-1， 2， -3}， 那么raw data 就按照 -x， y， -z传递上去；

如果传递进来的参数是：{-2， 3， -1}， 那么 x 对应的是raw数据的 -y， y 对应的是raw数据的 z， 在对应的是raw数据的 -x

总结一下他们的对应关系是， 数字代表的是raw数组里面的索引，正负符号代表了数据是否反向。

下面看看代码：

[cpp] view plain copy

print?

1. void sns_ddf_axes_map_init(sns_ddf_axes_map_s* axes_map, uint8_t* registry_data)  
2. {  
3.     const int8_t* registry_axes_map = (int8_t*)registry_data;  
4.   
5.     if(!sns_ddf_axes_map_is_reg_data_valid(registry_axes_map))  
6.     {  
7.         // Default to no change in orientation.  
8.         axes_map->indx_x = 0;  
9.         axes_map->indx_y = 1;  
10.         axes_map->indx_z = 2;  
11.         axes_map->sign_x = 1;  
12.         axes_map->sign_y = 1;  
13.         axes_map->sign_z = 1;  
14.         return;  
15.     }  
16.   
17.     axes_map->sign_x = SIGN_OF_INT8(registry_axes_map[0]);  
18.     axes_map->indx_x = registry_axes_map[0] * axes_map->sign_x - 1;  
19.   
20.     axes_map->sign_y = SIGN_OF_INT8(registry_axes_map[1]);  
21.     axes_map->indx_y = registry_axes_map[1] * axes_map->sign_y - 1;  
22.   
23.     axes_map->sign_z = SIGN_OF_INT8(registry_axes_map[2]);  
24.     axes_map->indx_z = registry_axes_map[2] * axes_map->sign_z - 1;  
25. }  

void sns_ddf_axes_map_init(sns_ddf_axes_map_s* axes_map, uint8_t* registry_data){    const int8_t* registry_axes_map = (int8_t*)registry_data;    if(!sns_ddf_axes_map_is_reg_data_valid(registry_axes_map))    {        // Default to no change in orientation.        axes_map->indx_x = 0;        axes_map->indx_y = 1;        axes_map->indx_z = 2;        axes_map->sign_x = 1;        axes_map->sign_y = 1;        axes_map->sign_z = 1;        return;    }    axes_map->sign_x = SIGN_OF_INT8(registry_axes_map[0]);    axes_map->indx_x = registry_axes_map[0] * axes_map->sign_x - 1;    axes_map->sign_y = SIGN_OF_INT8(registry_axes_map[1]);    axes_map->indx_y = registry_axes_map[1] * axes_map->sign_y - 1;    axes_map->sign_z = SIGN_OF_INT8(registry_axes_map[2]);    axes_map->indx_z = registry_axes_map[2] * axes_map->sign_z - 1;}

// Return -1 if an int8_t is negative, or +1 otherwise.
#define SIGN_OF_INT8(x) (1 | ((x) >> 7))

这个宏定义就只是取出了符号，代表正负，后面使用。

所以如果传递的是 -3，那么 axes_map->sign_x = -1;

这些索引index呢，我们可以算算，如果是 -1或者1， 就是 （1*1） - 1 = 0；或者 （-1 * -1） - 1 = 0；

同样的道理，如果是 -3或者3，那么就是 （3 * 1） - 1 = 2；或者 （-3 * -1） - 1 = 2；

这些index就是将来在raw数组里面寻找数据的数组下标了。

那我们在传递数据的时候，在调用了如下的函数：

[cpp] view plain copy

print?

1. void sns_ddf_map_axes(sns_ddf_axes_map_s* axes_map, q16_t* device_data)  
2. {  
3.     static q16_t tmp[3];  
4.   
5.     SNS_OS_MEMCOPY(tmp, device_data, sizeof(tmp));  
6.     device_data[0] = tmp[axes_map->indx_x] * axes_map->sign_x;  
7.     device_data[1] = tmp[axes_map->indx_y] * axes_map->sign_y;  
8.     device_data[2] = tmp[axes_map->indx_z] * axes_map->sign_z;  
9. }  

void sns_ddf_map_axes(sns_ddf_axes_map_s* axes_map, q16_t* device_data){    static q16_t tmp[3];    SNS_OS_MEMCOPY(tmp, device_data, sizeof(tmp));    device_data[0] = tmp[axes_map->indx_x] * axes_map->sign_x;    device_data[1] = tmp[axes_map->indx_y] * axes_map->sign_y;    device_data[2] = tmp[axes_map->indx_z] * axes_map->sign_z;}

device_data是从sensor寄存器里面读取出来的数据，那么我们把上面计算出来的index作为下标，来进行对应，是否反向就用sign来相乘，这就这样我们的坐标系就完成了转换了。

原理很简单，我们的sensor包括加速度，陀螺仪和地磁都需要坐标轴的转换，前面讲到是把这些nv参数传递进来初始化的。那么他们存在nv的那个位置呢，在sns_reg_api_v02.h这个文件中，我们定义了如下的一下宏定义：

[cpp] view plain copy

print?

1. /**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_ACCEL (Group Id: 1000)  */  
2. #define SNS_REG_DRIVER_ACCEL_X_ORIENT_V02 700  
3. #define SNS_REG_DRIVER_ACCEL_Y_ORIENT_V02 701  
4. #define SNS_REG_DRIVER_ACCEL_Z_ORIENT_V02 702  
5.   
6. /**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_GYRO (Group Id: 1010)  */  
7. #define SNS_REG_DRIVER_GYRO_X_ORIENT_V02 800  
8. #define SNS_REG_DRIVER_GYRO_Y_ORIENT_V02 801  
9. #define SNS_REG_DRIVER_GYRO_Z_ORIENT_V02 802  
10.   
11. /**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_MAG (Group Id: 1020)  */  
12. #define SNS_REG_DRIVER_MAG_X_ORIENT_V02 900  
13. #define SNS_REG_DRIVER_MAG_Y_ORIENT_V02 901  
14. #define SNS_REG_DRIVER_MAG_Z_ORIENT_V02 902  

/**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_ACCEL (Group Id: 1000)  */#define SNS_REG_DRIVER_ACCEL_X_ORIENT_V02 700#define SNS_REG_DRIVER_ACCEL_Y_ORIENT_V02 701#define SNS_REG_DRIVER_ACCEL_Z_ORIENT_V02 702/**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_GYRO (Group Id: 1010)  */#define SNS_REG_DRIVER_GYRO_X_ORIENT_V02 800#define SNS_REG_DRIVER_GYRO_Y_ORIENT_V02 801#define SNS_REG_DRIVER_GYRO_Z_ORIENT_V02 802/**  Item ID's corresponding to group SNS_REG_DRIVER_GROUP_MAG (Group Id: 1020)  */#define SNS_REG_DRIVER_MAG_X_ORIENT_V02 900#define SNS_REG_DRIVER_MAG_Y_ORIENT_V02 901#define SNS_REG_DRIVER_MAG_Z_ORIENT_V02 902

这些地址里面，存储的就是对应senosr的坐标系转换参数，我们可以用QSensorTest.apk这个程序来读取一下，刚好就是传递进dsps的nv参数。那么就有两个问题了，这些nv参数是哪里写入的呢，驱动里面又是哪里请求的这些nv项呢。

我们先来看后面一个问题，我们按照这些group ID 进行寻找，发现在sns_smgr_sensor_config.h这个文件中，最每一个sensor的相关信息进行了定义，也包括了需要的nv参数的信息，下面是一个加速度的信息摘取：

[cpp] view plain copy

print?

1. #ifdef CONFIG_USE_LIS3DH  
2.   #define SNS_SMGR_SENSOR_0_ENUM_CODE                   SNS_SMGR_ID_ACCEL_V01  
3.   #define SNS_SMGR_SENSOR_0_DD_FN_LIST_PTR              &sns_dd_acc_lis3dh_if  
4.   #define SNS_SMGR_SENSOR_0_DEVICE_ID                   0  
5.   #define SNS_SMGR_SENSOR_0_BUS_ADDRESS                 0x19 /* 0x1D alternative I2C address. ranfei modify */  
6.   #define SNS_SMGR_SENSOR_0_DATA_TYPE_1                 SNS_DDF_SENSOR_ACCEL  
7.   #define SNS_SMGR_SENSOR_0_DATA_TYPE_2                 SNS_DDF_SENSOR__NONE  
8.   #define SNS_SMGR_SENSOR_0_RANGE_TYPE                  SNS_SMGR_DATA_TYPE_PRIMARY_V01  
9.   #define SNS_SMGR_SENSOR_0_SENSITIVITY_DEFAULT         1 /* 16g fixed. */  
10.   #define SNS_SMGR_SENSOR_0_FLAGS                       SNS_SMGR_NO_SENSITIVITY //RK check this with SMGR requirements  
11.   #define SNS_SMGR_SENSOR_0_OFF_TO_IDLE                 1000 /* usec */ // RK check this with data sheet  
12.   #define SNS_SMGR_SENSOR_0_IDLE_TO_READY               100000 /* usec */  
13.   #define SNS_SMGR_SENSOR_0_REG_ITEM_TYPE               SNS_SMGR_REG_ITEM_TYPE_GROUP  
14.   #define SNS_SMGR_SENSOR_0_REG_ITEM_ID                 SNS_REG_DRIVER_GROUP_ACCEL_V02  
15.   #define SNS_SMGR_SENSOR_0_CAL_PRI_TYPE                SNS_SMGR_REG_ITEM_TYPE_GROUP  
16.   #define SNS_SMGR_SENSOR_0_CAL_PRI_ID                  SNS_REG_SCM_GROUP_ACCEL_FAC_CAL_PARAMS_V02  /* 0=SNS_REG_SMGR_GROUP_ACCEL_V02 in sns_reg_api_v02.h */  
17.   #define SNS_SMGR_SENSOR_0_GPIO_FIRST                  61    //ranfei modify  
18.   #define SNS_SMGR_SENSOR_0_GPIO_SECOND                 0xffff  
19. #endif /* CONFIG_USE_LIS3DH */  

#ifdef CONFIG_USE_LIS3DH  #define SNS_SMGR_SENSOR_0_ENUM_CODE                   SNS_SMGR_ID_ACCEL_V01  #define SNS_SMGR_SENSOR_0_DD_FN_LIST_PTR              &sns_dd_acc_lis3dh_if  #define SNS_SMGR_SENSOR_0_DEVICE_ID                   0  #define SNS_SMGR_SENSOR_0_BUS_ADDRESS                 0x19 /* 0x1D alternative I2C address. ranfei modify */  #define SNS_SMGR_SENSOR_0_DATA_TYPE_1                 SNS_DDF_SENSOR_ACCEL  #define SNS_SMGR_SENSOR_0_DATA_TYPE_2                 SNS_DDF_SENSOR__NONE  #define SNS_SMGR_SENSOR_0_RANGE_TYPE                  SNS_SMGR_DATA_TYPE_PRIMARY_V01  #define SNS_SMGR_SENSOR_0_SENSITIVITY_DEFAULT         1 /* 16g fixed. */  #define SNS_SMGR_SENSOR_0_FLAGS                       SNS_SMGR_NO_SENSITIVITY //RK check this with SMGR requirements  #define SNS_SMGR_SENSOR_0_OFF_TO_IDLE                 1000 /* usec */ // RK check this with data sheet  #define SNS_SMGR_SENSOR_0_IDLE_TO_READY               100000 /* usec */  #define SNS_SMGR_SENSOR_0_REG_ITEM_TYPE               SNS_SMGR_REG_ITEM_TYPE_GROUP  #define SNS_SMGR_SENSOR_0_REG_ITEM_ID                 SNS_REG_DRIVER_GROUP_ACCEL_V02  #define SNS_SMGR_SENSOR_0_CAL_PRI_TYPE                SNS_SMGR_REG_ITEM_TYPE_GROUP  #define SNS_SMGR_SENSOR_0_CAL_PRI_ID                  SNS_REG_SCM_GROUP_ACCEL_FAC_CAL_PARAMS_V02  /* 0=SNS_REG_SMGR_GROUP_ACCEL_V02 in sns_reg_api_v02.h */  #define SNS_SMGR_SENSOR_0_GPIO_FIRST                  61    //ranfei modify  #define SNS_SMGR_SENSOR_0_GPIO_SECOND                 0xffff#endif /* CONFIG_USE_LIS3DH */

可以看到 #define SNS_SMGR_SENSOR_0_REG_ITEM_ID                 SNS_REG_DRIVER_GROUP_ACCEL_V02

这个正是前面定义的加速度坐标系矫正的nv group ID了。在smgr里面调用 sns_err_code_e sns_smgr_req_reg_data( const uint16_t Id, const uint8_t Type )；就可以请求读取相应的nv参数了；