Commit 82bb022a authored by Pete Black's avatar Pete Black

make ps move controller rotation work, more or less, using complementary filter

parent f4077586
Pipeline #47944 failed with stages
in 1 minute and 4 seconds
......@@ -18,11 +18,18 @@
void
math_euler_to_quat(struct xrt_vec3 euler, struct xrt_quat* q)
{
//STUB
q->x=0.0f;
q->y=0.0f;
q->z=0.0f;
q->w=1.0f;
double cy = cos(euler.z* 0.5);
double sy = sin(euler.z * 0.5);
double cp = cos(euler.x * 0.5);
double sp = sin(euler.x * 0.5);
double cr = cos(euler.y * 0.5);
double sr = sin(euler.y * 0.5);
q->w = cy * cp * cr + sy * sp * sr;
q->x = cy * cp * sr - sy * sp * cr;
q->y = sy * cp * sr + cy * sp * cr;
q->z = sy * cp * cr - cy * sp * sr;
}
......
......@@ -9,8 +9,8 @@ struct filter_complementary_instance_t
filter_complementary_configuration_t configuration;
filter_state_t last_state;
filter_state_t state;
float gyro_yaw_correction;
uint8_t avg_count;
float gyro_x_bias,gyro_y_bias,gyro_z_bias;
uint16_t avg_count;
float alpha;
bool running;
};
......@@ -51,8 +51,7 @@ filter_complementary_get_state(filter_instance_t* inst, filter_state_t* state)
return false;
}
tracker_measurement_t* measurement_array;
uint64_t last_timestamp = internal->last_state.timestamp;
uint32_t count = measurement_queue_get_since_timestamp(inst->measurement_queue,0,last_timestamp,&measurement_array);
uint32_t count = measurement_queue_get_since_timestamp(inst->measurement_queue,0,internal->last_state.timestamp,&measurement_array);
float one_minus_bias = 1.0f - internal->configuration.bias;
for (uint32_t i=0;i<count;i++)
{
......@@ -63,29 +62,40 @@ filter_complementary_get_state(filter_instance_t* inst, filter_state_t* state)
return false; //this is our first frame, or something has gone wrong... big dt will blow up calculations.
}
float magAccel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z+m->accel.z);
//convert our
//assume that, if acceleration is only gravity, then any change in the gyro is drift and update compensation
int avg_max = 32;
if (internal->avg_count < avg_max) {
internal->avg_count++;
}
float mag_accel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z * m->accel.z) * internal->configuration.accel_to_radian;
printf("%f %f %f %f %f %f %f\n",m->accel.x,m->accel.y,m->accel.z,m->gyro.x,m->gyro.y,m->gyro.z,dt);
float roll = atan2(-1.0f * m->accel.x,sqrt(m->accel.y * m->accel.y + m->accel.z * m->accel.z));
float pitch = atan2(m->accel.y,m->accel.z);
float accelDiff =1.0f;
if (internal->gyro_yaw_correction > 0.0f)
{
accelDiff = (magAccel - internal->gyro_yaw_correction) / internal->gyro_yaw_correction;
}
internal->gyro_yaw_correction = magAccel + (magAccel - internal->gyro_yaw_correction) / math_min(internal->avg_count,avg_max);
//printf("accelDiff %f magAccel: %f gyro.z %f yaw corr %f\n",accelDiff,magAccel,m->gyro.z, internal->gyro_yaw_correction);
//calculate filtered euler angles
internal->state.rotation_euler.z = internal->configuration.bias * (internal->last_state.rotation_euler.z + m->gyro.z * dt) + one_minus_bias * (m->accel.z * internal->configuration.scale/magAccel);
internal->state.rotation_euler.y = internal->configuration.bias * (internal->last_state.rotation_euler.y + m->gyro.y * dt) + one_minus_bias * (m->accel.y * internal->configuration.scale/magAccel);
internal->state.rotation_euler.x = internal->configuration.bias * (internal->last_state.rotation_euler.x + m->gyro.x * dt) + one_minus_bias * (m->accel.x * internal->configuration.scale/magAccel);
//assume that, if acceleration is only gravity, then any change in the gyro is drift and update compensation
//if acceleration magnitude is close to 1.0f, we assume its just gravity, and can integrate our gyro reading
//as drift compensation - we can assume controller is static at startup, and gather data then, or continuously
//sample
int avg_max =16;
if (fabs(1.0f - mag_accel) < 0.05 ) { //looks like gravity only
//fill up the running average count as fast as possible, but subsequently
//only integrate measurements that are not outliers w/respect to the average
if (internal->avg_count < avg_max || fabs(m->gyro.z - internal->gyro_z_bias) < (internal->gyro_z_bias / 4)) {
if (internal->avg_count < avg_max) {
internal->avg_count++;
}
internal->gyro_x_bias = internal->gyro_x_bias + (m->gyro.x - internal->gyro_x_bias) / math_min(internal->avg_count, avg_max);
internal->gyro_y_bias = internal->gyro_y_bias + (m->gyro.y - internal->gyro_y_bias) / math_min(internal->avg_count, avg_max);
internal->gyro_z_bias = internal->gyro_z_bias + (m->gyro.z - internal->gyro_z_bias) / math_min(internal->avg_count, avg_max);
//printf("yaw correction: %f %f %f\n",internal->gyro_x_bias,internal->gyro_y_bias,internal->gyro_z_bias);
}
}
//printf("roll %f pitch %f gbc,%f,%f,%f,axyz,%f,%f,%f,gxyz,%f,%f,%f,dt,%f\n",roll,pitch,internal->gyro_x_bias,internal->gyro_y_bias,internal->gyro_z_bias,m->accel.x,m->accel.y,m->accel.z,m->gyro.x,m->gyro.y,m->gyro.z,dt);;
internal->state.rotation_euler.z = internal->last_state.rotation_euler.z + (m->gyro.z - internal->gyro_z_bias) * internal->configuration.gyro_to_radian * dt;
//push back towards zero, as 'returning to 0 slowly' is better than 'just drift', probably
internal->state.rotation_euler.z -=internal->state.rotation_euler.z * internal->configuration.drift_z_to_zero; //0.001;
internal->state.rotation_euler.y = internal->last_state.rotation_euler.y + (0.99 * ((m->gyro.y - internal->gyro_y_bias) * internal->configuration.gyro_to_radian * dt)) - 0.01 * (internal->last_state.rotation_euler.y - roll);
internal->state.rotation_euler.x = internal->last_state.rotation_euler.x + (0.99 * ((m->gyro.x - internal->gyro_x_bias) * internal->configuration.gyro_to_radian * dt)) - 0.01 * (internal->last_state.rotation_euler.x - pitch);
internal->state.timestamp = m->source_timestamp;
internal->last_state = internal->state;
//printf("source tstamp: %lld\n",m->source_timestamp);
......@@ -93,8 +103,9 @@ filter_complementary_get_state(filter_instance_t* inst, filter_state_t* state)
//TODO: come up with a way to avoid alloc/free - use max length buffer?
free(measurement_array);
//convert to a quat for consumption as pose
//printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,last_timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z);
math_euler_to_quat(internal->state.rotation_euler,&internal->state.pose.orientation);
//printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,internal->last_state.timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z);
struct xrt_quat integrate_quat;
math_euler_to_quat(internal->state.rotation_euler,&internal->state.pose.orientation);
*state = internal->state;
return true;
}
......@@ -125,8 +136,10 @@ filter_complementary_configure(filter_instance_t* inst,
filter_complementary_configuration_t* config =
(filter_complementary_configuration_t*)config_generic;
internal->configuration = *config;
internal->gyro_yaw_correction=0.0f;
internal->configured = true;
internal->gyro_x_bias=0.0f;
internal->gyro_y_bias=0.0f;
internal->gyro_z_bias=0.0f;
internal->configured = true;
return true;
}
......
......@@ -11,6 +11,9 @@ typedef struct filter_complementary_configuration
float bias;
float scale;
uint64_t max_timestamp; //wrap timestamp here
float accel_to_radian;
float gyro_to_radian;
float drift_z_to_zero;
} filter_complementary_configuration_t;
#ifdef __cplusplus
......
......@@ -13,6 +13,7 @@
#define MAX_CALIBRATION_SAMPLES \
23 // mo' samples, mo' calibration accuracy, at the expense of time.
static cv::Rect2f calibration_distrib_rectilinear[9] = {};
static bool tracker3D_calibration_stereo_calibrate(tracker_instance_t* inst);
......
......@@ -8,6 +8,7 @@
#define INTRINSICS_SIZE 9
#define DISTORTION_SIZE 5
#define DISTORTION_FISHEYE_SIZE 4
\
#ifdef __cplusplus
extern "C" {
......
......@@ -208,6 +208,7 @@ struct psmv_device
uint64_t last_frame_seq;
pthread_t* tracking_thread;
int64_t start_us;
float accel_scale;
bool print_spew;
......@@ -287,9 +288,9 @@ psmv_read_hid(struct psmv_device *psmv)
if (psmv->filter) {
tracker_measurement_t m = {0};
m.flags = (tracker_measurement_flags_t)(MEASUREMENT_IMU | MEASUREMENT_RAW_ACCEL | MEASUREMENT_RAW_GYRO);
m.accel.x = input.frame[1].accel.x;
m.accel.y = input.frame[1].accel.y;
m.accel.z = input.frame[1].accel.z;
m.accel.x = input.frame[1].accel.x * psmv->accel_scale;
m.accel.y = input.frame[1].accel.y * psmv->accel_scale;
m.accel.z = input.frame[1].accel.z * psmv->accel_scale;
m.gyro.x = input.frame[1].gyro.x;
m.gyro.y = input.frame[1].gyro.y;
m.gyro.z = input.frame[1].gyro.z;
......@@ -451,7 +452,13 @@ psmv_device_get_tracked_pose(struct xrt_device *xdev,
psmv_read_hid(psmv);
filter_state_t fs;
psmv->filter->filter_get_state(psmv->filter,&fs);
out_relation->pose = fs.pose;
struct xrt_quat temp = fs.pose.orientation;
fs.pose.orientation.x=temp.y;
fs.pose.orientation.y=temp.z;
fs.pose.orientation.z=-temp.x;
out_relation->pose = fs.pose;
}
static void
......@@ -547,6 +554,8 @@ psmv_found(struct xrt_prober *xp,
case 2: psmv->state.blue = 0xff; break;
}
psmv->accel_scale = 1.0f; //-8<->+8g (1G = ~4200 raw reading)
// Send the first update package
psmv_led_and_trigger_update(psmv, 1);
......@@ -561,8 +570,9 @@ psmv_found(struct xrt_prober *xp,
psmv->filter = filter_create(FILTER_TYPE_COMPLEMENTARY);
filter_complementary_configuration_t config;
config.bias = 0.1f;
config.scale = 3.14159 / 2.0f;
config.accel_to_radian = 0.000244f;
config.gyro_to_radian = 0.0001f;
config.drift_z_to_zero = 0.001f;
config.max_timestamp=65535; //ps move timestamps are 16 bit
psmv->filter->filter_configure(psmv->filter,&config);
......
......@@ -77,7 +77,7 @@ oxr_xdev_get_pose_at(struct oxr_logger *log,
// clang-format off
bool valid_pos = (relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
bool valid_ori = (relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0;
bool valid_ori = true; //(relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0;
// clang-format on
if (valid_ori) {
......
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