private byte[] read_bytes(Bno055Registers reg, int expectedLength, bool ack = true)
{
//build read command
var command = new byte[4];
command[0] = 0xAA; //start byte
command[1] = 0x01; //0x00: Write, 0x01: Read
command[2] = (byte)reg; // register address
command[3] = (byte)expectedLength; //Length
//send read command and get ack
var response = serial_send(command, expectedLength, ack);
if (response[0] != 0xBB)
{
throw new IOException("Serial Read error: 0x" + Bytearraytostring(response));
}
var length = response[1];
var data = new byte[length];
for (int i = 2; i < response.Length; i++)
{
data[i - 2] = response[i];
}
//Debug.Print("Serial Receive: 0x" + bytearraytostring(data));
if (data.Length != length)
{
throw new IOException("Timeout reading serial data...");
}
return(data);
}
private void write_bytes(Bno055Registers reg, byte[] data, bool ack = true)
{
var command = new byte[4 + data.Length];
command[0] = 0xAA; //start byte
command[1] = 0x00; //0x00: Write, 0x01: Read
command[2] = (byte)reg; // register address
command[3] = (byte)data.Length; //Length
for (var i = 0; i < data.Length; i++)
{
command[i + 4] = data[i];
}
var response = serial_send(command, 2, ack);
if (ack)
{
Verify(response);
}
}
public Bno055(I2C i2c, bool addressPin, int rateKhz = 100) : base()
{
if (addressPin)
{
dev = new SMBusDevice(0x29, i2c, rateKhz);
}
else
{
dev = new SMBusDevice(0x28, i2c, rateKhz);
}
registers = new Bno055Registers(new SMBusRegisterManagerAdapter(dev));
registers.operatingMode.setOperatingMode(OperatingModes.ConfigMode);
Task.Run(registers.operatingMode.write);
registers.sysTrigger.resetSys = 1;
Task.Run(registers.sysTrigger.write);
registers.sysTrigger.resetSys = 0;
int id = 0;
do
{
Task.Run(registers.chipId.read).Wait();
id = registers.chipId.value;
}while (id != 0xA0);
Task.Delay(50);
registers.powerMode.setPowerMode(PowerModes.Normal);
Task.Run(registers.powerMode.write);
Task.Delay(10);
registers.sysTrigger.selfTest = 0;
Task.Run(registers.sysTrigger.write);
Task.Delay(10);
registers.operatingMode.setOperatingMode(OperatingModes.NineDegreesOfFreedom);
Task.Run(registers.operatingMode.write);
Task.Delay(20);
}
public byte read_signed_byte(Bno055Registers reg)
{
var data = read_byte(reg);
return((data > 127) ? (byte)(data - 256) : data);
}
private byte read_byte(Bno055Registers reg)
{
return(read_bytes(reg, 1)[0]);
}
private void write_byte(Bno055Registers reg, byte data, bool ack = true)
{
write_bytes(reg, new [] { data }, ack);
}
private byte[] read_bytes(Bno055Registers reg, int expectedLength, bool ack = true)
{
//build read command
var command = new byte[4];
command[0] = 0xAA; //start byte
command[1] = 0x01; //0x00: Write, 0x01: Read
command[2] = (byte)reg; // register address
command[3] = (byte)expectedLength; //Length
//send read command and get ack
var response = serial_send(command, expectedLength, ack);
if (response[0] != 0xBB) throw new IOException("Serial Read error: 0x" + bytearraytostring(response));
var length = response[1];
var data = new byte[length];
for (int i = 2; i < response.Length; i++)
data[i - 2] = response[i];
//Debug.Print("Serial Receive: 0x" + bytearraytostring(data));
if (data.Length != length) throw new IOException("Timeout reading serial data...");
return data;
}
public byte read_signed_byte(Bno055Registers reg) {
var data = read_byte(reg);
return (data > 127) ? (byte) (data - 256) : data;
}
private byte read_byte(Bno055Registers reg) {
return read_bytes(reg, 1)[0];
}
private void write_bytes(Bno055Registers reg, byte[] data, bool ack = true) {
var command = new byte[4 + data.Length];
command[0] = 0xAA; //start byte
command[1] = 0x00; //0x00: Write, 0x01: Read
command[2] = (byte) reg; // register address
command[3] = (byte) data.Length; //Length
for (var i = 0; i < data.Length; i++) {
command[i + 4] = data[i];
}
var response = serial_send(command, 2, ack);
if (ack) verify(response);
}
private void write_byte(Bno055Registers reg, byte data, bool ack = true) {
write_bytes(reg,new []{data},ack);
}