diff options
| author | Justin Berger <j.david.berger@gmail.com> | 2018-03-18 23:15:54 -0600 |
|---|---|---|
| committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-20 16:09:07 -0600 |
| commit | 8ec5d7facc76e05859444335adbdbfd2bbbad852 (patch) | |
| tree | 9a07a479dadebe56365ad1affe3e55878aeecfb7 /src/survive_sensor_activations.c | |
| parent | 5cd3cca4191737ab4ae566975190c3b28bf11f2e (diff) | |
| download | libsurvive-8ec5d7facc76e05859444335adbdbfd2bbbad852.tar.gz libsurvive-8ec5d7facc76e05859444335adbdbfd2bbbad852.tar.bz2 | |
Added code to cancel out object rotation when calibrating
Diffstat (limited to 'src/survive_sensor_activations.c')
| -rw-r--r-- | src/survive_sensor_activations.c | 11 |
1 files changed, 11 insertions, 0 deletions
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 42de833..dce353c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -6,6 +6,17 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance); } +void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) { + for (int i = 0; i < 3; i++) { + self->accel[i] = .98 * self->accel[i] + .02 * imuData->accel[i]; + } + for (int i = 0; i < 3; i++) { + self->gyro[i] = .98 * self->gyro[i] + .02 * imuData->gyro[i]; + } + for (int i = 0; i < 3; i++) { + self->mag[i] = .98 * self->mag[i] + .02 * imuData->mag[i]; + } +} void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) { int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; |