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);