Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Monado
Monado
Commits
548f4ede
Commit
548f4ede
authored
Sep 15, 2019
by
Jakob Bornecrantz
Browse files
d/psmv: Don't use the copy sample on ZCM2
parent
9d1353c6
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/xrt/drivers/psmv/psmv_driver.c
View file @
548f4ede
...
...
@@ -318,7 +318,6 @@ struct psmv_parsed_sample
{
struct
xrt_vec3_i32
accel
;
struct
xrt_vec3_i32
gyro
;
uint8_t
trigger
;
};
/*!
...
...
@@ -331,7 +330,34 @@ struct psmv_parsed_input
uint8_t
battery
;
uint8_t
seq_no
;
struct
psmv_parsed_sample
sample
[
2
];
union
{
//! Trigger for the last two frames (ZCM1).
uint8_t
trigger_values
[
2
];
struct
{
//! Low-pass filtered versio of trigger (ZCM2).
uint8_t
trigger_low_pass
;
//! Trigger (ZCM2).
uint8_t
trigger
;
};
};
union
{
//! Accelerometer and gyro scope samples (ZCM1).
struct
psmv_parsed_sample
samples
[
2
];
struct
{
//! Accelerometer and gyro scope samples (ZCM2).
struct
psmv_parsed_sample
sample
;
//! Copy of above (ZCM2).
struct
psmv_parsed_sample
sample_copy
;
};
};
};
/*!
...
...
@@ -490,7 +516,7 @@ struct psmv_device
static
int
psmv_get_calibration
(
struct
psmv_device
*
psmv
);
static
void
static
int
psmv_parse_input
(
struct
psmv_device
*
psmv
,
void
*
data
,
struct
psmv_parsed_input
*
input
);
...
...
@@ -551,8 +577,7 @@ static void
psmv_update_trigger_value
(
struct
psmv_device
*
psmv
,
int
index
,
int64_t
now
)
{
psmv
->
base
.
inputs
[
index
].
timestamp
=
now
;
psmv
->
base
.
inputs
[
index
].
value
.
vec1
.
x
=
psmv
->
last
.
sample
[
1
].
trigger
/
255
.
0
f
;
psmv
->
base
.
inputs
[
index
].
value
.
vec1
.
x
=
psmv
->
last
.
trigger
/
255
.
0
f
;
}
...
...
@@ -653,7 +678,7 @@ psmv_run_thread(void *ptr)
timepoint_ns
now_ns
=
time_state_get_now
(
time
);
psmv_parse_input
(
psmv
,
data
.
buffer
,
&
input
);
int
num
=
psmv_parse_input
(
psmv
,
data
.
buffer
,
&
input
);
float
dt
=
time_ns_to_s
(
now_ns
-
then_ns
);
then_ns
=
now_ns
;
...
...
@@ -665,8 +690,16 @@ psmv_run_thread(void *ptr)
psmv
->
last
=
input
;
// Process the parsed data.
update_fusion
(
psmv
,
&
input
.
sample
[
0
],
dt
/
2
.
0
);
update_fusion
(
psmv
,
&
input
.
sample
[
1
],
dt
/
2
.
0
);
if
(
num
==
2
)
{
// ZCM1
update_fusion
(
psmv
,
&
input
.
samples
[
0
],
dt
/
2
.
0
);
update_fusion
(
psmv
,
&
input
.
samples
[
1
],
dt
/
2
.
0
);
}
else
if
(
num
==
1
)
{
// ZCM2
update_fusion
(
psmv
,
&
input
.
sample
,
dt
);
}
else
{
assert
(
false
);
}
// Now done.
os_mutex_unlock
(
&
psmv
->
lock
);
...
...
@@ -970,10 +1003,10 @@ psmv_found(struct xrt_prober *xp,
u_var_add_vec3_f32
(
psmv
,
&
psmv
->
calibration
.
gyro
.
factor
,
"gyro.factor"
);
u_var_add_vec3_f32
(
psmv
,
&
psmv
->
calibration
.
gyro
.
bias
,
"gyro.bias"
);
u_var_add_gui_header
(
psmv
,
&
psmv
->
gui
.
last_frame
,
"Last data"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
[
0
].
accel
,
"last.sample[0].accel"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
[
1
].
accel
,
"last.sample[1].accel"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
[
0
].
gyro
,
"last.sample[0].gyro"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
[
1
].
gyro
,
"last.sample[1].gyro"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
s
[
0
].
accel
,
"last.sample
s
[0].accel"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
s
[
1
].
accel
,
"last.sample
s
[1].accel"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
s
[
0
].
gyro
,
"last.sample
s
[0].gyro"
);
u_var_add_ro_vec3_i32
(
psmv
,
&
psmv
->
last
.
sample
s
[
1
].
gyro
,
"last.sample
s
[1].gyro"
);
u_var_add_ro_vec3_f32
(
psmv
,
&
psmv
->
read
.
accel
,
"read.accel"
);
u_var_add_ro_vec3_f32
(
psmv
,
&
psmv
->
read
.
gyro
,
"read.gyro"
);
u_var_add_gui_header
(
psmv
,
&
psmv
->
gui
.
control
,
"Control"
);
...
...
@@ -1213,7 +1246,7 @@ psmv_get_calibration_zcm1(struct psmv_device *psmv)
return
0
;
}
static
void
static
int
psmv_parse_input_zcm1
(
struct
psmv_device
*
psmv
,
struct
psmv_input_zcm1
*
data
,
struct
psmv_parsed_input
*
input
)
...
...
@@ -1228,13 +1261,14 @@ psmv_parse_input_zcm1(struct psmv_device *psmv,
input
->
timestamp
|=
data
->
timestamp_low
;
input
->
timestamp
|=
data
->
timestamp_high
<<
8
;
input
->
sample
[
0
].
trigger
=
data
->
trigger_f1
;
psmv_from_vec3_u16_wire
(
&
input
->
sample
[
0
].
accel
,
&
data
->
accel_f1
);
psmv_from_vec3_u16_wire
(
&
input
->
sample
[
0
].
gyro
,
&
data
->
gyro_f1
);
input
->
trigger_values
[
0
]
=
data
->
trigger_f1
;
input
->
trigger_values
[
1
]
=
data
->
trigger_f2
;
psmv_from_vec3_u16_wire
(
&
input
->
samples
[
0
].
accel
,
&
data
->
accel_f1
);
psmv_from_vec3_u16_wire
(
&
input
->
samples
[
0
].
gyro
,
&
data
->
gyro_f1
);
input
->
sample
[
1
].
trigger
=
data
->
trigger_f2
;
psmv_from_vec3_u16_wire
(
&
input
->
sample
[
1
].
accel
,
&
data
->
accel_f2
);
psmv_from_vec3_u16_wire
(
&
input
->
sample
[
1
].
gyro
,
&
data
->
gyro_f2
);
psmv_from_vec3_u16_wire
(
&
input
->
samples
[
1
].
accel
,
&
data
->
accel_f2
);
psmv_from_vec3_u16_wire
(
&
input
->
samples
[
1
].
gyro
,
&
data
->
gyro_f2
);
uint32_t
diff
=
psmv_calc_delta_and_handle_rollover
(
input
->
timestamp
,
psmv
->
last
.
timestamp
);
...
...
@@ -1250,24 +1284,26 @@ psmv_parse_input_zcm1(struct psmv_device *psmv,
"missed: %s
\n\t
"
"buttons: %08x
\n\t
"
"battery: %x
\n\t
"
"sample[0].accel: %6i %6i %6i
\n\t
"
"sample[1].accel: %6i %6i %6i
\n\t
"
"sample[0].gyro: %6i %6i %6i
\n\t
"
"sample[1].gyro: %6i %6i %6i
\n\t
"
"
sample[0].trigger
: %02x
\n\t
"
"
sample[1].trigger
: %02x
\n\t
"
"sample
s
[0].accel: %6i %6i %6i
\n\t
"
"sample
s
[1].accel: %6i %6i %6i
\n\t
"
"sample
s
[0].gyro: %6i %6i %6i
\n\t
"
"sample
s
[1].gyro: %6i %6i %6i
\n\t
"
"
trigger_values[0]
: %02x
\n\t
"
"
trigger_values[1]
: %02x
\n\t
"
"timestamp: %i
\n\t
"
"diff: %i
\n\t
"
"seq_no: %x
\n
"
,
missed
?
"yes"
:
"no"
,
input
->
buttons
,
input
->
battery
,
input
->
sample
[
0
].
accel
.
x
,
input
->
sample
[
0
].
accel
.
y
,
input
->
sample
[
0
].
accel
.
z
,
input
->
sample
[
1
].
accel
.
x
,
input
->
sample
[
1
].
accel
.
y
,
input
->
sample
[
1
].
accel
.
z
,
input
->
sample
[
0
].
gyro
.
x
,
input
->
sample
[
0
].
gyro
.
y
,
input
->
sample
[
0
].
gyro
.
z
,
input
->
sample
[
1
].
gyro
.
x
,
input
->
sample
[
1
].
gyro
.
y
,
input
->
sample
[
1
].
gyro
.
z
,
input
->
sample
[
0
].
trigger
,
input
->
sample
[
1
].
trigger
,
input
->
sample
s
[
0
].
accel
.
x
,
input
->
sample
s
[
0
].
accel
.
y
,
input
->
sample
s
[
0
].
accel
.
z
,
input
->
sample
s
[
1
].
accel
.
x
,
input
->
sample
s
[
1
].
accel
.
y
,
input
->
sample
s
[
1
].
accel
.
z
,
input
->
sample
s
[
0
].
gyro
.
x
,
input
->
sample
s
[
0
].
gyro
.
y
,
input
->
sample
s
[
0
].
gyro
.
z
,
input
->
sample
s
[
1
].
gyro
.
x
,
input
->
sample
s
[
1
].
gyro
.
y
,
input
->
sample
s
[
1
].
gyro
.
z
,
input
->
trigger_values
[
0
],
input
->
trigger_values
[
1
]
,
input
->
timestamp
,
diff
,
input
->
seq_no
);
return
2
;
}
...
...
@@ -1430,7 +1466,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
return
0
;
}
static
void
static
int
psmv_parse_input_zcm2
(
struct
psmv_device
*
psmv
,
struct
psmv_input_zcm2
*
data
,
struct
psmv_parsed_input
*
input
)
...
...
@@ -1445,14 +1481,14 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
input
->
timestamp
|=
data
->
timestamp_low
;
input
->
timestamp
|=
data
->
timestamp_high
<<
8
;
input
->
sample
[
0
].
trigger
=
data
->
trigger_low_pass
;
input
->
sample
[
1
].
trigger
=
data
->
trigger
;
input
->
trigger_low_pass
=
data
->
trigger_low_pass
;
input
->
trigger
=
data
->
trigger
;
psmv_from_vec3_i16_wire
(
&
input
->
sample
[
0
]
.
accel
,
&
data
->
accel
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
[
0
]
.
gyro
,
&
data
->
gyro
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
.
accel
,
&
data
->
accel
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
.
gyro
,
&
data
->
gyro
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
[
1
]
.
accel
,
&
data
->
accel_copy
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
[
1
]
.
gyro
,
&
data
->
gyro_copy
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
_copy
.
accel
,
&
data
->
accel_copy
);
psmv_from_vec3_i16_wire
(
&
input
->
sample
_copy
.
gyro
,
&
data
->
gyro_copy
);
uint32_t
diff
=
psmv_calc_delta_and_handle_rollover
(
input
->
timestamp
,
psmv
->
last
.
timestamp
);
...
...
@@ -1468,24 +1504,26 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
"missed: %s
\n\t
"
"buttons: %08x
\n\t
"
"battery: %x
\n\t
"
"sample[0].accel: %6i %6i %6i
\n\t
"
"sample[1].accel: %6i %6i %6i
\n\t
"
"sample[0].gyro: %6i %6i %6i
\n\t
"
"sample[1].gyro: %6i %6i %6i
\n\t
"
"sample.trigger_low_pass: %02x
\n\t
"
"sample.accel: %6i %6i %6i
\n\t
"
"sample_copy.accel: %6i %6i %6i
\n\t
"
"sample.gyro: %6i %6i %6i
\n\t
"
"sample_copy.gyro: %6i %6i %6i
\n\t
"
"sample.trigger: %02x
\n\t
"
"sample.trigger_low_pass: %02x
\n\t
"
"timestamp: %i
\n\t
"
"diff: %i
\n\t
"
"seq_no: %x
\n
"
,
missed
?
"yes"
:
"no"
,
input
->
buttons
,
input
->
battery
,
input
->
sample
[
0
].
accel
.
x
,
input
->
sample
[
0
].
accel
.
y
,
input
->
sample
[
0
].
accel
.
z
,
input
->
sample
[
1
].
accel
.
x
,
input
->
sample
[
1
].
accel
.
y
,
input
->
sample
[
1
].
accel
.
z
,
input
->
sample
[
0
].
gyro
.
x
,
input
->
sample
[
0
].
gyro
.
y
,
input
->
sample
[
0
].
gyro
.
z
,
input
->
sample
[
1
].
gyro
.
x
,
input
->
sample
[
1
].
gyro
.
y
,
input
->
sample
[
1
].
gyro
.
z
,
input
->
sample
[
0
].
trigger
,
input
->
sample
[
1
].
trigger
,
input
->
timestamp
,
diff
,
input
->
seq_no
);
input
->
samples
[
0
].
accel
.
x
,
input
->
samples
[
0
].
accel
.
y
,
input
->
samples
[
0
].
accel
.
z
,
input
->
samples
[
1
].
accel
.
x
,
input
->
samples
[
1
].
accel
.
y
,
input
->
samples
[
1
].
accel
.
z
,
input
->
samples
[
0
].
gyro
.
x
,
input
->
samples
[
0
].
gyro
.
y
,
input
->
samples
[
0
].
gyro
.
z
,
input
->
samples
[
1
].
gyro
.
x
,
input
->
samples
[
1
].
gyro
.
y
,
input
->
samples
[
1
].
gyro
.
z
,
input
->
trigger_low_pass
,
input
->
trigger
,
input
->
timestamp
,
diff
,
input
->
seq_no
);
return
1
;
}
...
...
@@ -1507,15 +1545,15 @@ psmv_get_calibration(struct psmv_device *psmv)
return
0
;
}
static
void
static
int
psmv_parse_input
(
struct
psmv_device
*
psmv
,
void
*
data
,
struct
psmv_parsed_input
*
input
)
{
switch
(
psmv
->
pid
)
{
case
PSMV_PID_ZCM1
:
psmv_parse_input_zcm1
(
psmv
,
data
,
input
);
break
;
case
PSMV_PID_ZCM2
:
psmv_parse_input_zcm2
(
psmv
,
data
,
input
);
break
;
default:
b
re
ak
;
case
PSMV_PID_ZCM1
:
return
psmv_parse_input_zcm1
(
psmv
,
data
,
input
);
case
PSMV_PID_ZCM2
:
return
psmv_parse_input_zcm2
(
psmv
,
data
,
input
);
default:
re
turn
0
;
}
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment