diff options
| author | Justin Berger <j.david.berger@gmail.com> | 2018-03-22 12:42:11 -0600 |
|---|---|---|
| committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-22 12:42:11 -0600 |
| commit | 3272ffe5245c6f39f93572d4059f35953dc38faa (patch) | |
| tree | 5dbca7448e303f86bb2c1180d5d44ac4ece03920 /src/survive_imu.c | |
| parent | dcf5d7a482e022e762a656253017ebbc721d8a83 (diff) | |
| download | libsurvive-3272ffe5245c6f39f93572d4059f35953dc38faa.tar.gz libsurvive-3272ffe5245c6f39f93572d4059f35953dc38faa.tar.bz2 | |
Integrated IMU into SBA
Diffstat (limited to 'src/survive_imu.c')
| -rw-r--r-- | src/survive_imu.c | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/src/survive_imu.c b/src/survive_imu.c index 6667d2f..5197266 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -7,7 +7,8 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, tracker->pose = *pose; for (int i = 0; i < 3; i++) - tracker->current_velocity[i] = pose->Pos[i] - tracker->lastGT.Pos[i]; + tracker->current_velocity[i] = + (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.; tracker->lastGTTime = timecode; tracker->lastGT = *pose; |