Connect a IMU sensor to Unity (MPU-6050)
This project show you how we can connect a 6DOF sensor to Unity. For this project, I used the MPU-6050 sensor, which is one of the most popular. If your sensor is different, this example shows you the main steps to follow to connect any sensor.
This package also include example with multiple MPU!
Extending libraries
To have more in formations on how to augment existing libraries, you can watch the dedicated http://www.marcteyssier.com/uduino/tutorials/add-external-libraries video tutorial.
Sending IMU from Arduino to Unity
Complex sensors are complex. Luckily, talented developers provide useful libraries. To connect the IMU to Arduino, we used the powerful i2cdev library. You need to add to your Arduino libraries folder the \Arduino\I2Cdev* folder and the \Arduino\MPU6050* folder. We wire the sensor to communicates in i2c with the Arduino.
This is a simple example included with the library.
To receive the Acceleration data, just uncomment the lines in the arduino code and adapt the c# code!
#include "Uduino.h" // Include Uduino library at the top of the sketch
Uduino uduino("IMU");
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
void setup() {
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(54); //++
mpu.setYGyroOffset(-21); //--
mpu.setZGyroOffset(5);
if (devStatus == 0) {
mpu.setDMPEnabled(true);
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// Error
Serial.println("Error!");
}
}
void loop() {
uduino.update();
if (uduino.isInit()) {
if (!dmpReady) {
Serial.println("IMU not connected.");
delay(10);
return;
}
int mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // check if overflow
mpu.resetFIFO();
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
SendQuaternion();
//SendEuler();
//SendYawPitchRoll();
//SendRealAccel();
//SendWorldAccel();
}
}
}
void SendQuaternion() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("r/");
Serial.print(q.w, 4); Serial.print("/");
Serial.print(q.x, 4); Serial.print("/");
Serial.print(q.y, 4); Serial.print("/");
Serial.println(q.z, 4);
}
void SendEuler() {
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print(euler[0] * 180 / M_PI); Serial.print("/");
Serial.print(euler[1] * 180 / M_PI); Serial.print("/");
Serial.println(euler[2] * 180 / M_PI);
}
void SendYawPitchRoll() {
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print(ypr[0] * 180 / M_PI); Serial.print("/");
Serial.print(ypr[1] * 180 / M_PI); Serial.print("/");
Serial.println(ypr[2] * 180 / M_PI);
}
void SendRealAccel() {
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("a/");
Serial.print(aaReal.x); Serial.print("/");
Serial.print(aaReal.y); Serial.print("/");
Serial.println(aaReal.z);
}
void SendWorldAccel() {
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("a/");
Serial.print(aaWorld.x); Serial.print("/");
Serial.print(aaWorld.y); Serial.print("/");
Serial.println(aaWorld.z);
}If you add some Serial.print commands in the setup function, it might break Uduino auto-detection process
Now we need to add Uduino library in this sketch to send the values to Unity. First, we create an Uduino object named "IMU". At this stage, you can test the code in the Arduino serial monitor. Everything should work as expected.
#include "Uduino.h" // Include Uduino library at the top of the sketch
Uduino uduino("IMU");
Then, we keep the same Arduino code and add uduino.update() in the loop function.
To write the values only when the board is connected, we add the condition uduino.isConnected(). Some delay may be added to not overflow the serial.
void loop() {
uduino.update(); // This part is needed to be detected by Unity.
if (uduino.isConnected()) {
// All the code previously in the loop() function
}
}The values have to be written in the same line. Serial.println should be called only once, ad the send of the loop.
If you are using Uduino Wifi, replace Serial.println by duino.println.
Get sensor values on Unity
In Arduino, the code is printing the values in the loop. We use the delegate OnDataReceived to continuously read the values coming from the serial.
void Start () {
UduinoManager.Instance.OnDataReceived += ReadIMU;
}The ReadIMU will parse the data to use them with unity. The string we receive has the format x/y/z/gx/gy/gz so it needs to be separated (by the character '/' ) and parse in a float.
public void ReadIMU (string data, UduinoDevice device) {
//Debug.Log(data);
string[] values = data.Split('/');
if (values.Length == 5 && values[0] == imuName) // Rotation of the first one
{
float w = float.Parse(values[1]);
float x = float.Parse(values[2]);
float y = float.Parse(values[3]);
float z = float.Parse(values[4]);
this.transform.localRotation = Quaternion.Lerp(this.transform.localRotation, new Quaternion(w, y, x, z), Time.deltaTime * speedFactor);
} else if (values.Length != 5)
{
Debug.LogWarning(data);
}
this.transform.parent.transform.eulerAngles = rotationOffset;
// Log.Debug("The new rotation is : " + transform.Find("IMU_Object").eulerAngles);
}Set default orientation
The IMU usually have a default rotation. To fix that, we added a boolean that will set the rotation offset.
public bool setOrigin = false;
Quaternion rotOffset = Quaternion.identity;
void Update()
{
if(setOrigin)
{
rotOffset = this.transform.rotation;
setOrigin = false;
}
}
void ReadIMU (string data, UduinoDevice device) {
//...
this.transform.rotation = new Quaternion(w, x, y, z) * Quaternion.Inverse(rotOffset);
}