public void ExtractStringTest() { byte[] byteIMUData = new byte[16] { 0x55, 1, 42, 0x0C, 0b_00000000, 0b_11000101, 0b_00000011, 0b_01101100, 0b_00000011, 0b_11100111, 0b_00100000, 0b_10100001, 0b_10011110, 0b00011100, 0b_00000100, 0b_00000110 }; double accScaleFactor = 8192; double gyroScaleFactor = 65.5; byte[] offset = new byte[15]; IMUDataEntry entry = ExtractIMUDataString(byteIMUData, accScaleFactor, gyroScaleFactor, offset); // Tests the right calculation for the Acceleration in G Assert.Equal((10 / 9.80665), (double)entry.Acc.G_X, 2); Assert.Equal(-30 / 9.80665, (double)entry.Acc.G_Y, 2); Assert.Equal(1.234 / 9.80665, (double)entry.Acc.G_Z, 2); // Tests the right calculation for the Acceleration in M/s Assert.Equal(10, (double)entry.Acc.MperS_X, 2); Assert.Equal(-30, (double)entry.Acc.MperS_Y, 2); Assert.Equal(1.234, (double)entry.Acc.MperS_Z, 2); // Tests the right calculation for the Gyroscope in Deg/s Assert.Equal(3.007, (double)entry.Gyro.DegsPerSec_X, 2); Assert.Equal(13.37, (double)entry.Gyro.DegsPerSec_Y, 2); Assert.Equal(15.25, (double)entry.Gyro.DegsPerSec_Z, 2); }
///<inheritdoc/> protected override void Analyse(DataEventArgs data) { IMUDataEntry _newValue = data.Data; //the accelerometer is the only relevant thing Accelerometer accV = _newValue.Acc; double accVAbs = Math.Sqrt(Math.Pow(accV.G_X, 2) + Math.Pow(accV.G_Y, 2) + Math.Pow(accV.G_Z, 2)); //first update average values //therefore calculate the weight of the old value int ref_weight = REF_WEIGHT_REL * _frequency; _avgAccAbsolute = (accVAbs + ref_weight * _avgAccAbsolute) / (ref_weight + 1); _avgAccX = (accV.G_X + ref_weight * _avgAccX) / (ref_weight + 1); _avgAccY = (accV.G_Y + ref_weight * _avgAccY) / (ref_weight + 1); _avgAccZ = (accV.G_Z + ref_weight * _avgAccZ) / (ref_weight + 1); if (accVAbs > TRIGGER_THRESHOLD * _avgAccAbsolute) { //threshold is passed if (cooldown <= 0) { //set cooldown, no matter what motion has been registered cooldown = COOLDOWN_DURATION; //check if direction of current value and average head in the same direction using simple formula for cosinus if ((_avgAccX * accV.G_X + _avgAccY * accV.G_Y + _avgAccZ * accV.G_Z) / accVAbs / _avgAccAbsolute > ANGLE_TOLERANCE_COS) { //step recognized ActivityDone.Invoke(this, new StepEventArgs()); } } } if (cooldown > 0) { cooldown -= 1.0 / _frequency; } }