DMP 6-axis processing

Rock, Roll and Yaw... and Barometric pressure!

A change to the Quadcopter design!  Previously I had MPU6050 on a GY-521 board (acc+gyro) and the magnetometer HMC5883L on a GY-271.  I decided I wanted to have a barometer on the flight board for height control.  A bit of research revealed the MS5611 was a pretty sensitive standard barometer.  There wasn't room to add a separate barometer module to the flight control board and in fact it was cheaper to replace the GY-521 and GY-271 board with a single board, GY-86, that contained the accelerometer, gyroscope, magnetometer and barometer!  On this single board we have the MPU6050 accelerometer and gyroscope, magnetometer HMC5883L and barometer MS5611.

Calculating quadcopter orientation

So I'm using the GY-86 10 DOF board to calculate the orientation of the quadcopter.  We have the MPU6050 processing chip on board, and although we can read off the various sensors readings, a technology/software known as Digital Motion Processor™ (DMP™) allows us to get the MPU6050 to do the heavy lifting.  In order to off-load the orientation calculations from the flight board to the MPU6050 and utilise this on board processing, we need to flash the MPU6050 chip with the DMP code to flash the MPU6050 memory every time we reset the chip.  To my knowledge the code has never been officially released, but some very clever people have reverse engineered the necessary code and made it ridiculously easy to flash onto the MPU6050 with an Arduino.

The 6-axis DMP works GREAT, straight out the box too.  We have an on board magnetometer of course, can the 9-axis DMP be used with the HMC5883L magnetometer? Unfortunately, it appears not.  The 9-axis variant appears to work ONLY with AK8975 magnetometer.  We will need to access the magnetometer though, and accessing it whilst the DMP is "on" is interesting in itself.

Reading magnetometer whilst DMP is flashed and combining to get absolute yaw

The MPU6050 is readily accessed by the I2C SDA and SCL lines.  In fact the magnetometer is on an auxiliary I2C bus so is only accessible under certain conditions the main I2C bus.  Whilst DMP is running and producing orientation information, the MPU6050 is in Master mode, meaning that the MPU6050 is an I2C Master to the magnetometer and the magnetometer cannot be queried directly on the main bus.  In order to access the magnetometer, we have to switch off the MPU6050 I2C Master mode and enable a 'bypass' register bit so that we can talk straight through to the magnetometer.

// Magnetometer setup code - magnetometer invisible/inaccessible here.  Need to flip some bits.

MPU6050_setI2CMasterModeEnabled(0); // Set Bit 5 of USER_CTRL (Register 106) to 0
MPU6050_setI2CBypassEnabled(1); // Set Bit of INT_PIN_CFG (Register 55) to 1

// Do stuff here to the magnetometer, it is now accessible!

MPU6050_setI2CBypassEnabled(0); // Disable bypass mode
MPU6050_setI2CMasterModeEnabled(1); // Reinstate MPU6050 in Master mode for DMP to work

This is all very good, but in normal operation mode we don't necessarily want to be disabling the DMP in order to access the auxiliary I2C bus (magnetometer).  In fact we don't have to.  The MPU6050 has a cool feature which means we can tell it the address of the slave magnetometer, and the registers which contain the mag XYZ and it will populate certain registers of the MPU6050 chip - so we can stay in Master mode and read the magnetometer information directly from the MPU6050 chip, on the main I2C bus - cool huh!  The MPU6050 does the leg work each time of getting the magnetometer data from the HMC5883L.  So essentially all we have to do is inform the MPU6050 how to map the magnetometers registers of interest onto the MPU6050 registers.  Interestingly it is also possible to write to, as well as read from, a slave.  In this case we just want to read however.  See I2C_SLV0_ADDR, I2C_SLV0_REG, and I2C_SLV0_CTRL for more details.

So the code in full:

MPU6050_setI2CMasterModeEnabled(0); // Set Bit 5 of USER_CTRL (Register 106) to 0
MPU6050_setI2CBypassEnabled(1); // Set Bit of INT_PIN_CFG (Register 55) to 1

// X axis word
MPU6050_setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
MPU6050_setSlaveRegister(0, HMC5883L_RA_DATAX_H);
MPU6050_setSlaveControl(0, true, false, false, false, 2);

// Y axis word
MPU6050_setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
MPU6050_setSlaveRegister(1, HMC5883L_RA_DATAY_H);
MPU6050_setSlaveControl(1, true, false, false, false, 2);

