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
d4e139f0
Commit
d4e139f0
authored
Sep 15, 2019
by
Jakob Bornecrantz
Browse files
d/psmv: Use gyro_bias from calibration data on ZCM2
parent
548f4ede
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/xrt/drivers/psmv/psmv_driver.c
View file @
d4e139f0
...
...
@@ -255,18 +255,16 @@ struct psmv_calibration_zcm2
struct
psmv_vec3_i16_wire
accel_min_y
;
struct
psmv_vec3_i16_wire
accel_max_z
;
struct
psmv_vec3_i16_wire
accel_min_z
;
uint16_t
_pad0
;
uint16_t
_pad1
;
uint16_t
_pad2
;
uint16_t
_pad3
;
uint16_t
_pad4
;
//! Pretty sure this is gryo bias.
struct
psmv_vec3_i16_wire
gyro_bias
;
uint8_t
_pad0
[
4
];
struct
psmv_vec3_i16_wire
gyro_pos_x
;
struct
psmv_vec3_i16_wire
gyro_pos_y
;
struct
psmv_vec3_i16_wire
gyro_pos_z
;
struct
psmv_vec3_i16_wire
gyro_neg_x
;
struct
psmv_vec3_i16_wire
gyro_neg_y
;
struct
psmv_vec3_i16_wire
gyro_neg_z
;
uint
16
_t
_pad
[
6
];
uint
8
_t
_pad
1
[
12
];
};
/*!
...
...
@@ -414,6 +412,9 @@ struct psmv_parsed_calibration_zcm2
struct
xrt_vec3_i32
gyro_pos_y
;
struct
xrt_vec3_i32
gyro_neg_z
;
struct
xrt_vec3_i32
gyro_pos_z
;
//! Pretty sure this is gryo bias.
struct
xrt_vec3_i32
gyro_bias
;
};
/*!
...
...
@@ -1366,6 +1367,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
psmv_from_vec3_i16_wire
(
&
zcm2
->
gyro_pos_y
,
&
data
.
gyro_pos_y
);
psmv_from_vec3_i16_wire
(
&
zcm2
->
gyro_neg_z
,
&
data
.
gyro_neg_z
);
psmv_from_vec3_i16_wire
(
&
zcm2
->
gyro_pos_z
,
&
data
.
gyro_pos_z
);
psmv_from_vec3_i16_wire
(
&
zcm2
->
gyro_bias
,
&
data
.
gyro_bias
);
/*
...
...
@@ -1405,6 +1407,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
psmv
->
calibration
.
gyro
.
factor
.
y
=
(
60
.
0
*
gy
)
/
(
2
.
0
*
M_PI
*
90
.
0
);
psmv
->
calibration
.
gyro
.
factor
.
z
=
(
60
.
0
*
gz
)
/
(
2
.
0
*
M_PI
*
90
.
0
);
#if 0
psmv->calibration.gyro.bias.x =
(zcm2->gyro_neg_y.x + zcm2->gyro_pos_y.x + zcm2->gyro_neg_z.x +
zcm2->gyro_pos_z.x) /
...
...
@@ -1417,6 +1420,11 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
(zcm2->gyro_neg_x.z + zcm2->gyro_pos_x.z + zcm2->gyro_neg_y.z +
zcm2->gyro_pos_y.z) /
4.0;
#else
psmv
->
calibration
.
gyro
.
bias
.
x
=
zcm2
->
gyro_bias
.
x
;
psmv
->
calibration
.
gyro
.
bias
.
y
=
zcm2
->
gyro_bias
.
y
;
psmv
->
calibration
.
gyro
.
bias
.
z
=
zcm2
->
gyro_bias
.
z
;
#endif
/*
...
...
@@ -1439,6 +1447,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
"
\t\t
gyro_pos_y: %6i %6i %6i
\n
"
"
\t\t
gyro_neg_z: %6i %6i %6i
\n
"
"
\t\t
gyro_pos_z: %6i %6i %6i
\n
"
"
\t\t
gyro_bias: %6i %6i %6i
\n
"
"
\t
Calculated:
\n
"
"
\t\t
accel.factor: %f %f %f
\n
"
"
\t\t
accel.bias: %f %f %f
\n
"
...
...
@@ -1456,6 +1465,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
zcm2
->
gyro_pos_y
.
x
,
zcm2
->
gyro_pos_y
.
y
,
zcm2
->
gyro_pos_y
.
z
,
zcm2
->
gyro_neg_z
.
x
,
zcm2
->
gyro_neg_z
.
y
,
zcm2
->
gyro_neg_z
.
z
,
zcm2
->
gyro_pos_z
.
x
,
zcm2
->
gyro_pos_z
.
y
,
zcm2
->
gyro_pos_z
.
z
,
zcm2
->
gyro_bias
.
x
,
zcm2
->
gyro_bias
.
y
,
zcm2
->
gyro_bias
.
z
,
psmv
->
calibration
.
accel
.
factor
.
x
,
psmv
->
calibration
.
accel
.
factor
.
y
,
psmv
->
calibration
.
accel
.
factor
.
z
,
psmv
->
calibration
.
accel
.
bias
.
x
,
psmv
->
calibration
.
accel
.
bias
.
y
,
psmv
->
calibration
.
accel
.
bias
.
z
,
...
...
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