public SerialBNOUpdater(int sigFigs = 4)
{
_bnoSensor = new SerialBNO(SerialPorts.COM3, 5000, 5000, SerialBNO.Bno055OpMode.Operation_Mode_Ndof);
_bnoData = new BNOData();
_newData = new byte[_dataSize + _metaDataCount];
_newData[0] = (byte)PacketType.StartByte; // start bit = 0xff
_newData[1] = (byte)PacketType.BNODump;
_newData[2] = (byte)((_dataSize >> 8) & 0xFF);
_newData[3] = (byte)(_dataSize & 0xFF);
_precision = (int)Math.Pow(10, sigFigs - 1);
//_precision = 1;
//for (int i = 0; i < sigFigs-1; i++)
//{
// _precision *= 10;
//}
_workItem = new WorkItem(GyroUpdater, ref _newData, EventType.BNOUpdate, _bnoData, true, true);
_bnoSensor.begin();
}
protected override void handleBNOData(BNOData bNOData)
{
rb.AddTorque(bNOData.quat.eulerAngles * torqueStrength * rb.mass, ForceMode.Force);
rb.AddForce(rb.velocity * velScale);
}
protected virtual void handleBNOData(BNOData bNOData)
{
rb.MoveRotation(isolateAxisRotation(bNOData.quat, Vector3.up)); // Vector3.right, .2f));
}
protected virtual void handleSerial(string incoming)
{
handleBNOData(BNOData.FromString(incoming));
}