// Z axis word
MPU6050_setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80); // Tells MPU6050 the address of the magnetometer
MPU6050_setSlaveRegister(2, HMC5883L_RA_DATAZ_H); // The register of the magnetometer to read
MPU6050_setSlaveControl(2, true, false, false, false, 2);  // slavenum, slave_enabled, word_byte_swap, write_mode, group_offset, data_length

MPU6050_setI2CBypassEnabled(0); // Disable bypass mode
MPU6050_setI2CMasterModeEnabled(1); // Reinstate MPU6050 in Master mode for DMP to work

where MPU6050_setSlaveControl() is a function as follows:

void MPU6050_setSlaveControl(uint8_t slavenum, uint8_t slave_enabled, uint8_t word_byte_swap, uint8_t write_mode, uint8_t group_offset, uint8_t data_length)

{

 if (slavenum<3) return;

 uint8_t b=(slave_enabled<<7) | (word_byte_swap<<6) | (write_mode<<5) | (group_offset<<4) | data_length;

 I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + slavenum*3, b);

}

DMP configuration 

It is very simple to configure the update/refresh frequency of the DMP.  In the file containing the actual DMP code that is flashed onto the MPU6050 (MPU6050_6_axis_MotionApps20.h), locate the dmpConfig array, and the following line.

Original, 100 Hz:

0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate 

Change to. 200 Hz:

0x02,   0x16,   0x02,   0x00, 0x00                // D_0_22 inv_set_fifo_rate

The final byte sets the refresh frequency = 200 Hz / (1 + value).  There is a comment in the code that the 200 Hz mode is very noisy.  In the graph below, I've outputted the roll angle during initilisation of the DMP (more on that in a moment).  The x-axis is in milliseconds and the y-axis (roll angle) is in degrees.  The roll at 100 Hz and 200 Hz are plotted - to my eye, the noise on both is extremely small. But this is running undisturbed on a flat surface - perhaps the noise at 200 Hz will reveal itself when I have the board mounted on a quadcopter with motors running!  Something to keep in mind perhaps.  It is possible to see changes in the angle as the DMP algorithm configures itself.

The DMP undergoes an initialisation procedure where the yaw, roll and pitch slowly settle on final values - this can take up to 30 seconds.  I've plotted this below; yaw is blue, orange is pitch and grey is roll.  The sensor is laying on a flat surface, horizontally.  In this case it takes about 10 seconds (10,000 microseconds) for the angles to converge.

In absolute yaw 

Various sources I've read about obtaining the absolute, 'magnetic' yaw and combining with DMP roll and pitch seemed to give funny results for me.  In some cases the mathematical switching of minus/plus signs is due to symmetries in the trigonometrical fucntions and are therefore equivalent.  In other cases, there seems to be typos and copying errors.  The below code works for taking the yaw, pitch and roll from the DMP and calculating the 'heading' (absolute yaw), corrected for pitch and roll.

MPU6050_dmpGetQuaternion(&q, fifoBuffer);

MPU6050_dmpGetGravity(&gravity, &q);

MPU6050_dmpGetYawPitchRoll(ypr, &q, &gravity);

yaw = ypr[0];

pitch = ypr[1];

roll = ypr[2];

mx=MPU6050_getExternalSensorWord(0)-mag_cx;

my=MPU6050_getExternalSensorWord(2)-mag_cy;

mz=MPU6050_getExternalSensorWord(4)-mag_cz;

XH = mx*cos(-pitch) + my*sin(-pitch)*sin(roll) + mz*sin(-pitch)*cos(roll);

YH = -mz*cos(roll) - my*sin(roll);

heading = atan2(YH, XH);

if(heading < 0) heading += 2 * M_PI;

Conversion to C

Finally I wanted to keep the project as a C project rather than convert to C++ to use the DMP Arduino library.  To be honest, this is mostly a reflection of my own biases - I have a C background and I know, more or less, what's going to happen to compiled C code on the AVR.  C++ is fantastically powerful, though I do worry - founded or not - about inefficient code speed, and bloating of code size in this AVR setting.  Also Arduino library functions for interfacing with hardware can be really SLOOOWW.  As we only ever have one instance of an MPU6050 (in this project anyway...), it seemed unnecessary to have a class to derive a single instance.  For these subjective reasons and my self-acknowledged bias, I converted the DMP library to pure C - this was surprisingly easy.  For my own reference it simply involved:

• Converting the classes to just functions, mostly involved changing "::" to "_", and removing the 'class' declaration.  
• The two variables in the MPU6050 were set as global.
• Some of the maths routines used classes for vectors, etc.  I converted these to structs and changed a bit of the way they were handled as a result, accessed and passed to functions, etc.  Minor stuff.