/*************************************************************************** This is a library for the BNO055 orientation sensor. Designed specifically to work with the Adafruit BNO055 Breakout. These sensors use I2C to communicate, 2 pins are required to interface. Written by Nick Raymond & Gene Massion for MBARI Coastal Profiling Float Project. Wiring Hardware I2C: * G400 - Socket 2 - PA30 - Pin 8 - SDA - >>>> Green Wire - SDA - BNO055 * G400 - Socket 2 - PA31 - Pin 9 - SCL - >>>> Blue Wire - SCL - BNO055 Wiring Software I2C: * G400 - Socket 1 - PB5 - Pin 6 - SDA - >>>> Green Wire - SDA - BNO055 * G400 - Socket 1 - PB0 - Pin 3 - SCL - >>>> Blue Wire - SCL - BNO055 ***************************************************************************/ using System; using System.Text; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using GHI.Pins; using GHI.Processor; using GHI.IO; using System.Threading; using System.IO; namespace IMULogger { public class BNO055 { // ----------------------------------------------------------------------------------------------------- // SETUP I2C communication with BNO055 IMU device // ----------------------------------------------------------------------------------------------------- // Setup software I2C communication private static SoftwareI2CBus swIMU_I2C = new SoftwareI2CBus( /*SCL*/ G400.PB0, /*SDA*/ G400.PB5); // Setup hardware I2C communication private static I2CDevice.Configuration IMUDevice = new I2CDevice.Configuration(0x28, 400); private static I2CDevice hwIMU_I2C = new I2CDevice(IMUDevice); #region Constant Definitions // ----------------------------------------------------------------------------------------------------- // DEFINE operation mode selection --- syntax from BNO055 data sheet - pg. 21 // ----------------------------------------------------------------------------------------------------- // Non-Fusion Mode, see Table 3-5 of BNO055 data sheet private const byte CONFIGMODE = 0x00; // all data is reset to zero and sensor fusion is halted. private const byte ACCONLY = 0x01; // accelerometer operation, gyroscope and magnetometer in low power mode. private const byte MAGONLY = 0x02; // magnetometer operation, accelerometer and gyroscope in low power mode. private const byte GRYOONLY = 0x03; // gyroscope operation, accelerometer and magnetometer in low power mode. private const byte ACCMAG = 0x04; // both accelerometer and magnetometer are switched on. private const byte ACCGRYO = 0x05; // both accelerometer and gyroscope are switched on. private const byte MAGGYRO = 0x06; // both magnetometer and gyroscope are switched on. private const byte AMG = 0x07; // all three sensors accelerometer, magnetometer and gyroscope are switched on. // Fusion Mode, see page Table 3-5 of BNO055 data sheet public const byte IMU = 0x08; // relative orientation of sensor calcualted from accelerometer and gyroscope, high output data rate public const byte COMPASS = 0x09; // intended to measure magnetic earthfield and calcualte geographic direction in XY planar frame public const byte M4G = 0x0A; // similar to IMU mode but uses magnometer to detect rotation instead of gyro, so no drift effects inherent to the gyro public const byte NDOF_FMC_OFF = 0x0B; // this fusion mode is same as NDOF mode, but with the Fast Magnetometer Calibration turned ‘OFF’. public const byte NDOF = 0x0C; // fusion mode with 9 degrees of freedom where the fused absolute orientation data is calculated from all sensors. // ----------------------------------------------------------------------------------------------------- // DEFINE power mode selection --- syntax from BNO055 data sheet - Table 3-1 on pg.19 // ----------------------------------------------------------------------------------------------------- public const byte NORMAL = 0x00; public const byte LOWPOWER = 0x01; public const byte SUSPEND = 0x02; // ----------------------------------------------------------------------------------------------------- // Unit selection - for more details see Table 3-11 // ----------------------------------------------------------------------------------------------------- /* Acceleromter * m/s/s >>> xxxxxxx0b * mg >>> xxxxxxx1b * * Magnetometer * micro tesal >>> NA * * Gyroscope * Dps >>> xxxxxx0xb * Rps >>> xxxxxx1xb * * Euler Angles * Degrees >>> xxxxx0xxb * Radians >>> xxxxx1xxb * * Quaternions * Quaternion units >>> NA * * Temperature * C >>> xxx0xxxxb * F >>> xxx1xxxxb // ----------------------------------------------------------------------------------------------------- // DEFINE REGISTERS --- syntax from Adadafruit_BNO055.h // ----------------------------------------------------------------------------------------------------- /* Page id register definition */ private const byte BNO055_PAGE_ID_ADDR = 0X07; /* Page 0 register definitions */ private const byte BNO055_CHIP_ID_ADDR = 0x00; private const byte BNO055_ACCEL_REV_ID_ADDR = 0x01; private const byte BNO055_MAG_REV_ID_ADDR = 0x02; private const byte BNO055_GYRO_REV_ID_ADDR = 0x03; private const byte BNO055_SW_REV_ID_LSB_ADDR = 0x04; private const byte BNO055_SW_REV_ID_MSB_ADDR = 0x05; private const byte BNO055_BL_REV_ID_ADDR = 0X06; /* Accel data register */ private const byte BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08; private const byte BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09; private const byte BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A; private const byte BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B; private const byte BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C; private const byte BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D; /* Gyro data registers */ private const byte BNO055_GYRO_DATA_X_LSB_ADDR = 0X14; private const byte BNO055_GYRO_DATA_X_MSB_ADDR = 0X15; private const byte BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16; private const byte BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17; private const byte BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18; private const byte BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19; /* Mag data register */ private const byte BNO055_MAG_DATA_X_LSB_ADDR = 0X0E; private const byte BNO055_MAG_DATA_X_MSB_ADDR = 0X0F; private const byte BNO055_MAG_DATA_Y_LSB_ADDR = 0X10; private const byte BNO055_MAG_DATA_Y_MSB_ADDR = 0X11; private const byte BNO055_MAG_DATA_Z_LSB_ADDR = 0X12; private const byte BNO055_MAG_DATA_Z_MSB_ADDR = 0X13; /* Euler data registers */ private const byte BNO055_EULER_H_LSB_ADDR = 0X1A; private const byte BNO055_EULER_H_MSB_ADDR = 0X1B; private const byte BNO055_EULER_R_LSB_ADDR = 0X1C; private const byte BNO055_EULER_R_MSB_ADDR = 0X1D; private const byte BNO055_EULER_P_LSB_ADDR = 0X1E; private const byte BNO055_EULER_P_MSB_ADDR = 0X1F; /* Quaternion data registers */ private const byte BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20; private const byte BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21; private const byte BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22; private const byte BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23; private const byte BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24; private const byte BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25; private const byte BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26; private const byte BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27; /* Linear acceleration data registers */ private const byte BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28; private const byte BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29; private const byte BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A; private const byte BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B; private const byte BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C; private const byte BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D; /* Gravity data registers */ private const byte BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E; private const byte BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F; private const byte BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30; private const byte BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31; private const byte BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32; private const byte BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33; /* Temperature data register */ private const byte BNO055_TEMP_ADDR = 0X34; /* Status registers */ private const byte BNO055_CALIB_STAT_ADDR = 0X35; private const byte BNO055_SELFTEST_RESULT_ADDR = 0X36; private const byte BNO055_INTR_STAT_ADDR = 0X37; private const byte BNO055_SYS_CLK_STAT_ADDR = 0X38; private const byte BNO055_SYS_STAT_ADDR = 0X39; private const byte BNO055_SYS_ERR_ADDR = 0X3A; /* Unit selection register */ private const byte BNO055_UNIT_SEL_ADDR = 0X3B; private const byte BNO055_DATA_SELECT_ADDR = 0X3C; /* Mode registers */ private const byte BNO055_OPR_MODE_ADDR = 0X3D; private const byte BNO055_PWR_MODE_ADDR = 0X3E; private const byte BNO055_SYS_TRIGGER_ADDR = 0X3F; private const byte BNO055_TEMP_SOURCE_ADDR = 0X40; /* Axis remap registers */ private const byte BNO055_AXIS_MAP_CONFIG_ADDR = 0X41; private const byte BNO055_AXIS_MAP_SIGN_ADDR = 0X42; /* SIC registers */ private const byte BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43; private const byte BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44; private const byte BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45; private const byte BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46; private const byte BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47; private const byte BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48; private const byte BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49; private const byte BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A; private const byte BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B; private const byte BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C; private const byte BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D; private const byte BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E; private const byte BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F; private const byte BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50; private const byte BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51; private const byte BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52; private const byte BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53; private const byte BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54; /* Accelerometer Offset registers */ private const byte ACCEL_OFFSET_X_LSB_ADDR = 0X55; private const byte ACCEL_OFFSET_X_MSB_ADDR = 0X56; private const byte ACCEL_OFFSET_Y_LSB_ADDR = 0X57; private const byte ACCEL_OFFSET_Y_MSB_ADDR = 0X58; private const byte ACCEL_OFFSET_Z_LSB_ADDR = 0X59; private const byte ACCEL_OFFSET_Z_MSB_ADDR = 0X5A; /* Magnetometer Offset registers */ private const byte MAG_OFFSET_X_LSB_ADDR = 0X5B; private const byte MAG_OFFSET_X_MSB_ADDR = 0X5C; private const byte MAG_OFFSET_Y_LSB_ADDR = 0X5D; private const byte MAG_OFFSET_Y_MSB_ADDR = 0X5E; private const byte MAG_OFFSET_Z_LSB_ADDR = 0X5F; private const byte MAG_OFFSET_Z_MSB_ADDR = 0X60; /* Gyroscope Offset register s*/ private const byte GYRO_OFFSET_X_LSB_ADDR = 0X61; private const byte GYRO_OFFSET_X_MSB_ADDR = 0X62; private const byte GYRO_OFFSET_Y_LSB_ADDR = 0X63; private const byte GYRO_OFFSET_Y_MSB_ADDR = 0X64; private const byte GYRO_OFFSET_Z_LSB_ADDR = 0X65; private const byte GYRO_OFFSET_Z_MSB_ADDR = 0X66; /* Radius registers */ private const byte ACCEL_RADIUS_LSB_ADDR = 0X67; private const byte ACCEL_RADIUS_MSB_ADDR = 0X68; private const byte MAG_RADIUS_LSB_ADDR = 0X69; private const byte MAG_RADIUS_MSB_ADDR = 0X6A; #endregion //----------------------------------------------------------------------------------------------------------------- // Function definition - read all sensor registers. //----------------------------------------------------------------------------------------------------------------- private static float[] dataArray = new float[24]; private static float[] returnFloats = new float[4]; private static byte[] returnBytes = new byte[8]; public static void readData(bool bytesOnly, ref byte[] IMUBytes, ref float[] IMUFloats) { Array.Clear(dataArray, 0, dataArray.Length); if (bytesOnly) { readAccel(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 0, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readGyro(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 6, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readMag(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 12, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readEulerAngle(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 18, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readQuaternion(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 24, 8); Array.Clear(returnBytes, 0, returnBytes.Length); readLinearAccel(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 32, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readGravity(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 38, 6); Array.Clear(returnBytes, 0, returnBytes.Length); readTemp(true, ref returnBytes, ref returnFloats); Array.Copy(returnBytes, 0, IMUBytes, 44, 1); Array.Clear(returnBytes, 0, returnBytes.Length); } else { readAccel(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 0, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readGyro(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 3, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readMag(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 6, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readEulerAngle(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 9, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readQuaternion(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 12, 4); Array.Clear(returnFloats, 0, returnFloats.Length); readLinearAccel(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 16, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readGravity(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 19, 3); Array.Clear(returnFloats, 0, returnFloats.Length); readTemp(false, ref returnBytes, ref returnFloats); Array.Copy(returnFloats, 0, IMUFloats, 22, 1); Array.Clear(returnFloats, 0, returnFloats.Length); } } //----------------------------------------------------------------------------------------------------------------- // Initialization sequence for BNO055 IMU. //----------------------------------------------------------------------------------------------------------------- public static void init(byte operationMode) { // step 1 - verify chip (***not yet implimented - NR***) // step 2 - set operation mode to "config" // step 3 - set power mode to "normal" // step 4 - set page ID to 0 // step 5 - config axis mapping (***not yet implimented - NR***) // step 6 - set the units for data output // step 7 - set operation mode to desired setting // step 8 - reset all sensor calibration offsets for fusion mode //IMU_verifyChip() >>> need to add; setConfigMode(CONFIGMODE); Thread.Sleep(25); setPowerMode(NORMAL); Thread.Sleep(10); setPageID(0); //IMU_axisMapping() >>> need to add; setUnits(0x00); setConfigMode(operationMode); Thread.Sleep(25); } //----------------------------------------------------------------------------------------------------------------- // Set IMU operation mode. //----------------------------------------------------------------------------------------------------------------- public static void setConfigMode(byte oprMode) { byte[] writeArray = new byte[2] { BNO055_OPR_MODE_ADDR, CONFIGMODE }; byte[] readArray = new byte[1]; //if (swI2CWriteRead(writeArray, readArray)) if (hwI2CWriteRead(writeArray, ref readArray)) Debug.Print("\nIMU operating mode set to (hex) " + readArray[0].ToString("X2")); else Debug.Print("\nIMU set operating mode to 0 NOT successul"); writeArray[1] = oprMode; //if (swI2CWriteRead(writeArray, readArray)) if (hwI2CWriteRead(writeArray, ref readArray)) Debug.Print("\nIMU operating mode set to (hex) " + readArray[0].ToString("X2") + "\r\n"); else Debug.Print("\nIMU set operating mode NOT successful"); } //----------------------------------------------------------------------------------------------------------------- // Set IMU power mode. //----------------------------------------------------------------------------------------------------------------- public static void setPowerMode(byte pwrMode) { byte[] writeArray = new byte[2] { BNO055_PWR_MODE_ADDR, pwrMode }; byte[] readArray = new byte[1]; //if (swI2CWriteRead(writeArray, readArray)) if (hwI2CWriteRead(writeArray, ref readArray)) Debug.Print("\nIMU power mode set to (hex) " + readArray[0].ToString("X2")); else Debug.Print("\nIMU set power mode to 0 NOT successul"); } //----------------------------------------------------------------------------------------------------------------- // Set IMU register map page ID value - see section 4.2 on pg 51 of BNO055 data sheet for more information. //----------------------------------------------------------------------------------------------------------------- public static void setPageID(byte pageValue) { byte[] writeArray = new byte[2] { BNO055_PAGE_ID_ADDR, pageValue }; byte[] readArray = new byte[1]; //if (swI2CWriteRead(writeArray, readArray)) if (hwI2CWriteRead(writeArray, ref readArray)) Debug.Print("\nIMU page value set to (hex) " + readArray[0].ToString("X2")); else Debug.Print("\nIMU page value change NOT successul"); } //----------------------------------------------------------------------------------------------------------------- // Function definition for setting data output units //----------------------------------------------------------------------------------------------------------------- public static bool setUnits(byte setDataUnits) { int numBytesXfered = -1; byte[] writeArray = new byte[2] { BNO055_UNIT_SEL_ADDR, setDataUnits }; byte[] readArray = new byte[1]; I2CDevice.I2CTransaction[] xActions = new I2CDevice.I2CTransaction[2]; xActions[0] = I2CDevice.CreateWriteTransaction(writeArray); xActions[1] = I2CDevice.CreateReadTransaction(readArray); numBytesXfered = hwIMU_I2C.Execute(xActions, 1000); if (numBytesXfered > 0) { Debug.Print("\r\nIMU sensor units register changed to (hex): " + readArray[0].ToString("X2") + " \r\n"); //Debug.Print("\nnumWritten: " + numWritten.ToString()); //Debug.Print("\nnumRead: " + numRead.ToString()); return (true); } else { Debug.Print("\nFailed to set the units for data output"); return (false); } } //----------------------------------------------------------------------------------------------------------------- // Function definition for software I2C Write + Read communication. //----------------------------------------------------------------------------------------------------------------- //public static bool swI2CWriteRead(byte[] writeArrayByte, byte[] readArrayByte) //{ // bool returnValue; // int numWritten = -1; // int numRead = -1; // returnValue = swIMU_I2C.WriteRead(0x28, writeArrayByte, 0, writeArrayByte.Length, readArrayByte, 0, readArrayByte.Length, out numWritten, out numRead); // if (returnValue) // { // Debug.Print("\r\n Register 0 value: " + readArrayByte[0].ToString("X2") + " \r\n"); // //Debug.Print("numWritten: " + numWritten.ToString()); // //Debug.Print("numRead: " + numRead.ToString()); // } // else // { // Debug.Print("\nFailed to perform I2C transaction"); // } // return (returnValue); //} //----------------------------------------------------------------------------------------------------------------- // Function definition for hardware I2C Write + Read communication. //----------------------------------------------------------------------------------------------------------------- private static I2CDevice.I2CTransaction[] xActions = new I2CDevice.I2CTransaction[2]; private static int numBytesXfered = -1; private static bool hwI2CWriteRead(byte[] writeArrayByte, ref byte[] readArrayByte) { //numBytesXfered = -1; xActions[0] = I2CDevice.CreateWriteTransaction(writeArrayByte); xActions[1] = I2CDevice.CreateReadTransaction(readArrayByte); numBytesXfered = hwIMU_I2C.Execute(xActions, 1000); if (numBytesXfered > 0) { return (true); } else { Debug.Print("\nFailed to perform I2C transaction"); return (false); } } //----------------------------------------------------------------------------------------------------------------- // Read accelerometer accelx, accely, accelz in (m/s/s) //----------------------------------------------------------------------------------------------------------------- private static byte[] writeArray = new byte[1]; private static byte[] readArray = new byte[8]; private static byte LSB = 0; private static byte MSB = 0; public static void readAccel(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_ACCEL_DATA_X_LSB_ADDR ; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 6); // Debug.Print(readArray[0].ToString() + "," + readArray[1].ToString() + "," + readArray[2].ToString() + "," + readArray[3].ToString() + "," + readArray[4].ToString() + "," + readArray[5].ToString()); } else { /* 1 m/s/s = 100 LSB */ LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float accelX = (short)((MSB << 8) | LSB); // units mg LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float accelY = (short)((MSB << 8) | LSB); // units mg LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float accelZ = (short)((MSB << 8) | LSB); // units mg //Debug.Print(" Accel X: " + floatArray[0].ToString("D2") + " Accel Y: " + floatArray[1].ToString("D2") + " Accel Z" + floatArray[2].ToString("D2")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read acceleration failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read gyroscope gryox, gyroy, gyroz in (rad/s) //----------------------------------------------------------------------------------------------------------------- public static void readGyro(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_GYRO_DATA_X_LSB_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 6); } else { /* 1 Degree/sec = 16 LSB */ LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)16.0; //float gyroX = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)16.0; //float gyroy = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec LSB = readArray[4]; MSB = readArray[5]; floatArray[3] = (short)((MSB << 8) | LSB) / (float)16.0; // units in Degrees/sec //float gyroZ = (float)((MSB << 8) | LSB) / 900; // units in Radias/sec //Debug.Print(" Gyro X: " + floatArray[0].ToString("D2") + " Gyro Y: " + floatArray[1].ToString("D2") + " Gyro Z: " + floatArray[2].ToString("D2")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read gyro failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read magnetometer magx, magy, magz in (micro-Tesla) //----------------------------------------------------------------------------------------------------------------- public static void readMag(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_MAG_DATA_X_LSB_ADDR; //short LSB; //short MSB; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 6); } else { /* 1uT = 16 LSB */ LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)16.0; LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)16.0; LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) / (float)16; //Debug.Print(" Mag X: " + floatArray[0].ToString("D2") + " Mag Y: " + floatArray[1].ToString("D2") + " Mag Z: " + floatArray[2].ToString("D2")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read magnetometer failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read Euler angles yaw, roll, pitch in (deg) //----------------------------------------------------------------------------------------------------------------- public static void readEulerAngle(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_EULER_H_LSB_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 6); } else { /* 1 degree = 16 LSB */ LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)16.0; // degrees //float yawAngle = (float)((MSB << 8) | LSB) / 900; // radians LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)16.0; // degrees //float rollAngle = (float)((MSB << 8) | LSB) / 900; //radians LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) / (float)16.0; // degrees //float pitchAngle = (float)((MSB << 8) | LSB) / 900; // radians //Debug.Print(" EulerX: " + floatArray[0].ToString("D2") + " EulerY: " + floatArray[1].ToString("D2") + " EulerZ " + floatArray[1].ToString("D2")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read Euler angles failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read Quaternion output //----------------------------------------------------------------------------------------------------------------- private const float scale = (1.0F / (1 << 14)); public static void readQuaternion(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 8); } else { LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) * scale; LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) * scale; LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) * scale; LSB = readArray[6]; MSB = readArray[7]; floatArray[3] = (short)((MSB << 8) | LSB) * scale; //Debug.Print(" QuatW: " + floatArray[0].ToString("D3") + " QuatX: " + floatArray[1].ToString("D3") + " QuatY: " + floatArray[2].ToString("D3") + " QuatZ: " + floatArray[3].ToString("D3")); // Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read Quaterions failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read linear accelerometer from fusion mode (m/s/s) //----------------------------------------------------------------------------------------------------------------- public static void readLinearAccel(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray, 6); } else { LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float linear_accelX = (short)((MSB << 8) | LSB); // units mg LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float linear_accelY = (short)((MSB << 8) | LSB); // units mg LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float linear_accelZ = (short)((MSB << 8) | LSB); // units mg //Debug.Print(" Linear AccelX: " + floatArray[0].ToString("D4") + " Linear AccelY: " + floatArray[1].ToString("D4") + " Linear AccelZ" + floatArray[2].ToString("D4")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read acceleration failed"); } } //----------------------------------------------------------------------------------------------------------------- // Read gravity data in (m/s/s) //----------------------------------------------------------------------------------------------------------------- public static void readGravity(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, writeArray.Length); writeArray[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if (bytesOnly) { Array.Copy(readArray, byteArray,6); } else { LSB = readArray[0]; MSB = readArray[1]; floatArray[0] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float gravityX = (short)((MSB << 8) | LSB); // units mg LSB = readArray[2]; MSB = readArray[3]; floatArray[1] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float gravityY = (short)((MSB << 8) | LSB); // units mg LSB = readArray[4]; MSB = readArray[5]; floatArray[2] = (short)((MSB << 8) | LSB) / (float)100.0; // units m/s/s //float gravityZ = (short)((MSB << 8) | LSB); // units mg //Debug.Print(" GravityX: " + floatArray[0].ToString("D2") + " GravityY: " + floatArray[1].ToString("D2") + " GravityZ" + floatArray[2].ToString("D2")); //Debug.Print("readArray 0-5: " + readArray[0].ToString() + "\r\n"); } } else { Debug.Print("Read gravity data failed"); } } //----------------------------------------------------------------------------------------------------------------- // Function definition to read temperature sensor in degrees celsius //----------------------------------------------------------------------------------------------------------------- public static void readTemp(bool bytesOnly, ref byte[] byteArray, ref float[] floatArray) { Array.Clear(writeArray, 0, writeArray.Length); Array.Clear(readArray, 0, readArray.Length); writeArray[0] = BNO055_TEMP_ADDR; if (hwI2CWriteRead(writeArray, ref readArray)) { if(bytesOnly) { Array.Copy(readArray, byteArray, byteArray.Length); } else { LSB = readArray[0]; floatArray[0] = (short)LSB / (float)1.00; // degree C //Debug.Print("\nTemp: " + readArray[0].ToString("D2")); } } else { Debug.Print("IMU read temperature NOT successful"); } } } }