openHMD-simple代码分析(2/2)

接上篇内容，这里主要以oculars在openHMD中的提交，来分析oculars的DK1/DK2在openHMD中是如何运行的。
simple代码的主流程看上篇文章”openHMD-simple代码分析(1/2)”。

打开设备:ohmd_list_open_device

    //打开设备列表中第一个设备
    ohmd_device* hmd = ohmd_list_open_device(ctx, 0);

    if(!hmd){
        printf("failed to open device: %s\n", ohmd_ctx_get_error(ctx));
        return -1;
    }

ohmd_list_open_device函数：

ohmd_device* OHMD_APIENTRY ohmd_list_open_device(ohmd_context* ctx, int index)
{
    ohmd_device_settings settings;
    //设置设备数据的更新方式:
    //false:需要手动调用ohmd_ctx_update来更新数据
    //true:创建线程，调用设备的update方法来更新数据(如rift.c里的update_devices)
    settings.automatic_update = true;

    return ohmd_list_open_device_s(ctx, index, &settings);
}

ohmd_list_open_device_s函数：

ohmd_device* OHMD_APIENTRY ohmd_list_open_device_s(ohmd_context* ctx, int index, ohmd_device_settings* settings)
{
    ohmd_lock_mutex(ctx->update_mutex);
    //轮询所有设备
    if(index >= 0 && index < ctx->list.num_devices){

        ohmd_device_desc* desc = &ctx->list.devices[index];
        ohmd_driver* driver = (ohmd_driver*)desc->driver_ptr;
        ohmd_device* device = driver->open_device(driver, desc);

        if (device == NULL)
            return NULL;
        //填充device参数值
        device->rotation_correction.w = 1;

        device->settings = *settings;

        device->ctx = ctx;
        device->active_device_idx = ctx->num_active_devices;
        ctx->active_devices[ctx->num_active_devices++] = device;

        ohmd_unlock_mutex(ctx->update_mutex);

        if(device->settings.automatic_update)
            //创建线程，用来更新上报的数据。
            ohmd_set_up_update_thread(ctx);

        return device;
    }

    ohmd_unlock_mutex(ctx->update_mutex);
    //如果没有设备则返回错误
    ohmd_set_error(ctx, "no device with index: %d", index);
    return NULL;
}

其中ohmd_set_up_update_thread会创建更新数据的线程ohmd_update_thread，这个函数主要用来读取sensor数据，调用fusion算法，将raw数据处理为四元素。

static void ohmd_set_up_update_thread(ohmd_context* ctx)
{
    if(!ctx->update_thread){
        ctx->update_mutex = ohmd_create_mutex(ctx);
        ctx->update_thread = ohmd_create_thread(ctx, ohmd_update_thread, ctx);
    }
}

static unsigned int ohmd_update_thread(void* arg)
{
    ohmd_context* ctx = (ohmd_context*)arg;
    //ctx->update_request_quit 控制循环是否结束，一般在ohmd_ctx_destroy里设置为true
    while(!ctx->update_request_quit)
    {
        ohmd_lock_mutex(ctx->update_mutex);
        //调用所有处于激活状态的设备的update方法来循环更新数据(如rift.c里的update_device)
        for(int i = 0; i < ctx->num_active_devices; i++){
            if(ctx->active_devices[i]->settings.automatic_update && ctx->active_devices[i]->update)
                ctx->active_devices[i]->update(ctx->active_devices[i]);
        }

        ohmd_unlock_mutex(ctx->update_mutex);
        //休眠一段时间，释放CPU
        ohmd_sleep(AUTOMATIC_UPDATE_SLEEP);
    }

    return 0;
}

ctx->active_devices[i]->update(ctx->active_devices[i])实际上是调用到具体设备中的数据更新的接口中,这里以oculars的DK1/DK2为例进行说明数据的更新

