private void BnoUpdater()
{
var dataIndex = _metaDataCount;
var time = BitConverter.GetBytes(Clock.Instance.ElapsedMilliseconds);
_dataArray[dataIndex++] = time[0];
_dataArray[dataIndex++] = time[1];
_dataArray[dataIndex++] = time[2];
var accelVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorAccelerometer);
accelVec.X *= 100;
accelVec.Y *= 100;
accelVec.Z *= 100;
_dataArray[dataIndex++] = (accelVec.X < 0 ? (byte)1 : (byte)0);
accelVec.X = (float)System.Math.Abs(accelVec.X);
_dataArray[dataIndex++] = (byte)(((short)accelVec.X >> 8) & 0xFF);
_dataArray[dataIndex++] = (byte)((short)accelVec.X & 0xFF);
_dataArray[dataIndex++] = (accelVec.Y < 0 ? (byte)1 : (byte)0);
accelVec.Y = (float)System.Math.Abs(accelVec.Y);
_dataArray[dataIndex++] = (byte)(((short)accelVec.Y >> 8) & 0xFF);
_dataArray[dataIndex++] = (byte)((short)accelVec.Y & 0xFF);
_dataArray[dataIndex++] = (accelVec.Z < 0 ? (byte)1 : (byte)0);
accelVec.Z = (float)System.Math.Abs(accelVec.Z);
_dataArray[dataIndex++] = (byte)(((short)accelVec.Z >> 8) & 0xFF);
_dataArray[dataIndex] = (byte)((short)accelVec.Z & 0xFF);
Array.Copy(_dataArray, _workItem.PacketData, _dataArray.Length);
Thread.Sleep(_delay);
}
private void OnTaskExecute()
{
var dataIndex = _metaDataCount;
var time = BitConverter.GetBytes(Clock.Instance.ElapsedMilliseconds);
_dataArray[dataIndex++] = time[0];
_dataArray[dataIndex++] = time[1];
_dataArray[dataIndex++] = time[2];
var accelVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorAccelerometer);
var gravVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorGravity);
var gyroVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorGyroscope);
var linAccVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorLinearaccel);
var eulerVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorEuler);
var magVec = _bnoSensor.read_vector(SerialBno.Bno055VectorType.VectorMagnetometer);
//3 bytes each component, 3 components each vector, 6 vectors
//= 54 bytes + 3 bytes for time + 2 bytes for metadata +
//4 bytes calib = 63 bytes per update, 58 bytes of sensor data (54 + 4)
var test = new ArrayList {
accelVec, gravVec, linAccVec, gyroVec, magVec, eulerVec
};
foreach (Vector vector in test)
{
for (var i = 0; i < 3; i++)
{
var component = vector.InnerArray[i] * _precision;
_dataArray[dataIndex++] = (component < 0 ? (byte)1 : (byte)0);
component = (float)Math.Abs(component);
_dataArray[dataIndex++] = (byte)(((short)component >> 8) & 0xFF);
_dataArray[dataIndex++] = (byte)((short)component & 0xFF);
}
}
var calib = _bnoSensor.GetCalibration();
_dataArray[dataIndex++] = calib[0];
_dataArray[dataIndex++] = calib[1];
_dataArray[dataIndex++] = calib[2];
_dataArray[dataIndex] = calib[3];
Array.Copy(_dataArray, _workItem.PacketData, _dataArray.Length);
Thread.Sleep(_delay);
}