mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-16 11:55:39 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

xrt: Add units to xrt_imu_sample fields

Browse source 
After using the structure a bit more, and realizing that while different components
sometimes use different units, it is easier to force them to send the imu_samples
in a standardized unit.
This commit is contained in:
Mateo de Mayo 2021-10-06 15:57:49 -03:00
parent 5845155229
commit 67a5946778
4 changed files with 30 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
View file

@ -174,11 +174,16 @@ extern "C" void
t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
{
auto &t = *container_of(sink, TrackerSlam, imu_sink);
timepoint_ns ts = s->timestamp_ns;
xrt_vec3_f64 a = s->accel_m_s2;
xrt_vec3_f64 w = s->gyro_rad_secs;
//! @todo There are many conversions like these between xrt and
//! slam_tracker.hpp types. Implement a casting mechanism to avoid copies.
imu_sample sample{s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz};
imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z};
t.slam->push_imu_sample(sample);
SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz);
SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", ts, a.x, a.y, a.z, w.x, w.y, w.z);
}
/*!

23
src/xrt/drivers/euroc/euroc_player.cpp
View file

@ -159,7 +159,7 @@ euroc_player_preload_imu_data(struct euroc_player *ep,
set_base_ts = false;
}
xrt_imu_sample sample{timestamp, v[3], v[4], v[5], v[0], v[1], v[2]};
xrt_imu_sample sample{timestamp, {v[3], v[4], v[5]}, {v[0], v[1], v[2]}};
samples->push_back(sample);
}
return true;
@ -220,7 +220,7 @@ euroc_player_user_skip(struct euroc_player *ep)
{
timepoint_ns skip_first_ns = ep->playback.skip_first_s * 1000 * 1000 * 1000;
while (ep->imus->at(ep->imu_seq).timestamp < skip_first_ns) {
while (ep->imus->at(ep->imu_seq).timestamp_ns < skip_first_ns) {
ep->imu_seq++;
}
@ -343,7 +343,7 @@ euroc_player_is_imu_next(struct euroc_player *ep)
}
bool more_imgs = ep->img_seq < ep->left_imgs->size();
timepoint_ns imu_ts = more_imus ? ep->imus->at(ep->imu_seq).timestamp : INT64_MAX;
timepoint_ns imu_ts = more_imus ? ep->imus->at(ep->imu_seq).timestamp_ns : INT64_MAX;
timepoint_ns img_ts = more_imgs ? ep->left_imgs->at(ep->img_seq).first : INT64_MAX;
return imu_ts < img_ts;
}
@ -356,7 +356,7 @@ euroc_player_push_next_sample(struct euroc_player *ep)
// Push next IMU sample
if (euroc_player_is_imu_next(ep)) {
xrt_imu_sample sample = ep->imus->at(ep->imu_seq++);
sample.timestamp = euroc_player_mapped_ts(ep, sample.timestamp);
sample.timestamp_ns = euroc_player_mapped_ts(ep, sample.timestamp_ns);
xrt_sink_push_imu(ep->in_sinks.imu, &sample);
return;
}
@ -502,15 +502,18 @@ receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
{
struct euroc_player *ep = container_of(sink, struct euroc_player, imu_sink);
timepoint_ns ts = s->timestamp_ns;
xrt_vec3_f64 a = s->accel_m_s2;
xrt_vec3_f64 w = s->gyro_rad_secs;
// UI log
const xrt_vec3 gyro{(float)s->wx, (float)s->wy, (float)s->wz};
const xrt_vec3 accel{(float)s->ax, (float)s->ay, (float)s->az};
m_ff_vec3_f32_push(ep->gyro_ff, &gyro, s->timestamp);
m_ff_vec3_f32_push(ep->accel_ff, &accel, s->timestamp);
const xrt_vec3 gyro{(float)w.x, (float)w.y, (float)w.z};
const xrt_vec3 accel{(float)a.x, (float)a.y, (float)a.z};
m_ff_vec3_f32_push(ep->gyro_ff, &gyro, ts);
m_ff_vec3_f32_push(ep->accel_ff, &accel, ts);
// Trace log
EUROC_TRACE(ep, "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", s->timestamp, s->ax, s->ay, s->az, s->wx,
s->wy, s->wz);
EUROC_TRACE(ep, "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", ts, a.x, a.y, a.z, w.x, w.y, w.z);
if (ep->out_sinks.imu) {
xrt_sink_push_imu(ep->out_sinks.imu, s);
}

9
src/xrt/drivers/realsense/rs_hdev.c
View file

@ -547,7 +547,7 @@ partial_imu_sample_push(struct rs_hdev *rs, timepoint_ns ts, struct xrt_vec3 val
if (should_submit) {
struct xrt_vec3 gyro = rs->partial_imu_sample.gyro;
struct xrt_vec3 accel = rs->partial_imu_sample.accel;
struct xrt_imu_sample sample = {ts, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z};
struct xrt_imu_sample sample = {ts, {accel.x, accel.y, accel.z}, {gyro.x, gyro.y, gyro.z}};
RS_TRACE("imu t=%ld a=%f,%f,%f w=%f,%f,%f", ts, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
xrt_sink_push_imu(rs->in_sinks.imu, &sample);
@ -862,7 +862,12 @@ static void
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
{
struct rs_hdev *rs = container_of(sink, struct rs_hdev, imu_sink);
RS_TRACE("imu t=%ld a=(%f %f %f) w=(%f %f %f)", s->timestamp, s->ax, s->ay, s->az, s->wx, s->wy, s->wz);
timepoint_ns ts = s->timestamp_ns;
struct xrt_vec3_f64 a = s->accel_m_s2;
struct xrt_vec3_f64 w = s->gyro_rad_secs;
RS_TRACE("imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
if (rs->out_sinks.imu) {
xrt_sink_push_imu(rs->out_sinks.imu, s);
}

5
src/xrt/include/xrt/xrt_tracking.h
View file

@ -137,8 +137,9 @@ struct xrt_tracking_sample
*/
struct xrt_imu_sample
{
timepoint_ns timestamp;
double ax, ay, az, wx, wy, wz;
timepoint_ns timestamp_ns;
struct xrt_vec3_f64 accel_m_s2;
struct xrt_vec3_f64 gyro_rad_secs;
};
/*!