Does anyone here has an optimized driver for I2C? The one I used from Fezzer triggered GC every few seconds. If someone has a more optimized version please let me know…
Optimized for what driver or what sensor?
Sparkfun 9DoF Stick IMU
Maybe post the drivers so we can take a look?
here comes the code!
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
namespace StickIMU_Driver
{
public class StickIMUDriver
{
/// <summary>
/// I2C communication bus
/// </summary>
private I2CDevice i2cBus;
private I2CDevice.Configuration accelerometer;
private I2CDevice.Configuration magnetometer;
private I2CDevice.Configuration gyroscope;
private I2CDevice.I2CWriteTransaction write;
private I2CDevice.I2CTransaction[] writeTransaction;
private byte[] writeData = new byte[2];
private byte[] readData;
private byte[] accelData = new byte[6];
private byte[] gyroData = new byte[6];
private byte[] magnetData = new byte[6];
private IMUData imuData = new IMUData();
public struct IMUData
{
/// <summary>
/// The Raw Accelerometer Data
/// </summary>
public short AccelX, AccelY, AccelZ;
/// <summary>
/// The Raw Gyroscope Data
/// </summary>
public short GyroX, GyroY, GyroZ;
/// <summary>
/// The Raw Magnetometer Data
/// </summary>
public short MagnetX, MagnetY, MagnetZ;
}
/// <summary>
/// Private function used to write data into the sensor's registers
/// </summary>
/// <param name="device">The I2C device to write to</param>
/// <param name="register">The register address to write to</param>
/// <param name="value">The value to write</param>
private void WriteRegister(I2CDevice.Configuration device, byte register, byte value)
{
// Create a new transaction to write a register value
writeData[0] = register;
writeData[1] = value;
write = I2CDevice.CreateWriteTransaction(writeData);
writeTransaction = new I2CDevice.I2CTransaction[] { write };
// Lock the i2c bus so multiple threads don't try to access it at the same time
lock (i2cBus)
{
// Set the bus to use specified sensor
i2cBus.Config = device;
// Execute the transation
i2cBus.Execute(writeTransaction, 10);
}
}
/// <summary>
/// Private function used to read data from the sensor's registers
/// </summary>
/// <param name="device">The I2C device to read from</param>
/// <param name="register">The register to read from</param>
/// <param name="length">The length of data to read</param>
/// <returns>A byte value containing the data read from the sensor's register</returns>
private byte[] ReadRegister(I2CDevice.Configuration device, byte register, int length)
{
// Create a new transaction to read a register value
readData = new byte[length];
I2CDevice.I2CWriteTransaction write = I2CDevice.CreateWriteTransaction(new byte[] { register });
I2CDevice.I2CReadTransaction read = I2CDevice.CreateReadTransaction(readData);
I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[] { write, read };
// Lock the i2c bus so multiple threads don't try to access it at the same time
lock (i2cBus)
{
// Set the bus to use our sensor
i2cBus.Config = device;
// Execute the transation
i2cBus.Execute(readTransaction, 10);
}
// Return the register value
return readData;
}
private byte ReadRegister(I2CDevice.Configuration device, byte register)
{
// Create a new transaction to read a register value
readData = new byte[1];
I2CDevice.I2CWriteTransaction write = I2CDevice.CreateWriteTransaction(new byte[] { register });
I2CDevice.I2CReadTransaction read = I2CDevice.CreateReadTransaction(readData);
I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[] { write, read };
// Lock the i2c bus so multiple threads don't try to access it at the same time
lock (i2cBus)
{
// Set the bus to use our sensor
i2cBus.Config = device;
// Execute the transation
i2cBus.Execute(readTransaction, 10);
}
// Return the register value
return readData[0];
}
/// <summary>
/// Constructor
/// </summary>
public StickIMUDriver()
{
i2cBus = new I2CDevice(new I2CDevice.Configuration(0x00, 400));
accelerometer = new I2CDevice.Configuration(0x53, 400);
magnetometer = new I2CDevice.Configuration(0x1e, 400);
gyroscope = new I2CDevice.Configuration(0x68, 400);
// Initialize the sensors...
accelerometerInit();
gyroscopeInit();
magnetometerInit();
}
public string ToString()
{
return "A: " + imuData.AccelX.ToString() + ", " + imuData.AccelY.ToString() + ", " + imuData.AccelZ.ToString() + ", "
+ "G: " + imuData.GyroX.ToString() + ", " + imuData.GyroY.ToString() + ", " + imuData.GyroZ.ToString() + ", "
+ "M: " + imuData.MagnetX.ToString() + ", " + imuData.MagnetY.ToString() + ", " + imuData.MagnetZ.ToString();
}
/// <summary>
/// Initializing Accelerometer
/// </summary>
private void accelerometerInit()
{
// Set POWER_CTL register to use measurement mode
WriteRegister(accelerometer, 0x2d, 0x08);
// Set DATA_FORMAT register to use full resolution
WriteRegister(accelerometer, 0x31, 0x08);
// Set BW_RATE register to 50Hz (25Hz bandwidth)
WriteRegister(accelerometer, 0x2c, 0x09);
}
/// <summary>
/// Read Accelerometer Values
/// </summary>
public void readAccelerometer()
{
accelData = ReadRegister(accelerometer, 0x32, 6);
imuData.AccelY = (short)(accelData[1] << 8 | accelData[0]); // Y axis (internal sensor X axis)
imuData.AccelX = (short)(accelData[3] << 8 | accelData[2]); // X axis (internal sensor Y axis)
imuData.AccelZ = (short)(accelData[5] << 8 | accelData[4]); // Z axis (internal sensor Z axis)
}
/// <summary>
/// Initialize Gyroscope
/// </summary>
private void gyroscopeInit()
{
// Set Gyroscope to use Full Scale (±2000°/s),
// internal LPF bandwith to 188Hz and Internal Sampling Rate to 1kHz
WriteRegister(gyroscope, 0x16, 0x19);
}
/// <summary>
/// Read Gyroscope Values
/// </summary>
public void readGyroscope()
{
gyroData = ReadRegister(gyroscope, 0x1d, 6);
imuData.GyroY = (short)(gyroData[0] << 8 | gyroData[1]); // Y (Pitch) axis (internal sensor X axis)
imuData.GyroX = (short)(gyroData[2] << 8 | gyroData[3]); // X (Roll) axis (internal sensor Y axis)
imuData.GyroZ = (short)(gyroData[4] << 8 | gyroData[5]); // Z (Yaw) axis (internal sensor Z axis)
}
/// <summary>
/// Initialize Magnetometer
/// </summary>
private void magnetometerInit()
{
// Write to mode register to set at continuous mode
WriteRegister(magnetometer, 0x02, 0x00);
// Write to config register A to set output rate to 50Hz
WriteRegister(magnetometer, 0x00, 0x06);
}
/// <summary>
/// Read Magnetometer Values
/// </summary>
public void readMagnetometer()
{
magnetData = ReadRegister(magnetometer, 0x03, 6);
imuData.MagnetX = (short)(magnetData[0] << 8 | magnetData[1]);
imuData.MagnetY = (short)(magnetData[2] << 8 | magnetData[3]);
imuData.MagnetZ = (short)(magnetData[4] << 8 | magnetData[5]);
}
}
}
To grab data from the IMU I use
public static StickIMUDriver stickIMU;
...
stickIMU.readAccelerometer();
stickIMU.readGyroscope();
stickIMU.readMagnetometer();
I optimized the code so the GC doesn’t need to work anymore (except for the ToString method)
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
namespace StickIMU_Driver
{
public class StickIMUDriver
{
#region Declarations
private I2CDevice i2cBus = new I2CDevice(new I2CDevice.Configuration(0x00, 400));
private I2CDevice.Configuration accelerometer = new I2CDevice.Configuration(0x53, 400);
private I2CDevice.Configuration magnetometer = new I2CDevice.Configuration(0x1e, 400);
private I2CDevice.Configuration gyroscope = new I2CDevice.Configuration(0x68, 400);
private I2CDevice.I2CTransaction[] writeTransactions;
private I2CDevice.I2CTransaction[] readTransactions;
private IMUData imuData = new IMUData();
public struct IMUData
{
/// <summary>
/// The Raw Accelerometer Data
/// </summary>
public short AccelX, AccelY, AccelZ;
/// <summary>
/// The Raw Gyroscope Data
/// </summary>
public short GyroX, GyroY, GyroZ;
/// <summary>
/// The Raw Magnetometer Data
/// </summary>
public short MagnetX, MagnetY, MagnetZ;
}
#endregion
#region Construction
public StickIMUDriver()
{
writeTransactions = new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(new byte[] { 0, 0 }) };
readTransactions = new I2CDevice.I2CTransaction[] { I2CDevice.CreateWriteTransaction(new byte[] { 0 }), I2CDevice.CreateReadTransaction(new byte[] { 0, 0, 0, 0, 0, 0 }) };
// Initialize the sensors...
accelerometerInit();
gyroscopeInit();
magnetometerInit();
}
#endregion
#region Private methods
/// <summary>
/// Private function used to write data into the sensor's registers
/// </summary>
/// <param name="device">The I2C device to write to</param>
/// <param name="register">The register address to write to</param>
/// <param name="value">The value to write</param>
private void WriteRegister(I2CDevice.Configuration device, byte register, byte value)
{
lock (this)
{
writeTransactions[0].Buffer[0] = register;
writeTransactions[0].Buffer[1] = value;
// Set the bus to use specified sensor
i2cBus.Config = device;
// Execute the transation
i2cBus.Execute(writeTransactions, 10);
}
}
/// <summary>
/// Private function used to read data from the sensor's registers
/// </summary>
/// <param name="device">The I2C device to read from</param>
/// <param name="register">The register to read from</param>
/// <param name="length">The length of data to read</param>
/// <returns>A byte value containing the data read from the sensor's register</returns>
private byte[] ReadRegister(I2CDevice.Configuration device, byte register, int length)
{
if (length != 6)
throw new Exception("Code optimised for reading a 6 bytes buffer");
lock (this)
{
readTransactions[0].Buffer[0] = register;
// Set the bus to use our sensor
i2cBus.Config = device;
// Execute the transation
if (i2cBus.Execute(readTransactions, 10) == length)
return readTransactions[1].Buffer;
}
return null;
}
/// <summary>
/// Initializing Accelerometer
/// </summary>
private void accelerometerInit()
{
// Set POWER_CTL register to use measurement mode
WriteRegister(accelerometer, 0x2d, 0x08);
// Set DATA_FORMAT register to use full resolution
WriteRegister(accelerometer, 0x31, 0x08);
// Set BW_RATE register to 50Hz (25Hz bandwidth)
WriteRegister(accelerometer, 0x2c, 0x09);
}
/// <summary>
/// Initialize Gyroscope
/// </summary>
private void gyroscopeInit()
{
// Set Gyroscope to use Full Scale (���±2000���°/s),
// internal LPF bandwith to 188Hz and Internal Sampling Rate to 1kHz
WriteRegister(gyroscope, 0x16, 0x19);
}
/// <summary>
/// Initialize Magnetometer
/// </summary>
private void magnetometerInit()
{
// Write to mode register to set at continuous mode
WriteRegister(magnetometer, 0x02, 0x00);
// Write to config register A to set output rate to 50Hz
WriteRegister(magnetometer, 0x00, 0x06);
}
#endregion
#region Public methods
public string ToString()
{
lock (this)
return "A: " + imuData.AccelX.ToString() + ", " + imuData.AccelY.ToString() + ", " + imuData.AccelZ.ToString() + ", "
+ "G: " + imuData.GyroX.ToString() + ", " + imuData.GyroY.ToString() + ", " + imuData.GyroZ.ToString() + ", "
+ "M: " + imuData.MagnetX.ToString() + ", " + imuData.MagnetY.ToString() + ", " + imuData.MagnetZ.ToString();
}
/// <summary>
/// Read Accelerometer Values
/// </summary>
public bool readAccelerometer()
{
lock (this)
{
byte[] data = ReadRegister(accelerometer, 0x32, 6);
if (data == null)
return false;
imuData.AccelY = (short)(data[1] << 8 | data[0]); // Y axis (internal sensor X axis)
imuData.AccelX = (short)(data[3] << 8 | data[2]); // X axis (internal sensor Y axis)
imuData.AccelZ = (short)(data[5] << 8 | data[4]); // Z axis (internal sensor Z axis)
return true;
}
}
/// <summary>
/// Read Gyroscope Values
/// </summary>
public bool readGyroscope()
{
lock (this)
{
byte[] data = ReadRegister(gyroscope, 0x1d, 6);
if (data != null)
return false;
imuData.GyroY = (short)(data[0] << 8 | data[1]); // Y (Pitch) axis (internal sensor X axis)
imuData.GyroX = (short)(data[2] << 8 | data[3]); // X (Roll) axis (internal sensor Y axis)
imuData.GyroZ = (short)(data[4] << 8 | data[5]); // Z (Yaw) axis (internal sensor Z axis)
return true;
}
}
/// <summary>
/// Read Magnetometer Values
/// </summary>
public bool readMagnetometer()
{
lock (this)
{
byte[] data = ReadRegister(magnetometer, 0x03, 6);
if (data != null)
return false;
imuData.MagnetX = (short)(data[0] << 8 | data[1]);
imuData.MagnetY = (short)(data[2] << 8 | data[3]);
imuData.MagnetZ = (short)(data[4] << 8 | data[5]);
return true;
}
}
#endregion
}
}
Note that I couldn’t test the code because I don’t have the correct hardware. But I did a quick debug and it should work.
Let me know please…
Thanks! I’ll try it when I am out of work.
somethings wrong here:
byte[] data = ReadRegister(accelerometer, 0x32, 6);
if (data == null)
return false;
imuData.AccelY = (short)(data[1] << 8 | data[0]); // Y axis (internal sensor X axis)
imuData.AccelX = (short)(data[3] << 8 | data[2]); // X axis (internal sensor Y axis)
imuData.AccelZ = (short)(data[5] << 8 | data[4]); // Z axis (internal sensor Z axis)
return true;
the variable data is always null therefore nothings get passed to imuData.
Ok. I fixed the problem. Tweak it further more and I dont have GC messages popping out since I run it 30 mins ago. Good one. Thanks!
EDIT: Optimized code is on FEZZERS.
Why have you added the Array.Copy instead of
byte[] data = ReadRegister(accelerometer, 0x32, 6);
I think you just create an extra overhead with Array.Copy.
Also I do not know if you can copy ‘null’ that way. And if it does, your next call to Array.Copy would possibly fail.
Modify the read methods like this:
data = ReadRegister(accelerometer, 0x32, 6);
if (data == null)
return false;
imuData.AccelY = (short)(data[1] << 8 | data[0]); // Y axis (internal sensor X axis)
imuData.AccelX = (short)(data[3] << 8 | data[2]); // X axis (internal sensor Y axis)
imuData.AccelZ = (short)(data[5] << 8 | data[4]); // Z axis (internal sensor Z axis)
return true;
And global declare ‘byte[] data;’ without initialising it.
data is initialized as a 6-byte array at the beginning.
You can’t assign an array to an array. You have to copy it.
Your instance creates an array every time the sensors are read. Therefore there is still GC punching in. Therefore I put it as a global variable.
I still think it’s better to remove the Array.Copy methods, define a global ‘byte[] data;’ (uninitialised) and use following code to copy the reference of the internal transaction buffer to the data array.
data = ReadRegister(accelerometer, 0x32, 6);