diff options
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; |