diff options
Diffstat (limited to 'src/survive_sensor_activations.c')
| -rw-r--r-- | src/survive_sensor_activations.c | 16 |
1 files changed, 13 insertions, 3 deletions
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index d14bbec..4d1801c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -3,10 +3,20 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, uint32_t timecode_now, uint32_t idx, int lh) { const uint32_t *data_timecode = self->timecode[idx][lh]; - return !(timecode_now - data_timecode[0] > tolerance || - timecode_now - data_timecode[1] > tolerance); + 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]; @@ -16,4 +26,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *data_timecode = lightData->timecode; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t) (48000000/*mhz*/ * (16.7 * 2/*ms*/) / 1000);
\ No newline at end of file +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);
\ No newline at end of file |