Commit 046c9e18 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz
Browse files

d/psmv: Fix timestamp parsing (ZCM2)

parent 63d62cf4
Pipeline #65123 passed with stages
in 2 minutes and 33 seconds
...@@ -358,15 +358,17 @@ struct psmv_input_zcm2 ...@@ -358,15 +358,17 @@ struct psmv_input_zcm2
uint8_t trigger; uint8_t trigger;
uint8_t trigger_low_pass; uint8_t trigger_low_pass;
uint8_t pad0[4]; uint8_t pad0[4];
uint8_t timestamp_high; uint8_t timestamp_high_copy;
uint8_t battery; uint8_t battery;
struct psmv_vec3_i16_wire accel; struct psmv_vec3_i16_wire accel;
struct psmv_vec3_i16_wire accel_copy; struct psmv_vec3_i16_wire accel_copy;
struct psmv_vec3_i16_wire gyro; struct psmv_vec3_i16_wire gyro;
struct psmv_vec3_i16_wire gyro_copy; struct psmv_vec3_i16_wire gyro_copy;
uint8_t temp[2]; uint8_t temp[2];
uint8_t pad1[2];
uint8_t timestamp_low; uint8_t timestamp_low;
uint8_t timestamp_high;
uint8_t pad1[2];
uint8_t timestamp_low_copy;
}; };
/*! /*!
...@@ -385,6 +387,7 @@ struct psmv_parsed_input ...@@ -385,6 +387,7 @@ struct psmv_parsed_input
{ {
uint32_t buttons; uint32_t buttons;
uint16_t timestamp; uint16_t timestamp;
uint16_t timestamp_copy;
uint8_t battery; uint8_t battery;
uint8_t seq_no; uint8_t seq_no;
...@@ -1493,9 +1496,12 @@ psmv_parse_input_zcm2(struct psmv_device *psmv, ...@@ -1493,9 +1496,12 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
input->buttons |= data->buttons[1] << 16; input->buttons |= data->buttons[1] << 16;
input->buttons |= data->buttons[2] << 8; input->buttons |= data->buttons[2] << 8;
input->buttons |= data->buttons[3] & 0xf0; input->buttons |= data->buttons[3] & 0xf0;
input->timestamp |= data->timestamp_low; input->timestamp = 0;
input->timestamp |= data->timestamp_high << 8; input->timestamp |= (uint16_t)data->timestamp_low;
input->timestamp |= ((uint16_t)data->timestamp_high) << 8;
input->timestamp_copy = 0;
input->timestamp_copy |= (uint16_t)data->timestamp_low_copy;
input->timestamp_copy |= ((uint16_t)data->timestamp_high_copy) << 8;
input->trigger_low_pass = data->trigger_low_pass; input->trigger_low_pass = data->trigger_low_pass;
input->trigger = data->trigger; input->trigger = data->trigger;
...@@ -1525,7 +1531,8 @@ psmv_parse_input_zcm2(struct psmv_device *psmv, ...@@ -1525,7 +1531,8 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
"sample_copy.gyro: %6i %6i %6i\n\t" "sample_copy.gyro: %6i %6i %6i\n\t"
"sample.trigger: %02x\n\t" "sample.trigger: %02x\n\t"
"sample.trigger_low_pass: %02x\n\t" "sample.trigger_low_pass: %02x\n\t"
"timestamp: %i\n\t" "timestamp: %04x\n\t"
"timestamp_copy: %04x\n\t"
"diff: %i\n\t" "diff: %i\n\t"
"seq_no: %x\n", "seq_no: %x\n",
missed ? "yes" : "no", input->buttons, input->battery, missed ? "yes" : "no", input->buttons, input->battery,
...@@ -1536,7 +1543,7 @@ psmv_parse_input_zcm2(struct psmv_device *psmv, ...@@ -1536,7 +1543,7 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
input->samples[0].gyro.z, input->samples[1].gyro.x, input->samples[0].gyro.z, input->samples[1].gyro.x,
input->samples[1].gyro.y, input->samples[1].gyro.z, input->samples[1].gyro.y, input->samples[1].gyro.z,
input->trigger_low_pass, input->trigger, input->timestamp, input->trigger_low_pass, input->trigger, input->timestamp,
diff, input->seq_no); input->timestamp_copy, diff, input->seq_no);
return 1; return 1;
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment