What better way to christen the blog than to have a post dual as a rubber duck? Below lies a recount of the efforts to remedy the issues encountered when working with the gyroscopic sensors used as part of the active component of our camera stabiliser.
The big picture
Here we are working purely on the active portion of the stabiliser, which corrects about $\pm45°$ in the roll and pitch axis, and $\pm180°$ in the yaw axis. In each axis, the logic of the active portion would act as such:
This is simple enough, but of course, the devil is in the details.
What doesn’t work
The gyroscope worked initially. The motors worked. But they didn’t work together. Examples of the outputs are show below – here we have programmed the Arduino to format the data in JSON.
This is when things are going okay
...
{"speed":-31,"time":2762,"euler":{"x":-0.21,"y":0.02,"z":-0.13}}
{"speed":-28,"time":2775,"euler":{"x":-0.19,"y":0.02,"z":-0.13}}
{"speed":-25,"time":2787,"euler":{"x":-0.17,"y":0.02,"z":-0.13}}
{"speed":-22,"time":2799,"euler":{"x":-0.16,"y":0.02,"z":-0.13}}
{"speed":-22,"time":2810,"euler":{"x":-0.14,"y":0.02,"z":-0.13}}
{"speed":-19,"time":2823,"euler":{"x":-0.13,"y":0.02,"z":-0.13}}
{"speed":-17,"time":2835,"euler":{"x":-0.11,"y":0.02,"z":-0.13}}
{"speed":-14,"time":2847,"euler":{"x":-0.10,"y":0.02,"z":-0.13}}
{"speed":-11,"time":2859,"euler":{"x":-0.08,"y":0.02,"z":-0.13}}
{"speed":-8,"time":2871,"euler":{"x":-0.07,"y":0.03,"z":-0.13}}
{"speed":-5,"time":2883,"euler":{"x":-0.05,"y":0.03,"z":-0.13}}
{"speed":-5,"time":2895,"euler":{"x":-0.04,"y":0.03,"z":-0.12}}
{"speed":-2,"time":2907,"euler":{"x":-0.02,"y":0.03,"z":-0.12}}
{"speed":-0,"time":2919,"euler":{"x":-0.01,"y":0.03,"z":-0.12}}
{"speed":0,"time":2931,"euler":{"x":0.01,"y":0.03,"z":-0.12}}
...The speed is a number whose magnitude $0\leq|\text{speed}|\leq255$ represents $\text{V}_{\text{motor},\text{low}}$ and $\text{V}_{\text{motor}, \text{high}}$ respectively in a DC motor, while its sign represents the polarity of the circuit – hence it gives the direction of rotation of the motor. The sign is arbitrary, and depends on how the circuit is wired.
Furthermore, it works slightly differently with a brushless and a stepper motor, though the behaviour of each should be comparable – a higher magnitude should correspond to a higher torque in that motor. Its implementation will be explored in another post.
The time is the time passed in milliseconds since the Arduino started the script.
The euler object is an object that gives the angle measurements in terms of yaw (x), roll (y), and pitch (z).
We are working first with the yaw axis – here given by euler.x. With this, the effect of the above output is clear:
- At $\mathrm{time} = 2762$, the camera is pointing at $\mathrm{yaw} = -0.21\,\mathrm{rad}$, and with that data, the motor’s speed is $-31$. This rotates the camera towards the origin, or $\mathrm{yaw} = 0$.
- Sometime later, at $\mathrm{time} = 2810$, we have $\mathrm{yaw} = -0.14\,\mathrm{rad}$ and $\mathrm{speed} = -22$. The yaw of the camera gets closer to the origin, and so does the speed of the motor decrease due to the lessened correction needed by the gimbal.
- Finally, at $\mathrm{time} = 2931$, we have $\mathrm{yaw} = 0.01$, and the motor stops, since there is no longer any need for a correction.
When working correctly, we can see how it took $2931-2762=169\,\mathrm{ms}$, to correct a camera that has been offset by $\mathrm{yaw} \approx-12°$: quickly, but continuously.
This is when things are going wild
...
{"speed":-164,"time":61282,"euler":{"x":-1.03,"y":-0.64,"z":0.14}}
{"speed":-0,"time":61292,"euler":{"x":-0.00,"y":-0.00,"z":1.57}}
{"speed":34,"time":61302,"euler":{"x":0.22,"y":-0.23,"z":-1.18}}
{"speed":255,"time":61313,"euler":{"x":1.72,"y":0.33,"z":1.15}}
{"speed":-0,"time":61322,"euler":{"x":0.00,"y":0.00,"z":1.57}}
{"speed":-255,"time":61332,"euler":{"x":-3.14,"y":0.06,"z":-0.13}}
{"speed":144,"time":61342,"euler":{"x":0.90,"y":1.49,"z":0.08}}
{"speed":-116,"time":61352,"euler":{"x":-0.73,"y":0.23,"z":0.87}}
{"speed":19,"time":61362,"euler":{"x":0.14,"y":0.71,"z":0.78}}
{"speed":147,"time":61373,"euler":{"x":0.91,"y":1.27,"z":0.23}}
{"speed":56,"time":61382,"euler":{"x":0.35,"y":-0.83,"z":0.69}}
{"speed":28,"time":61392,"euler":{"x":0.18,"y":-0.47,"z":-0.80}}
{"speed":-96,"time":61403,"euler":{"x":-0.60,"y":0.20,"z":-0.18}}
{"speed":-229,"time":61412,"euler":{"x":-1.42,"y":-1.13,"z":0.35}}
...After a few seconds, something like this happens. The measurements expounded by the gyroscopic sensors become complete garbage, and the motor reacts accordingly by swinging wildly and randomly. Note that these are values from when the gimbal is entirely stationary, and not at all perturbed.
This is when things go quiet
There is nothing much to show for this: after things go crazy (or sometimes, the crazy section doesn’t even happen), the Arduino stops responding, and no new measurement is passed into the output. The motor continues rotating in the last defined speed until either the power source is removed, or the Arduino physically reset.
Our initial suspicions
Given the behaviour of things going crazy, it seemed like some form of interference was going on. Hence, we postulated
- Electromagnetic interference – perhaps the magnetic fields induced by the motor interferes with the gyroscopic internals
- Mechanical interference – perhaps the vibration of the motor causes the sensor to give inaccurate data points
- Electrical interference – perhaps the current/voltage backwash from the motor driver is interfering with the analogue pins used to transfer the sensor data
Our first attempt at a resolution involved tackling the first two points. Electromagnetic interference was minimised by enclosing the sensor in a small metal box used for storing pen nibs, while mechanical interference was minimised by enclosing that in a watch pillow. That did not improve the situation at all.
Oh hindsight, electrical interference was the most likely. We were still using the h-bridge we built for ourselves using component transistors, resistors and diodes. Although it included some form of back e.m.f. protection via the diodes, our unfamiliarity then (and now) with the nuances of electrical engineering meant that our choice of components might have caused an oversight, resulting in inadequate protection. Eventually, we removed all of the motorised components in a desperate attempt to get just the sensor working at the very least.
On removing the motorised components, the garbage sensor output no longer occurred, but the sensor still went quiet after a while. This pointed to a problem with the gyroscopic sensor, and henceforth, the story truly begins.
A new beginning
Since the sensor seemed to be the problem, the thinking that perhaps it was the particular unit we were using that was faulty led us to buy another of the same (MPU-6050) and an upgraded model (MPU-6500). Desperation brought us to even get brand new Arduino Nanos due to similar fears.
The feedback given during our progress meeting was also taken. Voltages and currents running through the circuit were checked and double-checked with various multimeters at hand. Interestingly, we found that both VOUT ports – $3.3\,\mathrm{V}$ and $5\,\mathrm{V}$ – were slightly undervolted, actually giving out $3\,\mathrm{V}$ and $4\,\mathrm{V}$ respectively. Nonetheless, this shouldn’t cause any problems as the MPU-6050/6500 boards are paired with a voltage regulator whose input is rated at $3\,\mathrm{V} – 5\,\mathrm{V}$, so attaching it into the $5\,\mathrm{V}$ VOUT port is well within the acceptable range.
Nothing above made the problem go away, so we will had to look deeper into the interface between our Arduino and the problematic family of sensors.
The antagonists of this story
Gyroscopic sensors we are using: the MPU6050 (left) and the MPU6500 (right)
For our project, we are using the TDK InvenSense Six-axis gyroscope/accelerator, which is commonly used to maintain flight orientation in drones. Part of its draw is its onboard Digital Motion Processor (DMP), which automatically calibrates and translates the sensor’s inner readings into readable measurements, offloading computation from the main microprocessor.
We have used Jeff Rowberg’s i2cdevlib to allow for our Arduino Nano to communicate with the Gyroscope sensors through the I²C bus. Given the specific sensor implementation, our logic flow looks like the following:
Due to how the inter-device communication works, there are a few subtleties involved in the steps to get the data from the gyroscopic sensor:
- The gyroscope is constantly making measurements and processing them. When each measurement is ready, it is passed into a buffer.
- Computation on the sensor’s side takes time, so we cannot constantly read the buffer – this results in repeated (and thus wrong) data points. To buffer is accessed only after the data is ready, a “data ready” flag is set.
- The “data ready” flag is set using the interrupt pins of the Arduino. The Arduino constantly monitors the interrupt pin for change, and triggers a section of the code interrupting whatever is currently running. Hence, we reset the “data ready” flag at the start of the loop, knowing that it will be automatically set the moment new sensor data has been written into the buffer.
After getting the orientation data, we simply set the motor according to the magnitude and direction of the angle measurement. Through our motor tests, we have a function setMotor(speed, direction, motor) that is defined differently according to the type of motor being used (that is, we define a function that takes in the same three arguments, but sends out a certain set of instructions in the script where we are testing a brushed DC motor, and another if we are testing a stepper motor, etc.).
This was the approach we used to build the smaller-scale Lego single and double-axis stabiliser. The setMotor() function also worked as expected in their independent (sensorless) tests.
Pinpointing the failure point
It was likely that the failure is due to an infinite loop or a synchronous instruction that never gets its response. To pinpoint where this failure occurs, we added a serial print behind every major instruction that prints exactly what the code is doing that that point:
...
volatile bool mpuInterrupt = false;
void dmpDataReady() {
Serial.println(F("Interrupt"));
mpuInterrupt = true;
}
...dmpDataReady() runs at every “interrupt”, which triggers whenever the data from the sensor is ready.
...
void loop() {
if (!dmpReady)
return;
mpuInterrupt = false;
Serial.println(F("Get interrupt status"));
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("Get buffer count"));
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
Serial.println(F("Get buffer count again"));
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
Serial.println(F("Get buffer"));
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
Serial.println(F("Get quaternion"));
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.println(F("Get gravity"));
mpu.dmpGetGravity(&gravity, &q);
Serial.println(F("Get yaw, pitch & roll"));
mpu.dmpGetYawPitchRoll(euler, &q, &gravity);
...After adding the serial prints, an interesting behaviour is observed upon failure:
...
Interrupt
Get interrupt status
Get buffer count
Get buffer count again
Get buffer
Get quaternion
Get gravity
Get yaw, pitch & roll
{"time":15703,"euler":{"x":-0.24,"y":0.01,"z":-1.49}}
Interrupt
Get interrupt status
Get buffer count
Get buffer count again
Interrupt
Interrupt
Interrupt
Interrupt
Interrupt
Interrupt
Interrupt
Interrupt
...
Notice not only that the programme fails at line 11476, but also, more importantly, that the interrupt doesn’t stop triggering!
This allows us to detect when the programme has failed, even when it fails non-responsively in the main loop, by attaching some kind of timeout:
...
volatile bool mpuInterrupt = false;
unsigned long lastInterrupt = 0;
bool resetSetup = false;
#define TIMEOUT 1000
void dmpDataReady() {
if (millis()-lastInt > TIMEOUT && !resetSetup) {
Serial.println("It has died.");
resetSetup = true;
}
mpuInterrupt = true;
}
......
void loop() {
...
lastInterrupt = millis();
}
Essentially we introduce a variable lastInterrupt that is overwritten with the time when the main loop runs. When everything is working well, this variable is always milliseconds within the current time. However, upon failure, lastInterrupt stops updating, but the dmpDataReady() does not. So, if the actual current time is much later than the last time the loop has run, we know that the main loop is no longer responsive.
...
Get quaternion
Get gravity
Get yaw, pitch & roll
{"time":4172,"euler":{"x":0.00,"y":-0.03,"z":-0.10}}
Get interrupt status
It has died.
In the above example, we used a timeout of 1 second: previously we’ve seen that the main loop runs every few milliseconds, so if it is no longer actionable for an entire second, it has, for certain, broken down.
If at first you don’t succeed, lower your expectations
There were a few more things we tried doing in order to salvage the situation, but given that this post is already past a thousand and a half words, we shall fast forward to our current situation, where we gave up trying to solve the the problem itself and instead looked at mitigating its effects. For safety, we stopped the motors once the failure was detected; for faux-continuity, we reset the Arduino to get everything working again.
...
volatile bool mpuInterrupt = false;
unsigned long lastInterrupt = 0;
bool resetSetup = false;
#define TIMEOUT 1000
void dmpDataReady() {
if (millis()-lastInt > TIMEOUT && !resetSetup) {
Serial.println("It has died.");
setMotor(0, 0, 1);
pinMode(RESET_PIN, OUTPUT);
resetSetup = true;
}
mpuInterrupt = true;
}
...setMotor(0, 0, 1); shuts down the motor. RESET_PIN is a pin connected to the reset pin and armed as HIGH, allowing pinMode(RESET_PIN, OUTPUT); to immediately trigger the physical reset of the Arduino. Where previously our setup runs for a while and goes crazy, with this code, it runs for a while, pauses for a few seconds, then continues running, and so on. While not actually solving the problem, it prevents this issue from being a complete roadblock. Until we have another breakthrough, this sloppy solution shall do.