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;
}
}
