Thank you very much for your answer, I think it’s the right way. I tried to implement my own sensor-class, see code below. But my test program reads only invalid and constant values.
Does anyone know what’s wrong with my sensor implementation?
Thanks,
Andreas
public class AbsoluteIMU_ACGSensor : I2CSensor
{
private const byte ADDRESS = 0x22;
private enum I2CRegisters : byte
{
Command = 0x41,
XGyroDataLSB = 0x53,
XGyroDataMSB = 0x54,
YGyroDataLSB = 0x55,
YGyroDataMSB = 0x56,
ZGyroDataLSB = 0x57,
ZGyroDataMSB = 0x58
};
private static int MLBToInteger(byte lsb, byte msb)
{
return (int)lsb + ((int)msb << 8);
}
public AbsoluteIMU_ACGSensor(SensorPort port)
: base(port, ADDRESS, I2CMode.LowSpeed)
{
base.Initialise();
}
public GyroData ReadGyro()
{
LcdConsole.WriteLine("ReadGyro...");
byte[] result = ReadRegister((byte)I2CRegisters.XGyroDataLSB, 6);
LcdConsole.WriteLine("{0}, {1}, {2}, {3}, {4}, {5}", result[0], result[1], result[2], result[3], result[4], result[5]);
return new GyroData(result);
}
public class GyroData
{
public GyroData(byte[] rawData)
{
X = MLBToInteger(rawData[0], rawData[1]);
Y = MLBToInteger(rawData[2], rawData[3]);
Z = MLBToInteger(rawData[4], rawData[5]);
}
public int X { get; private set; }
public int Y { get; private set; }
public int Z { get; private set; }
};
Testprogram:
class Program
{
static void Main(string[] args)
{
var buts = new ButtonEvents();
LcdConsole.WriteLine("Starting...");
var gyroSensor = new AbsoluteIMU_ACGSensor(SensorPort.In2);
gyroSensor.SendCommand((byte)AbsoluteIMUCommands.ChangeAccelerometerSensitivityTo2G);
bool end = false;
buts.EscapePressed += () =>
{
end = true;
LcdConsole.WriteLine("Stopping...");
};
while (!end)
{
// LcdConsole.Clear();
var result = gyroSensor.ReadGyro();
LcdConsole.WriteLine("GX: {0}", result.X);
LcdConsole.WriteLine("GY: {0}", result.Y);
LcdConsole.WriteLine("GZ: {0}", result.Z);
Thread.Sleep(250);
}
}
The AbsoluteIMU-User-Guide describes the I2C-registers.
Follow