高通sensor坐标轴的转换

74 阅读 0 评论 49 点赞

我是靠谱客的博主仁爱电脑，最近开发中收集的这篇文章主要介绍高通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数组里面的索引，正负符号代表了数据是否反向。

下面看看代码：

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数组里面寻找数据的数组下标了。

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

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这个文件中，我们定义了如下的一下宏定义：

/**  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参数的信息，下面是一个加速度的信息摘取：

#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参数了；