From 2c153ba846d05c2a13bc48154e2bcc5b3709e73b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 21 Apr 2018 19:23:44 -0600 Subject: Additional tuning of gyro scaling parameters --- src/survive_default_devices.c | 18 ++++++++++++------ src/survive_imu.c | 3 ++- 2 files changed, 14 insertions(+), 7 deletions(-) (limited to 'src') 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; } -- cgit v1.2.3