static void update_device(ohmd_device* device)
{
    rift_priv* priv = rift_priv_get(device);
    unsigned char buffer[FEATURE_BUFFER_SIZE];

    //处理心跳包信息:按照keep_alive_interval参数值(默认1秒)为间隔，发送心跳包给HMD
    double t = ohmd_get_tick();
    if(t - priv->last_keep_alive >= (double)priv->sensor_config.keep_alive_interval / 1000.0 - .2){
        // send keep alive message
        pkt_keep_alive keep_alive = { 0, priv->sensor_config.keep_alive_interval };
        int ka_size = encode_keep_alive(buffer, &keep_alive);
        send_feature_report(priv, buffer, ka_size);

        // Update the time of the last keep alive we have sent.
        priv->last_keep_alive = t;
    }

    //处理HMD上报的所有数据
    while(true){
        //通过HID接口读取HMD的数据
        int size = hid_read(priv->handle, buffer, FEATURE_BUFFER_SIZE);
        if(size < 0){
            LOGE("error reading from device");
            return;
        } else if(size == 0) {//数据读完退出
            return; 
        }

        //根据oculars协议，对上报数据进行处理
        if(buffer[0] == RIFT_IRQ_SENSORS){
            handle_tracker_sensor_msg(priv, buffer, size);
        }else{
            LOGE("unknown message type: %u", buffer[0]);
        }
    }
}

其中handle_tracker_sensor_msg函数如下：

static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, int size)
{
    //按照oculars的数据协议解析buffer中的数据填充到priv->sensor中
    if(!decode_tracker_sensor_msg(&priv->sensor, buffer, size)){
        LOGE("couldn't decode tracker sensor message");
    }

    pkt_tracker_sensor* s = &priv->sensor;
    //打印调试信息
    dump_packet_tracker_sensor(s);

    //根据采样数设置dt值(一般采样数都在3以内)
    float dt = s->num_samples > 3 ? (s->num_samples - 2) * TICK_LEN : TICK_LEN;

    int32_t mag32[] = { s->mag[0], s->mag[1], s->mag[2] };
    //将上报数据转为浮点数(HMD上报时不能直接上报浮点数，所以先*10000转化为整数上报)
    //这里*0.0001相当于做了还原).
    vec3f_from_rift_vec(mag32, &priv->raw_mag);

    for(int i = 0; i < OHMD_MIN(s->num_samples, 3); i++){
        //将上报数据转为浮点数
        vec3f_from_rift_vec(s->samples[i].accel, &priv->raw_accel);
        //将上报数据转为浮点数
        vec3f_from_rift_vec(s->samples[i].gyro, &priv->raw_gyro);
        //融合算法
        ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &priv->raw_mag);

        // reset dt to tick_len for the last samples if there were more than one sample
        dt = TICK_LEN;
    }
}

主要是解析HMD上报的数据，关于ofusion_update融合算法，中间的四元素操作比较复杂，可以看这篇老外的文章IMU指南，需要花点时间琢磨。

数据更新：ohmd_ctx_update

void OHMD_APIENTRY ohmd_ctx_update(ohmd_context* ctx)
{
    for(int i = 0; i < ctx->num_active_devices; i++){
        ohmd_device* dev = ctx->active_devices[i];
        //如果设备对应驱动中没有创建线程实时刷新数据则，通过这里刷新数据。
        //(Oculars DK1是通过线程刷新数据的.
        if(!dev->settings.automatic_update && dev->update)
            dev->update(dev);

        ohmd_lock_mutex(ctx->update_mutex);
        //获取位置信息填充到dev->position中
        dev->getf(dev, OHMD_POSITION_VECTOR, (float*)&dev->position);
        //获取经过融合算法的orient数据填充到dev->rotation中。
        dev->getf(dev, OHMD_ROTATION_QUAT, (float*)&dev->rotation);
        ohmd_unlock_mutex(ctx->update_mutex);
    }
}

获取四元素：ohmd_device_setf/ohmd_device_getf

        float zero[] = {.0, .1, .2, 1};
        //计算QUAT和VECTOR的校准因子correction
        ohmd_device_setf(hmd, OHMD_ROTATION_QUAT, zero);
        ohmd_device_setf(hmd, OHMD_POSITION_VECTOR, zero);
        //利用上面计算的校准因子，来得出最后的四元素并打印出来。
        print_infof(hmd, "rotation quat:", 4, OHMD_ROTATION_QUAT);