Commit af0cc827 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz

WIP

parent a4ccd22c
Pipeline #197668 passed with stages
in 1 minute and 33 seconds
......@@ -10,6 +10,8 @@
#include "util/u_misc.h"
#include "util/u_var.h"
#include "os/os_time.h"
#include "math/m_api.h"
#include "math/m_fusion.h"
#include "math/m_filter_fifo.h"
......@@ -18,6 +20,14 @@
#include <stdio.h>
/*
*
* Defines and structs.
*
*/
#define DUR_1_MS_IN_NS (1000 * 1000)
#define DUR_50_MS_IN_NS (50 * DUR_1_MS_IN_NS)
struct m_imu_on_arm
{
......@@ -27,7 +37,8 @@ struct m_imu_on_arm
struct xrt_vec3 offset;
struct
{
struct xrt_vec3 position;
struct xrt_vec3 body_position;
struct xrt_vec3 tracked_position;
struct xrt_vec3 velocity;
uint64_t timestamp_ns;
} last;
......@@ -46,6 +57,89 @@ struct m_imu_on_arm
};
/*
*
* Helper functions.
*
*/
static void
calc_angular_velocity(const struct m_imu_on_arm *f,
uint64_t when_ns,
struct xrt_vec3 *out_angular_velocity)
{
struct xrt_vec3 angular_velocity;
struct xrt_quat inverted_rot;
// Average 100ms around the sample point.
size_t n = m_ff_vec3_f32_filter(f->i3dof.gyro_ff, // Filter
when_ns - DUR_50_MS_IN_NS, // Start
when_ns + DUR_50_MS_IN_NS, // Stop
&angular_velocity);
if (n == 0) {
uint64_t dummy = 0;
// Get the last sample if none has been pushed.
m_ff_vec3_f32_get(f->i3dof.gyro_ff, 0, &angular_velocity,
&dummy);
}
math_quat_invert(&f->i3dof.rot, &inverted_rot);
math_quat_rotate_vec3(&inverted_rot, &angular_velocity,
out_angular_velocity);
}
static void
calc_body_position(const struct m_imu_on_arm *f,
const struct xrt_vec3 *body_position,
struct xrt_vec3 *out_body_position)
{
struct xrt_vec3 lever_arm = {0};
math_quat_rotate_vec3(&f->i3dof.rot, &f->offset, &lever_arm);
out_body_position->x = body_position->x - lever_arm.x;
out_body_position->y = body_position->y - lever_arm.y;
out_body_position->z = body_position->z - lever_arm.z;
}
/*!
* We have gotten a position sample.
*/
static void
do_linear_velocity(struct m_imu_on_arm *f)
{
// Have we gotten two or more?
if (f->num_pos < 2) {
f->vel = 0.0f;
return;
}
struct
{
struct xrt_vec3 pos;
uint64_t timestamp_ns;
} samlpes[2];
for (int i = 0; i < 2; i++) {
m_ff_vec3_f32_get(f->position_ff, i, &samlpes[i].pos,
&samlpes[i].timestamp_ns);
}
struct xrt_vec3 body_position = {0};
calc_body_position(f, &f->last.tracked_position, &body_position);
struct xrt_vec3 diff = m_vec3_sub(body_position, f->last.body_position);
uint64_t dur_ns = samlpes[0].timestamp_ns - samlpes[1].timestamp_ns;
double dur_s = time_ns_to_s(dur_ns);
f->last.body_position = body_position;
f->last.velocity = m_vec3_mul_scalar(diff, 1 / dur_s);
#if 1
f->chart.samples[f->chart.index] = m_vec3_len(f->last.velocity);
f->chart.index = (f->chart.index + 1) % ARRAY_SIZE(f->chart.samples);
#endif
}
/*
*
* IMU on a arm.
......@@ -73,8 +167,8 @@ m_imu_on_arm_add_vars(struct m_imu_on_arm *f, void *root, const char *prefix)
snprintf(tmp, sizeof(tmp), "%s3dof.", prefix);
m_imu_3dof_add_vars(&f->i3dof, root, tmp);
snprintf(tmp, sizeof(tmp), "%slast.position", prefix);
u_var_add_ro_vec3_f32(root, &f->last.position, tmp);
snprintf(tmp, sizeof(tmp), "%slast.tracked_position", prefix);
u_var_add_ro_vec3_f32(root, &f->last.tracked_position, tmp);
snprintf(tmp, sizeof(tmp), "%slast.velocity", prefix);
u_var_add_ro_vec3_f32(root, &f->last.velocity, tmp);
snprintf(tmp, sizeof(tmp), "%saccel", prefix);
......@@ -113,40 +207,16 @@ m_imu_on_arm_position(struct m_imu_on_arm *f,
uint64_t timestamp_ns,
const struct xrt_vec3 *pos)
{
// Push the last position into the queue.
m_ff_vec3_f32_push(f->position_ff, pos, timestamp_ns);
f->last.position = *pos;
// And for book keeping.
f->last.tracked_position = *pos;
// We got a position.
f->num_pos++;
// Have we gotten three or more?
if (f->num_pos < 2) {
f->vel = 0.0f;
return;
}
struct
{
struct xrt_vec3 pos;
uint64_t timestamp_ns;
} samlpes[2];
for (int i = 0; i < 2; i++) {
m_ff_vec3_f32_get(f->position_ff, i, &samlpes[i].pos,
&samlpes[i].timestamp_ns);
}
uint64_t dur_ns = samlpes[0].timestamp_ns - samlpes[1].timestamp_ns;
struct xrt_vec3 diff = m_vec3_sub(samlpes[0].pos, samlpes[1].pos);
double dur_s = time_ns_to_s(dur_ns);
f->last.velocity = m_vec3_mul_scalar(diff, 1 / dur_s);
#if 0
fprintf(stderr, "%f {%f %f %f}\n", dur_s, f->last.velocity.x,
f->last.velocity.y, f->last.velocity.z);
#endif
do_linear_velocity(f);
#if 0
// Have we gotten three or more?
......@@ -214,24 +284,49 @@ m_imu_on_arm_get_relation(struct m_imu_on_arm *f,
uint64_t when_ns,
struct xrt_space_relation *out_relation)
{
uint64_t now_ns = os_monotonic_get_ns();
if (now_ns > when_ns) {
uint64_t diff = now_ns - when_ns;
fprintf(stderr, "\tNOW HISTORY %ums\n",
(uint32_t)(diff / DUR_1_MS_IN_NS));
} else {
/*
uint64_t diff = when_ns - now_ns;
fprintf(stderr, "\tNOW FUTURE %ums\n",
(uint32_t)(diff / DUR_1_MS_IN_NS));
*/
}
if (f->i3dof.last.timestamp_ns > when_ns) {
uint64_t diff = now_ns - when_ns;
fprintf(stderr, "\tIMU HISTORY %ums\n",
(uint32_t)(diff / DUR_1_MS_IN_NS));
} else {
/*
uint64_t diff = when_ns -
f->i3dof.last.timestamp_ns; fprintf(stderr, "\tIMU FUTURE
%ums\n", (uint32_t)(diff / DUR_1_MS_IN_NS));
*/
}
if (f->last.timestamp_ns > when_ns) {
fprintf(stderr, "HISTORY\n");
fprintf(stderr, "\tHISTORY POSITION\n");
}
struct xrt_vec3 body_position = {0};
struct xrt_vec3 angular_velocity = {0};
calc_body_position(f, &f->last.tracked_position, &body_position);
calc_angular_velocity(f, when_ns, &angular_velocity);
out_relation->pose.orientation = f->i3dof.rot;
out_relation->pose.position = f->last.position;
out_relation->pose.position = body_position;
out_relation->linear_velocity = f->last.velocity;
out_relation->angular_velocity = angular_velocity;
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
struct xrt_vec3 lever_arm = {0};
math_quat_rotate_vec3(&f->i3dof.rot, &f->offset, &lever_arm);
out_relation->pose.position.x -= lever_arm.x;
out_relation->pose.position.y -= lever_arm.y;
out_relation->pose.position.z -= lever_arm.z;
out_relation->linear_velocity = f->last.velocity;
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT |
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
}
......@@ -123,11 +123,16 @@ get_flags(const struct xrt_space_relation *r)
}
static void
apply_relation(struct xrt_space_relation *a, const struct xrt_space_relation *b)
apply_relation(const struct xrt_space_relation *a,
const struct xrt_space_relation *b,
struct xrt_space_relation *out_relation)
{
flags nf = {};
flags af = get_flags(a);
flags bf = get_flags(b);
flags nf = {};
struct xrt_pose pose = {};
struct xrt_vec3 linear_velocity = {};
struct xrt_vec3 angular_velocity = {};
......@@ -135,12 +140,13 @@ apply_relation(struct xrt_space_relation *a, const struct xrt_space_relation *b)
* Linear velocity.
*/
struct xrt_vec3 linear_velocity = {};
if (af.has_linear_velocity) {
nf.has_linear_velocity = true;
struct xrt_vec3 tmp = {};
math_quat_rotate_vec3(&b->pose.orientation, // Base rotation
&a->linear_velocity, // In base space
&linear_velocity); // Output
&tmp); // Output
linear_velocity += tmp;
}
if (bf.has_linear_velocity) {
......@@ -155,14 +161,48 @@ apply_relation(struct xrt_space_relation *a, const struct xrt_space_relation *b)
if (af.has_angular_velocity) {
nf.has_angular_velocity = true;
struct xrt_vec3 tmp = {};
math_quat_rotate_vec3(&b->pose.orientation, // Base rotation
&a->angular_velocity, // In base space
&angular_velocity); // Output
&tmp); // Output
angular_velocity += tmp;
}
if (bf.has_angular_velocity) {
nf.has_angular_velocity = true;
nf.has_linear_velocity = true;
angular_velocity += b->angular_velocity;
struct xrt_vec3 rotated_position = {};
struct xrt_vec3 position = {};
struct xrt_quat orientation = {};
struct xrt_vec3 tangetal_velocity = {};
position = a->pose.position; // In the base space
orientation = b->pose.orientation; // Base space
math_quat_rotate_vec3(&orientation, // Rotation
&position, // Vector
&rotated_position); // Result
math_vec3_cross(&b->angular_velocity, // A
&rotated_position, // B
&tangetal_velocity); // Result
#if 0
fprintf(stderr, "angular_velocity %f %f %f\n",
b->angular_velocity.x, b->angular_velocity.y,
b->angular_velocity.z);
fprintf(stderr, "rotated_position %f %f %f\n",
rotated_position.x, rotated_position.y,
rotated_position.z);
fprintf(stderr, "tangetal_velocity %f %f %f\n",
tangetal_velocity.x, tangetal_velocity.y,
tangetal_velocity.z);
fprintf(stderr, "\n");
#endif
linear_velocity += tangetal_velocity;
}
......@@ -170,7 +210,7 @@ apply_relation(struct xrt_space_relation *a, const struct xrt_space_relation *b)
* Apply the pose part.
*/
math_relation_apply_offset(&b->pose, a);
math_pose_transform(&b->pose, &a->pose, &pose);
/*
......@@ -193,9 +233,13 @@ apply_relation(struct xrt_space_relation *a, const struct xrt_space_relation *b)
new_flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
}
a->relation_flags = (enum xrt_space_relation_flags)new_flags;
a->linear_velocity = linear_velocity;
a->angular_velocity = angular_velocity;
struct xrt_space_relation tmp = {};
tmp.relation_flags = (enum xrt_space_relation_flags)new_flags;
tmp.pose = pose;
tmp.linear_velocity = linear_velocity;
tmp.angular_velocity = angular_velocity;
*out_relation = tmp;
}
......@@ -210,6 +254,7 @@ m_space_graph_resolve(const struct xrt_space_graph *xsg,
struct xrt_space_relation *out_relation,
bool dump)
{
dump = false;
if (xsg->num_steps == 0 || has_step_with_no_pose(xsg)) {
U_ZERO(out_relation);
return;
......@@ -217,7 +262,7 @@ m_space_graph_resolve(const struct xrt_space_graph *xsg,
struct xrt_space_relation r = xsg->steps[0];
for (uint32_t i = 1; i < xsg->num_steps; i++) {
apply_relation(&r, &xsg->steps[i]);
apply_relation(&r, &xsg->steps[i], &r);
}
if (dump) {
......
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