private void UpdateCalib()
{
var time = Clock.Instance.ElapsedMilliseconds / 1000;
var test = _bno.GetCalibration();
//_lcd.Clear();
//_lcd.Write("S:" + test[0] + " G:" + test[1] + " A:" + test[2] + " M:" + test[3]);
//_lcd.SetCursorPosition(0,1);
//_lcd.Write("T+ " + time + " s");
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);
}