Two MPU-6050s
Ugh. After learning how to talk to one accelerometer, I needed to talk to two of them. The MPU-6050 is, for reasons I do not understand, bound to I2C addresses 0x68 or 0x69. In order to put two of them on the same I2C bus, one will have to take on 0x68 and the other 0x69. The I2Cdev library makes assigning these addresses on the software side pretty straightforward. You specify a sensor’s address at construction time:
MPU6050 sensorA(0x68);
MPU6050 sensorB(0x69);
On the hardware side, the MPU-6050 listens to its ADO pin to know which address it has been assigned. If the voltage is low, it associates with 0x68. If high, 0x69. We’ll need to connect an output pin on the Arduino to the ADO pin of the MPU-6050. Suppose we use pin 8 on the Arduino for the 0x68 sensor. In our sketch, we add:
#define ADO68 8
pinMode(ADO68, OUTPUT);
digitalWrite(ADO68, LOW);
Since each sensor will receive a different ADO signal, we need an output pin per sensor. Let’s add pin 9 for the 0x69 sensor.
#define ADO69 9
pinMode(ADO69, OUTPUT);
digitalWrite(ADO69, HIGH);
So, my connections looked like this, from Arduino to the MPU-6050s:
- 3.3V to both VCCs.
- GND to both GNDs.
- Analog 4 to both SDAs.
- Analog 5 to both SCLs.
- Digital 8 to the first sensor’s ADO.
- Digital 9 to the second sensor’s ADO.
The first four connections are all shared. We can fan out the signal. Also note that there’s no connection to INT here. More on that later.
All this is great, but we had hoped to talk to more than two MPUs. Two addresses wasn’t going to be enough. When I searched the internet to figure out how to scale this up, I found myself in uncharted waters. People said it probably could be done, but I didn’t find anybody that lived to tell about it. What I did find suggested that I should be able to read from a whole lot of MPUs with the following algorithm, which cycles through all the sensors and has only the active sensor associated with 0x68:
for each mpu
set mpu to 0x68 through its ADO
for each other mpu
set other mpu to 0x69 through its ADO
read the 0x68 mpu
On the software side, all MPUs stay registered with 0x68. It’s only on the hardware side where things change—the inactive ones respond to 0x69 commands, of which there are none because the software is only talking to 0x68. In this way, at any given moment, only the active MPU will ever send data.
I made an Accelerometer class to manage the state of a single MPU-6050 and threw the activation cycling into loop. Here’s the complete sketch, which depends on the I2Cdev library:
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
// CONSTANTS ------------------------------------------------------------------
#define MPU_COUNT 2
const int ADO_PINS[MPU_COUNT] = {8, 9};
#define READ_ADDRESS 0x68 // could have been 0x69
#define MILLIS_TILL_RESET 1000
#define MILLIS_TILL_SHIP 100
// TYPES ----------------------------------------------------------------------
class Accelerometer : public MPU6050 {
public:
Accelerometer(int id) :
MPU6050(READ_ADDRESS),
id(id) {
}
bool initialize() {
MPU6050::initialize();
pinMode(ADO_PINS[id], OUTPUT);
last_serial_at = millis();
dev_status = dmpInitialize();
if (dev_status == 0) {
Serial.println(F("Enabling DMP..."));
setXAccelOffset(-1093);
setYAccelOffset(1662);
setZAccelOffset(1526);
setXGyroOffset(37);
setYGyroOffset(-40);
setZGyroOffset(57);
setDMPEnabled(true);
packet_size = dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(dev_status);
Serial.println(F(")"));
}
return dev_status == 0;
}
void deactivate() {
int drop_state = READ_ADDRESS == 0x68 ? HIGH : LOW;
digitalWrite(ADO_PINS[id], drop_state);
}
void activate() {
int read_state = READ_ADDRESS == 0x68 ? LOW : HIGH;
digitalWrite(ADO_PINS[id], read_state);
}
void read() {
if (dev_status != 0) {
Serial.print("reinitializing ");
Serial.println(id);
initialize();
}
if (dev_status != 0) {
return;
}
// Wait until we have enough data. Once we do, let's push out a
// quarternion onto the serial port! After abandoning interrupts,
// which didn't make sense with multiple MPU6050s issuing them,
// this code would occasionally hang. The workaround for the time
// being is to reset the MPU if it takes to long to acquire a
// full packet.
fifo_size = getFIFOCount();
if (fifo_size == 1024) {
resetFIFO();
Serial.println(F("FIFO overflow!"));
} else {
bool is_reset = false;
unsigned long polling_started_at = millis();
while (!is_reset && fifo_size < packet_size) {
fifo_size = getFIFOCount();
if (millis() - polling_started_at > MILLIS_TILL_RESET) {
Serial.print("resetting ");
Serial.println(id);
is_reset = true;
}
}
if (is_reset) {
initialize();
resetFIFO();
} else {
getFIFOBytes(fifo_buffer, packet_size);
fifo_size -= packet_size;
emit_quaternion();
}
}
}
void emit_quaternion() {
// The serial reading in Unity seems to get bogged down if the data
// comes in too quickly. We issue a serial write only once every
// MILLIS_TILL_SHIP milliseconds.
unsigned long presentTime = millis();
unsigned long elapsedTime = presentTime - last_serial_at;
if (elapsedTime > MILLIS_TILL_SHIP) {
dmpGetQuaternion(&q, fifo_buffer);
Serial.print("[");
Serial.print(id);
Serial.print("\t");
Serial.print(q.w, 5);
Serial.print("\t");
Serial.print(q.x, 5);
Serial.print("\t");
Serial.print(q.y, 5);
Serial.print("\t");
Serial.print(q.z, 5);
Serial.println("]");
last_serial_at = presentTime;
}
}
private:
int id;
uint16_t fifo_size;
uint8_t fifo_buffer[64];
uint8_t dev_status;
uint16_t packet_size;
unsigned long last_serial_at;
Quaternion q;
};
// GLOBALS --------------------------------------------------------------------
bool is_dmp_ready = false;
Accelerometer mpus[] = {
Accelerometer(0),
Accelerometer(1)
};
// ----------------------------------------------------------------------------
void setup() {
Wire.begin();
Wire.setClock(400000);
Serial.begin(115200);
Serial.println("Gimme a char, please, and then I'll start up the MPUs.");
while (Serial.available() && Serial.read());
while (!Serial.available());
while (Serial.available() && Serial.read());
Serial.println("Initializing DMP...");
is_dmp_ready = true;
for (int i = 0; i < MPU_COUNT; ++i) {
is_dmp_ready = is_dmp_ready && mpus[i].initialize();
}
}
// ----------------------------------------------------------------------------
void loop() {
if (!is_dmp_ready) return;
for (int i = 0; i < MPU_COUNT; ++i) {
// Associate this MPU with the read address.
mpus[i].activate();
// Divert all other MPUs to the ignored address.
for (int oi = 0; oi < MPU_COUNT; ++oi) {
if (i != oi) {
mpus[oi].deactivate();
}
}
mpus[i].read();
}
}
The code is largely a reorganization of the I2Cdev example that deals with a single MPU-6050, with activation cycling thrown in. Another significant difference is that I removed all the interrupt logic. When a sensor has data, it can signal the Arduino that it’s ready to be read from. But for this to work, each sensor would need to have its own interrupt pin, and I don’t have that many pins. I wanted to strip out the nonessentials.
Removing interrupts led to frequent hanging. I don’t really know what was wrong, but resetting the sensor if it took too long to fill its buffer seemed to set things right!
To test things out, I went to Unity and wired up one sensor to the left box and the other to the right box with this controller:
using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System;
using UnityEngine;
public class GameController : MonoBehaviour {
public Transform left;
public Transform right;
private SerialPort serial;
private bool isReading;
void Start() {
serial = new SerialPort("/dev/tty.usbmodem1411", 115200);
serial.ReadTimeout = 100;
serial.Open();
isReading = false;
}
void Update() {
if (Input.GetKeyDown (KeyCode.I)) {
serial.WriteLine ("y");
Debug.Log("start 'em");
isReading = true;
}
if (isReading) {
try {
string line = serial.ReadLine().TrimEnd();
if (line.StartsWith("[") && line.EndsWith("]")) {
string[] thingys = line.Substring(1, line.Length - 2).Split('\t');
if (thingys.Length == 5) {
int i;
int.TryParse(thingys[0], out i);
Quaternion q = new Quaternion();
float.TryParse(thingys[1], out q.w);
float.TryParse(thingys[2], out q.y);
float.TryParse(thingys[3], out q.z);
float.TryParse(thingys[4], out q.x);
q.x *= -1;
q.y *= -1;
if (i == 0) {
left.localRotation = q;
} else {
right.localRotation = q;
}
}
} else {
Debug.Log(line);
}
} catch (TimeoutException) {
}
}
}
}
And here are the two sensors in action:
I think my breadboard connections are a little loose. When the wires don’t bounce around, the rotation is pretty smooth.
Now, let’s get sixteen MPU-6050s working!