static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, int size)
{
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);
// TODO handle missed samples etc.
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] };
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;
}
}
static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, int size)
{
uint32_t last_sample_tick = priv->sensor.tick;
if(!dp_decode_tracker_sensor_msg(&priv->sensor, buffer, size)){
LOGE("couldn't decode tracker sensor message");
}
pkt_tracker_sensor* s = &priv->sensor;
dp_dump_packet_tracker_sensor(s);
uint32_t tick_delta = 1000;
if(last_sample_tick > 0) //startup correction
tick_delta = s->tick - last_sample_tick;
float dt = tick_delta * TICK_LEN;
vec3f mag = {{0.0f, 0.0f, 0.0f}};
for(int i = 0; i < 1; i++){ //just use 1 sample since we don't have sample order for this frame
vec3f_from_dp_vec(s->samples[i].accel, &priv->raw_accel);
vec3f_from_dp_vec(s->samples[i].gyro, &priv->raw_gyro);
ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &mag);
// reset dt to tick_len for the last samples if there were more than one sample
dt = TICK_LEN;
}
}
