aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-21 19:23:44 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-21 19:25:47 -0600
commit2c153ba846d05c2a13bc48154e2bcc5b3709e73b (patch)
treee2158b53a48066134fb1d976af63698df618f69c
parentbc96509c719da116c5e0ebcdb39e082def15d306 (diff)
downloadlibsurvive-2c153ba846d05c2a13bc48154e2bcc5b3709e73b.tar.gz
libsurvive-2c153ba846d05c2a13bc48154e2bcc5b3709e73b.tar.bz2
Additional tuning of gyro scaling parameters
-rw-r--r--include/libsurvive/survive.h1
-rw-r--r--include/libsurvive/survive_imu.h2
-rw-r--r--src/survive_default_devices.c18
-rw-r--r--src/survive_imu.c3
4 files changed, 17 insertions, 7 deletions
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 9f692e9..fb95c80 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -101,6 +101,7 @@ struct SurviveObject {
survive_timecode last_lighttime; // May be a 24- or 32- bit number depending on what device.
+ FLT imu_freq;
FLT *acc_bias; // size is FLT*3. contains x,y,z
FLT *acc_scale; // size is FLT*3. contains x,y,z
FLT *gyro_bias; // size is FLT*3. contains x,y,z
diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
index 892df38..5453e4d 100644
--- a/include/libsurvive/survive_imu.h
+++ b/include/libsurvive/survive_imu.h
@@ -2,6 +2,7 @@
#define _SURVIVE_IMU_H
#include "poser.h"
+#include "survive.h"
#include "survive_types.h"
#include <stdbool.h>
#include <stdint.h>
@@ -13,6 +14,7 @@ extern "C" {
struct SurviveIMUTracker_p;
typedef struct {
+ SurviveObject *so;
bool is_initialized;
FLT updir[3];
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 7417227..c24bb92 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -24,7 +24,7 @@ survive_create_device(SurviveContext *ctx, const char *driver_name,
device->pulse_synctime_offset = 20000;
device->pulse_synctime_slack = 5000;
device->timecenter_ticks = device->timebase_hz / 240;
-
+ device->imu_freq = 250.f;
device->haptic = fn;
return device;
@@ -178,15 +178,21 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
if (so->acc_bias)
scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right.
if (so->gyro_scale) {
- scale3d(so->gyro_scale, so->gyro_scale, 0.000065665);
+ FLT deg_per_sec = 500;
+ scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.);
+ // scale3d(so->gyro_scale, so->gyro_scale, 0.000065665);
}
+
+ so->imu_freq = 1000.f;
} else if (memcmp(so->codename, "WM", 2) == 0) {
if (so->acc_scale)
scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0);
if (so->acc_bias)
scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Need to verify.
+
+ FLT deg_per_sec = 2000;
if (so->gyro_scale)
- scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8); //??! 1.8 feels right but why?!
+ scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.);
int j;
for (j = 0; j < so->sensor_ct; j++) {
so->sensor_locations[j * 3 + 0] *= 1.0;
@@ -206,10 +212,10 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
scale3d(so->acc_bias, so->acc_bias, 1. / 1000.);
// From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits
- // FLT deg_per_sec = 250;
+ FLT deg_per_sec = 2000;
if (so->gyro_scale)
- scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 3600.);
-
+ scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.);
+ // scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8);
}
char fname[64];
diff --git a/src/survive_imu.c b/src/survive_imu.c
index f866caf..494b246 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -15,7 +15,7 @@ static void mahony_ahrs(SurviveIMUTracker *tracker, LinmathVec3d _gyro, LinmathV
LinmathVec3d accel;
memcpy(accel, _accel, 3 * sizeof(FLT));
- const FLT sample_f = 240;
+ const FLT sample_f = tracker->so->imu_freq;
const FLT prop_gain = .5;
const FLT int_gain = 0;
@@ -119,6 +119,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
tracker->is_initialized = true;
+ tracker->so = so;
return;
}