Calibrating HMC5983 Magnetometer for Hard- and Soft-iron

In a couple previous posts we have been discussing HMCSPIlibrary, which is responsible for communication with the HMC5983 magnetometer from Honeywell installed on my board. If you were checking the code and followed on my explanation you might have noticed that HMCSPI_Local.c code file includes several constant vectors:

// MAG Hard Iron correction vector

Vector      _HMC_HardIron     =  {-22.85, -112.55, -38.64};

// MAG Soft Iron correction matrix

Vector      _HMC_SoftIron_X   =  { 1.0000, -0.0183,  0.0305};

Vector      _HMC_SoftIron_Y   =  {-0.0183,  1.0269,  0.0009};

Vector      _HMC_SoftIron_Z   =  { 0.0305,  0.0009,  1.0585};

These vectors are applied to the raw HMC5983 measurements in function _HMC_NormHMCData(…) defined in the header file HMCSPI_Inlines.h. So what are these vectors and where their values come from and what does the function _HMC_NormHMCData(…) do? These vectors represent the hard-iron correction vector and soft-iron correction matrix calculated specifically for the sensor installed on the board and _HMC_NormHMCData(…) applies these corrections to the magnetometer measurements. In this post we will see how these values are calculated and how we can apply them to improve precision of our measurements.

First, let’s define the problem. When a sensor installed on the board it becomes a subject to interference from the nearby components, board traces (even if we follow manufacturer’s recommendation on how to lay out the traces), some physical stress on the package resulting from soldering, etc. Despite the datasheet statement about factory calibration of all the sensors, I still haven’t seen a single magnetometer that would not require in-place calibration!

When we rotate magnetometer in Earth magnetic field we would expect that the measurement vector would follow a sphere centered at the origin of the coordinate system. In practice most of the time the measurement vector would follow ellipsoid with the center at some offset from the origin of coordinate system. The “hard-iron” correction centers measurements around the origin of coordinate system and “soft-iron” correction converts ellipsoid back to a circle. Application notes AN4246 and AN4248 from Freescale WEB site provide a wealth of details about the effects of distortion on magnetic measurements and how to correct them. The algorithm discussed in this post is based upon the information provided in these notes. AN4246 also provides a picture illustrating measurements subject to hard- and soft-iron distortion and results of correction, which I copied for this post:

The pink ellipsoid with the red dots represent uncorrected measurements achieved while rotating magnetometer while the bluish sphere with blue dots represent the same measurements after the hard- and soft-iron correction is applied.

The idea of the hard- and soft-iron correction of magnetometer measurements (slightly simplified notation as compared to AN4246) is that the corrected vector M’ can be obtained from the raw vector of measurements M by applying the following linear transformation:

M’ = W*(M – V),                    (1)

where V is the hard-iron correction vector and W is a 3x3 symmetric matrix transforming ellipsoid into a sphere, which is the soft-iron correction. So the problem is reduced to identifying these vector V and matrix W. The application notes referred above discuss analytic methods of identifying these elements; I resorted to Excel and Solver to calculate vector V and matrix W based upon the results of multiple measurements, which I will discuss below. To collect these measurements I programmed the board with the code generated from project 13-HMCSPI-Async, which we briefly discussed in one of the previous posts. Obviously for this experiment I was interested in uncorrected measurements so initially the hard-iron correction vector V was set to a zero-vector and soft-iron correction matrix W was set to be an identity matrix in the HMCSPI_Local.c code file.

After programming the board with the firmware from this project, I started the board and collected about 25,000 measurements while rotating the board in all 3 dimensions. All collected measurements I imported into Excel. The following picture provides the snapshot of the spreadsheet preloaded with the samples data:

Columns headed Mx, My, and Mz represent respective components of the magnetic vector as reported by the sensor. For each of these columns I calculated the minimum and maximum value (rows Min and Max) and their mid-point (average between respective Min and Max) in the row Vinit. The formula for Vinit(X) is shown on the snapshot above. Now for each sample we should calculate the strength of the magnetic field derived from the respective measurement as illustrated in the following picture:

The strength of the magnetic field is the size of the measurement vector; we calculate it as a square root of the dot product of the measurement vector to itself as illustrated in the picture above. Just remember that this is an “array formula” so after it is built in in the formula bar to commit it to the spreadsheet you need to press Ctrl+Shift+Enter keys simultaneously. After you enter the formula in the first cell you may extend it down to calculate magnetic field strength for each of the measurements. After I did that I calculated average strength of the magnetic field across all measurements.

In the ideal case all these measurements should be on the surface of a sphere; to evaluate how close our measurements match the sphere we can calculate the variance in the size of the magnetic field across all measurements. The formula for this calculation is presented on the picture below – just remember that this is again is an “array formula”:

Unfortunately both the variance at 3,774.96 and the standard deviation at 61.5 are quite large indicating that the measurements are quite far from being on the sphere so there is a need for hard- and soft-iron correction. Next to the columns containing measurement data we will allocate columns for corrected values as well as the hard-iron correction vector V and soft-iron correction matrix W. We will copy values of Vinit onto vector V as they represent a good initial estimate and set matrix W to be the identity matrix. As we know matrix W to be symmetric, in the cells under main diagonal (highlighted in blue) instead of actual 0 values I entered the formula making their values equal to the respective values above the diagonal as illustrated on the picture below:

To simplify entering further formulas on the spreadsheet it is convenient to assign names to the range of cell representing vector V – naturally I gave it a name “V”. Similarly the range of cells representing matrix W was named “W”. Now that we have the original measurements and vector V and matrix W, we may enter the formula for calculating corrected measurement vector M’ in accordance with formula (1) above.

Excel snapshot above illustrates this formula for the first row of corrected values – from the vector of original measurements B2:D2 we subtract vector V, transpose the result to make it into a column vector, which we then left-multiply by the matrix W using Excel MMULT function. Result of multiplication is again transposed to make it into a row and this formula is entered as Excel array formula into cells H2:J2 as the first row of corrected measurements. Now this formula could be extended down to calculate corrected measurements for each of the source measurements.

Same as we did before for the original measurements, we should calculate the strength of magnetic field for each of the corrected measurement, the average strength of magnetic field, its variance and standard deviation. Results are presented on the picture below:

As you may see from the picture above, by applying very simple hard-iron correction using Vinit vector the variance of the results and standard deviation have been reduced quite significantly! Let’s see what we can achieve by optimizing hard- and soft-iron correction values – our tool to achieve this will be Excel’s Solver. Our variables will be the elements of vector V and highlighted in red elements of matrix W. Using these variables we will instruct Solver to find a solution that minimizes the value of the variance. There is one caveat to this approach – we have to force one of the diagonal elements of W to be 1.0; otherwise Solver will just bring both vector V and matrix W to 0 resulting in a very trivial solution. The limitation of this approach is that we will not be able to identify exactly the strength of magnetic field, but it will be proportional to the value we calculate while being a few percentage points off. When configuring Solver just remember to uncheck “Make Unconstrained Variables Non-Negative” as shown in the following screenshot:

Now we may run the Solver! On my Core i-7 machine Solver took about 10 minutes to reach the optimal solution – your results could vary. After accepting Solver’s solution, I got the following results:

From comparing original samples with corrected ones we may see that applying hard- and soft-iron correction to magnetometer measurements decreased variance of the measurements more than 100-fold and brought standard deviation down to about 1%. The evaluated strength of Earth magnetic field achieved after correction is around 579 mGa, which is consistent with the expectations for my location. The only thing left is to copy the values of vector V and matrix W into the code as the constants specific to my sensor on the board, which now explains the constants you saw in HMCSPI_Local.c code file J.

I do not have tools to create nice 3-D diagrams like those provided by Freescale, so I resort to a simple chart tracing the strength of magnetic field (the length of the magnetic measurement vector) across original and corrected measurements, which is presented below:

The corrected one looks much more reliable to use for estimating orientation based upon the magnetic vector – at least to me.

Having reliable sensor data is quite important for achieving stability in the control algorithm. Looks like we addressed this issue for the magnetometer, so in the next post we will look at another calibration – applying temperature compensation to MPU-6050 measurements.

Using ADC to measure battery voltage and charge level

For proper control of a drone the flight control board need to know the charge level of the battery so that it may take necessary actions before the battery get depleted. Luckily we do not need to equip the board with any special sensor to achieve this as all PIC MCUs have the ADC (Analog-to-Digital Converter) module – we just need to know how to use it.

PIC ADC can convert voltages ranging between 0 and Vdd, which, in case of our MCU, is 3.3V. Obviously this is not enough to measure LiPo battery voltage which ranges from 4.2V to 12.6V for 1 to 3 cell battery – thus we need a voltage divider to bring it into an acceptable range. There are several requirements that the voltage divider should satisfy. First, it should bring low-side voltage to the range of 0 to 3.3V for all possible input voltages. Second, it should not drain too much power from the battery. Third, the power dissipated by the divider’s resistors should be within the acceptable range for these resistors. Finally, voltage divider should be able to provide sufficient current so that the Charge Holding Capacitor (CHOLD) on the input to ADC can fully charge during the sampling interval. The last requirement, to some extent, is at odds with the first three – we will discuss later on in this post what can be done to meet it.

On my board the voltage divider is represented by 2 resistors – 3.3 kOhm on the high-side and 1 kOhm on the low side (between the input to ADC and ground). This result in voltage dampening rate of 1/(3.3 + 1) ~ 0.2326, which results in maximum acceptable input voltage of 3.3V/0.2326 ~ 14.2V, which is well below the maximum voltage of 3-cell LiPo battery of 12.6V. For a 3-cell battery divider will consume about 12.6V/(3.3kOhm + 1kOhm) ~ 2.9mA, which is much smaller than the power consumption of the rest of the board and absolutely negligible as compared to the power consumption of the motors. At this current the high-side resistor of 3.3kOhm will dissipate about 30mW, which is well within the range of 125mW for 0805 resistor and 100mW of 0603, so here we are good as well. The numbers will be even smaller for 2- or 1-cell battery.

PIC ADC can operate at 10-bit or 12-bit precision – for our purpose we would select the 12-bit mode so that ADC will have enough precision to accurately represent voltages across the whole range of possible inputs (1- to 3-cell batteries). In a 12-bit mode ADC is capable of speed of up to 500 kilo-samples per second. To achieve this speed ADC requires the input to provide around 15mA to allow the CHOLD to fully charge during a fraction of a microsecond allowed for sampling the input. Battery voltage does not change that fast so we may safely reduce ADC speed to the minimum recommended sampling rate of 10 kilo-samples per second. Considering that by reducing speed we increased sampling interval roughly 50 times, a few milliamperes provides by the voltage divider should be enough to charge the CHOLD.

To measure voltage and, subsequently, discharge level of the battery, using the ADC we need to establish conversion factor – volts per LSB (least significant bit) of the ADC sample. In a 12-bit mode ADC sample is a number between 0 for input at the ground level and 4095 for input voltage being at the level of Vdd, which is 3.3V, thus the sample-to-voltage conversion factor for the ADC is ideally 3.3V/4095 = 0.0008059 V/LSB. Combining this with the voltage divider ratio we may obtain sample-to-input-voltage conversion ratio as 0.0008059/0.2325581 = 0.003465 V/LSB.

Now we may calculate several important ADC values corresponding to the following voltages:

Important Voltages	ADC Counts	Comment
1	289	Useful discharge range per cell
3.2	923	Cutoff discharge voltage per cell
5.7	1,645	Min voltage for 2-cell battery
8.8	2,540	Min voltage for 3-cell battery

We would like our battery management code to automatically identify number of cells of the battery – the last two entries in the table above serve exactly this role. The code assumes that if during initialization the battery voltage 9as measured by ADC) exceeds 8.8V then the battery is a 3-cell one. Otherwise, if the measured voltage is above 5.7V we assume that we have a 2-cell battery. Finally, if the voltage exceeds 3.2V we have a 1-cell battery. If it is lower than 3.2V we have a “special case” – the board is powered by the PIC Kit directly using the MCLR header and bypassing the voltage regulator. In this case the input voltage on the voltage divider will be what the voltage regulator passes back to the input and may be in the range of 2.5 to 2.7V. In the latter case the battery management code will report the charge level at 100% to avoid triggering immediate shutdown due to severe battery discharge.

Finally it is important to point out that the sample-to-input-voltage conversion ratio calculated above represent an ideal case – the voltage regulator provides exactly 3.3V and voltage divider resistors have resistance exactly at nominal values. In real life voltage regulator has some error in the output voltage and resistors typically are within +/-5% of the nominal value. Thus the conversion rate and various critical ADC sample values may need to be adjusted based upon the results of some testing. But that will be the task for later – for now we should have a look at the library responsible for working with ADC.

All the functionality related to managing ADC and reporting on battery status consolidated in theADC Library. As usually, all the definitions specific to the board (like the MCU pin selected for ADC) and to the MCU are consolidated in the ADCProfile.h header. ADCLogal.h header file and corresponding ADCLocal.c code file define and implement variables and constants local to the library. ADC count constants calculated above are also defined in ADCLocal.c code file.

Initialization of the ADC module and establishing cell count of the battery performed in the ADCInit(…) function defined in the ADCInit.c code file. Every configuration setting implemented in ADCInit(…) is well documented by the comments provided in code. Additional information about configuring ADC is available in PIC24EP512GP806 datasheet and Section16 “Analog-to-Digital Converter” of the Family Reference Guide. Contrary to other initialization routines in the libraries that we discussed thus far, ADCInit(…) does not enable ADC module – it is already enabled in the common initialization routine Init() defined in the Init.c code file. The reason for this is specific to PIC MCUs and explained in the comment notes in the body of Init() function.

PIC24EP512GP806 MCU provides 16 buffers to store conversion results. Using ADC configuration parameters they could be split into two banks – lower half and upper half. Using interrupt control configuration for ADC, we may instruct module to raise interrupt not after each sample is converted, but when 8 conversions are performed and one of the buffer banks filled in with new samples. This configuration reduces the number of interrupts and provide MCU more time to read data from the buffer. This configuration allows to give ADC interrupt a relatively low priority of 3 without the danger of missing some of the samples.

_AD1Interrupt() routine defined in the ADCISR.c code file is responsible for processing ADC interrupts. Results of ADC conversion are accumulated in the _ADCSampleSum variable. When 256 samples accumulated (takes about 25.6 milliseconds with ADC running at 10 kHz), the ISR routine calculates their average and update the _ADCValue variable. It then divides the value in the accumulator by 2 and drops the count of samples in the accumulator to 128. Thus the next update of the _ADCValue variable with the new value will take place when 128 new samples are added to the accumulator bringing the count of samples to be averaged again to 256. This mechanism reduces the frequency of battery status update to about 78 Hz as well as implements a form of a simple IIR low-pass filter. The goal of this is to eliminate juddering of the battery charge measurements due to immediate changes in throttle level to allow the board to make decisions based upon the average battery charge level.

ADC.h header and corresponding ADC.c code file provide definition and implementation of the API functions exposed by the library. Battery voltage and charge level are calculated in the function ADCGetBatteryStatus(…) based upon the last available sample from the ADC interrupt routine, provided in the variable _ADCValue. This variable is of type “int” and thus being read and written atomically (in one operation) so we do not have to protect access to this variable with the critical section.

Project 19-ADC provides sample code to demonstrate reading battery parameters and expose them through the logging interface. After programming the board with the firmware produced by this project I ran a test capturing measurements at different levels of battery discharge. At the same time I was capturing the actual battery voltage using digital voltmeter. Results of the test presented in the following table:

Raw	Charge	V (ADC)	V (D.V.)	Error	V/LSB	V (Adj.)	Error (Adj)
2365	0.90	8.19	8.17	0.30%	0.003455	8.17	0.00%
2364	0.90	8.19	8.16	0.38%	0.003452	8.17	0.08%
2216	0.64	7.68	7.66	0.24%	0.003457	7.66	0.06%
2214	0.64	7.67	7.65	0.28%	0.003455	7.65	0.02%
2194	0.60	7.60	7.58	0.29%	0.003455	7.58	0.01%
2192	0.60	7.60	7.57	0.33%	0.003453	7.57	0.03%
					0.003454

As it follows from the analysis of the data, using ideal conversion value of 0.003465 V/LSB calculated based upon the nominal values of resistors and output voltage of the voltage regulator results in a slightly higher value of battery voltage calculated based upon the ADC data then the real voltage of the battery. The error is not very significant – around 0.3%. However, if we recalculate value of the conversion factor based upon the experimental data by averaging it across several test points, the error drops by the order of magnitude to a negligible level. Based upon this new value of the conversion factor, ADC counts corresponding to important voltage levels were also re-calculated and constants in the code updated correspondingly. Recalculated values presented in the following table:

Important Voltages	ADC Counts	Comment
1	289	Useful discharge range per cell
3.2	926	Cutoff discharge voltage per cell
5.7	1,650	Min voltage for 2-cell battery
8.8	2,547	Min voltage for 3-cell battery

This post happened to be rather long, but we managed to review all the aspects of measuring battery voltage and discharge level. In the next post we will review soft- and hard-iron calibration of the HMC5983 magnetometer.

Sharing I2C bus in interrupt-driven IO

For quite some time we have been discussing various libraries supporting I2C communication with multiple devices using interrupt-driven IO. Today we will look at some timing diagrams to confirm that everything works as expected. We will be looking at I2C buss hosting two sensors – MPL3115A2 barometric altimeter and MPU-6050 gyroscope and accelerometer. MPLab-X project 18-I2C-All-Async contains the code that drives the board during this experiment.

The main routine of the project, defined in the code file main.c, initializes all the HW components and supporting libraries, and then in the loop reads all sensors. MPL3115A2 initialized with the OSR=3 resulting in update rate of about 37.7 Hz so a new sample is produced about every 26.5 msec. MPU-6050 initialized with the 0 value for the frequency divider, resulting in update rate of 1 KHz. Both sensors operate in asynchronous mode implementing interrupt-driven IO. To facilitate contention on the I2C bus, the bus is configured to run at 200 KHz (half of the normal “high” speed) so every read/write operation takes twice the normal time.

Using SaleaeLogic (mine is the older version than the ones showcased on the site) logic analyzer I capture interrupt signals from each of the sensors as well as the SCL and SDA lines of the I2C bus so that we may see I2C communication in relationship to the Data Ready interrupt signals from the sensors. Conveniently Saleae Logic has a parser for the I2C bus so we may see the actual commands on the bus (if we zoom into the capture with enough resolution). The following picture provides an overview of about 30 msec.

  

The top line (input line 0) captures Data Ready interrupt line from MPL3115A2 barometric sensor – you see strobes about every 26.5 msec. The next line captures Data Ready strobes from the MPU-6050 – these strobes are 50 usec in width and come, as expected, every millisecond. Line number 2 (third from the top) presents I2C clock line, which has strobes at 200 KHz (bus speed) while there is a communication on the bus. At this resolution we cannot see individual clock strobes – they come as the white area on the chart. Finally, the last line (fourth from the top, line 3) captures I2C data line.

The next figure provides a blow-up of the data exchange session with MPU-6050.

A few microseconds after receiving a Data Ready interrupt from the MPU-6050, a communication session with the sensor is started. The session starts with the I2C Start command (indicated by the green circle on the chart) immediately followed by the address of the sensor and then the address of the register to work with. This is followed by I2C Repeated-Start command (also indicated by the green circle), which is required to change the direction of communication, and the read command with the address of the sensor. Then there is a stream of data bytes from the sensor. Each byte is acknowledged by the I2C ACK command. When the master (our program) reads the last byte from the sensor, it informs sensor that no more bytes is needed by issuing I2C NACK (negative acknowledgement). The session ends with the master issuing I2C Stop command indicated on the chart by the red square. The whole communication session, demarcated by the green marker 1 and 2, takes just under 0.9 milliseconds leaving about 100 microseconds free bus time until the next sample is available from the MPU-6050 sensor.

Now let’s see what will happen when MPL3115A2 signals that the new sample is ready. This situation is captured on the following chart:

At a time point around 14 msec MPU-6050 signals that a new sample is ready (please zoom in into the image). As expected, a few microseconds later a communication session is started with MPU-6050 to read this sample. At around 14.4 millisecond point MPL3115A2 signals that it has a new sample ready. Respective interrupt routine captures this signal and requests I2C library to start communication session with MPL315A2; however the bus is busy, so I2C Library puts this request into the queue.

Around 14.9 millisecond point (highlighted by the green marker 2) the session with MPU-6050 ends and I2C Library practically immediately starts communication session with MPL3115A2, which was earlier put into the queue. Around the 15 millisecond point MPU-6050 reports that the next sample is ready – respective interrupt routine requests communication session now with MPU-6050. However as the bus is busy, this request also goes into the queue – by the way, the queue was empty at this point as the previously queued request for MPL3115A2 was de-queued and is active now.

At around 15.7 millisecond point the communication with MPL3115A2 concludes and previously queued request for MPU-6050 is promoted to active and respective session is started. However at around 16 millisecond point MPU-6050 generates a new sample as indicated by the strobe on the respective interrupt line. The interrupt routine again requests I2C library to start communication session with MPU-6050. However this request is rejected by the I2C library as there is no reason to queue request for the same device with which I2C bus is communicating currently – this sample is being lost!

  

The current communication session with MPU-6050 ends at a point around 16.6 milliseconds; at this time as there is no queued request I2C bus goes quiescent. Then the new sample arrives from the MPU-6050 (around 17 millisecond point) and everything starts again.

In real life the I2C bus would run at 400 KHz, so communication session with MPU-6050 would take half the time, about 450 microseconds; similarly, communication session with MPL3115A2 will also take half the time – about 380 microseconds, so both of them would fit nicely in a 1 millisecond interval between the MPU-6050 data samples and now data samples will be lost due to the contention on the bus.

In the next post we will look at measuring battery voltage level, which is an important feature as we are dealing with the LiPo batteries and should be very careful not to over-discharge them.

MPU-6050 Configuration and Operation

In this post we will discuss MPU library responsible for configuration and enabling synchronous and asynchronous communication with up to two MPU-6050 sensors. If you followed my other posts, you will notice that the structure of this library is very similar to the one supporting another I2C device on my board – namely the MPL library, which we discussed thoroughly in several posts starting with this one. As such I will not go into the minute details of the MPU library - the code is available and you may review it at your leisure. I will just focus on some aspects which are specific to this library – specifically the implementation details allowing this library to manage up to two sensors at the same time. I have two MPU-6050 sensors on the board and did not want practically repeating the code to create two libraries – one for each device.

As was discussed with regards to other libraries, configuration of the MPU library itself is contained in the MPU-Profile.h header. There are several definitions in this header which specify how many sensors are available on the board and should be managed by the library, as well as what are the addresses of these sensors, what I2C modules they are connected to, and what MCU pins are allocated to capture sensors’ interrupts. This header also provides two inline functions, MPUSetIF(…) and MPUSetIE(…) to control interrupt lines associated with each sensor. _MPUInitPinMap() configures interrupt pin(s) based upon other definitions in this header.

To enable the MPU library working with two sensors we need to be able to specify for each function the details of the sensor that it will be working on. Unfortunately the MPLab-X XC-16 compiler supports only “C” language – otherwise we would just create an instance of the library code for each of the sensor from the same class. As we are limited to plain “C”, I had to emulate the object-oriented class-based approach by creating a structure containing all pertinent information for each sensor and explicitly passing a pointer to the instance of this structure to every internal library function emulating the “this” pointer. This structure, MPU_CB, defined in the MPU_Local.h header file.

MPU_CB structure is critical for correct operation of the library, so the pointer(s) to this structure is not exposed to the user code. Instead interface functions of the MPU library accept a numeric parameter identifying which of the sensors (1 or 2) the operation should be directed to. MPU_Local.h header provides definition for the MPUpCB(…) internal function (not exposed outside the library), which converts the sensor number to the pointer to respective MPU_CB structure for subsequent use by the internal library functions.

Two helper functions, _MPU_ConvertReadBuffer(…) and _MPU_ApplyCalibration(…), also defined in the MPU_Local.h header. The former is used by low-level synchronous and asynchronous read routines to convert data stream from the sensor into a set of values representing acceleration, rotation, and temperature measurement. The latter one is used by the read routines exposing measurements to the user to apply rotation and calibration.

As you may recall from the discussion of the board, it contains two MPU-6050 sensors rotated 22.5 degree left and right with respect to the X axis of the board (45 degree rotation respective to each other sensor). This orientation of the sensors guarantee that at minimum three measured values from 2 sensors will contribute to calculation of the pitch and roll of the board with the expectation that this would reduce sensitivity to vibration and improve precision of the measurements. However this orientation of the sensors require that measurements from each sensor need to be “rotated” to match the axes of the board.

The rest of the function is dealing with applying temperature and axis calibration values – we will talk about calibrating MPU-6050 in a subsequent post.

Finally the MPU_Local.h header maps addresses of the control registers of the MPU-6050 to mnemonic names, which are being used by various routines of the library.

Instances of the MPU_CB structure (one for each sensor) are initialized in the MPUInit(…) function and its extensions defined in the MPU_Init.c code file. Some of the fields in MPU_CB structure, like sensor sensitivities, are filled in by the MPURest(…) function based upon the configuration parameters provided by the user. MPURest(…) function invoked either from MPUInit(…) for initial configuration or directly by the user code after initialization to change sensor configuration. MPURest(…) function defined in the MPU_Reset.c code file.

MPU.h header file provides definitions of functions and objects comprising the MPU library API – this is the only header file required by the user programs to access MPU-6050 functionality. This header provides definitions for error codes, enums defining MPU-6050 configuration parameters (like gyro and accelerometer sensitivities), and the MPUData structure through which measurements are communicated to the user program. The rest of the header provides prototypes for the MPU library interface functions, which are primarily defined in MPU_Sync.c, MPU_Async.c, and MPU_Mgmt.c code files.

Code file MPU_Isr.c defines interrupt routines processing Data Ready interrupts from respective sensors and common for both sensors I2C callback function _MPUCallBack(…). If you recall from our discussion of the I2C Library in one of the previous posts, one of the parameters passed to the I2CAsyncStart (…) function is the I2CAsyncRqst structure, which includes the “client parameter” field. This field becomes very useful for the MPU library as it uses it to pass the pointer to the respective MPU_CB control block. When I2C Library invokes the _MPUCallBack(…) function from the I2C interrupt routine it passes this “client parameter” to the callback so that the callback routine knows which sensor it operates upon.

Initialization routine puts MPU-6050 into the “automatic sampling mode” at defined sampling frequency. In this mode every time that new sample is obtained the Data Ready line receives a 50 usec pulse. This architecture significantly simplifies interrupt and I2C callback routines as we do not have to deal with the “missing interrupt” (it will always be repeated at the next sample) and do not have to trigger next sample acquisition – things we had to do while working with MPL-3115A barometric altitude sensor.

This concludes the review of the MPU library - the code has detailed comments and should be rather easy to read following the review above. In the next post we will see how the three libraries we have been discussing lately (MPU library, MPL Library, and I2C Library) work together to allow sharing of the I2C bus between two devices in the interrupt-driven IO.

Timing of interrupt-driven IO

Throughout the blog I have been promoting interrupt-driven IO as a slightly more difficult, but much better performing option. In this post I would try to provide detailed examples allowing to quantify advantages of using interrupt-driven IO and prove that the libraries that we have been discussing for the past several posts actually do work.

In this post we will be working with a very simple project 15-MPLI2C-Async, which is almost identical to the project we used in the post AnalyzingMPL3115A2 Precision Altimeter performance with the exception that in the main routine main.c we put MPLlibrary into asynchronous mode prior to reading samples from the sensor. This project outputs the same data stream through Serial Data Logger with the exception that we do not loop over various values of OSR and we do not limit sampling to 5 second interval – sampling continues as long as power is applied to the board.

However instead of looking at the output of the board (as we did in AnalyzingMPL3115A2 Precision Altimeter), I will connect logic analyzer to the test pads on the board to capture the actual I2C traffic on the bus, the status of DREADY line, and the signal on the blinker LED.

The first logic analyzer screen capture allows us to measure time interval between the DREADY line going high and the I2C module sending the START command to the sensor. As you may recall, during this time the DREADY going high results in external interrupt, which is captured by the MPL_Interrupt() function (please see MLP_ISR.c code file). MPL_Interrupt() function subscribes to I2Clibrary to perform I2C data session with the sensor. As in this project there is only one sensor on the bus, subscription is successful and I2C Library initiates I2C data exchange with the sensor by setting the START condition on the bus, at which time control is returned to the main thread. As you may see from the screenshot, all of this takes 5.5 microseconds.

From now on all I2C bus control will take place in the _MPLCallBack(…) function, which will be called by the I2C ISR (as discussed in the earlier post) in response to I2C interrupts generated by the hardware I2C module. The whole I2C data session, which includes reading data from the sensor and triggering next sample acquisition by setting OST flag, captured in the following screenshot.

My logic analyzer provides I2C data parser, which interprets I2C bus signals and allows seeing the actual data moved across the bus as well as interprets bus conditions. The green circles on the I2C data capture represent START or REPEATED-START conditions and the red square represent the STOP condition on the bus. Measuring time from the START to STOP reveals that the whole packet takes about 467 microseconds. By the way, on this screenshot we may also see that the DREADY goes low after the sample is read.

Now it would be really interesting to find out how much time was actually spent in the ISR routine and the _MPLCallBack(…) function. For that I temporarily modified the I2C ISR routine so that it will bring up the ISR line at the entry to the interrupt routine and bring it down after the return from the _MPLCallBack(…) function right before performing the return. Results are presented on the following screenshot:

The narrow spikes at the bottom of the screen represent the time spent in the interrupt routine. They are too narrow to be visible as more than just a line at the scale factor sufficient to represent the whole packet. However, at a higher resolution I was able measure the duration of each of these “spikes” – they range from 0.6 microseconds to 0.825 microseconds except for the next to last one, which is about 3 microseconds. This one corresponds to the State 18 in the _MPLCallBack(…) State Machine, when the measurement is constructed from the bytes read from the sensor and stored in the library. The very last spike with the duration of 0.3 microsecond is the interrupt after the final STOP, which is processed completely in the I2C ISR without the call to _MPLCallBack(…).

Now let us do some calculations. The total time from the moment DREADY goes high to completion of the I2C operation is 471.3 microseconds. Out of this time the MPL and I2C ISR routines including the calls to _MPLCallBack(…) take (5.5 + 25*0.8 + 3) = 28.5 microseconds, or about 6% of the IO time; the rest of the time – 442.8 microseconds (94% of the IO time) is returned to the main execution thread!

In this particular example project the main thread does nothing but waits for the next available sample. However, in a real control loop the main thread could be doing attitude calculation or calculating control input for individual motors, or something else and for the cost of being briefly interrupted for a few microseconds here and there it will have a new sensor data ready for it! With more sensors that need to be queried for data, the benefits become even more pronounced!

Considering that the speed of the control loop and the frequency at which motors’ control can be updated are critical for stable flight, the interrupt-driven IO is the way to go! On my previous board I managed to keep the duration of control loop to about 5 milliseconds resulting in motor control update frequency of about 200 Hz. With the new board utilizing MCU running at 64 MHz (versus 40 MHz on the previous one) I expect to even further reduce the duration of the control loop eventually getting closer to the maximum frequency of motor control updates of about 400 Hz supported by my ESC.

In the next post we will look at MPU Library responsible for configuration and operation of two Invensense MPU-6050 sensors installed on the board.

Asynchronous (interrupt-driven) communication with MPL3115A2

Now that we are quite familiar with MPL3115A2 sensor, it is time to look at how we may read samples from this sensor without wasting time on polling. The implementation of asynchronous sample acquisition is split between the rather generic features implemented in the I2Clibrary, which we discussed earlier, and specific to this sensor operations implemented in the MPLlibrary.  Features of the MPLlibrary responsible for asynchronous communication implemented in two code files MPL_Async.c and MLP_ISR.c. The former implements related user-accessible and some internal helper functions, while the latter implements the interrupt service routine (ISR) to process DREADY (data ready) interrupt from the sensor and the callback function invoked by I2Clibrary  in response to I2C interrupts.

Let us first review functions implemented in MPL_Async.c code file. The first one is the MPLAsyncStart() function. It first checks for library initialization and then checks for Asynchronous Mode (_MPL_Async) flag. If it is already set, the function returns success code and does nothing else. Otherwise it sets the Asynchronous Mode flag in the library, clears external interrupt request, if any, and then enables external interrupt linked (in MPLInit() function through the call to _MPLInitPinMap() defined in MPL_Profile.h) to the sensor DREADY line.

One special point need to be noted here – as the sensor operates in the OST (one-shot) mode, if the last read was unsuccessful the one-shot was not triggered, so DREADY line may never go high triggering the interrupt. We deal with this possibility in the Read functions. However to facilitate this we need to capture the time of last “successful” read so that we may compare the interval since then with the maximum time to acquire the sample _MPL_MaxInt, which was calculated in MPLReset(…) function based upon the specified value of OSR during initialization of the sensor. As a “safe bet” in the MPLAsyncStart()  function we set the time of the last “successful” read to the current timestamp to guarantee that at most after _MPL_MaxInt, time the interrupt will be forcefully triggered in one of the Read functions. We will see shortly how that is being implemented.

On the other hand, if the one-shot was successfully triggered in preceding read operation, the DREADY line may already be high, but the respective interrupt request was ignored as the Asynchronous Mode was not enabled. To address this contingency, MPLAsyncStart()  function checks for the line status to be high and, if so, triggers “missed” interrupt forcefully by setting interrupt flag MPL_IF (which is link to the selected interrupt port in MPL_Profile.h). This concludes transition to Asynchronous Mode.

If we need to terminate Asynchronous Mode, for example – to reset sensor with a different value of OSR, we need to call MPLAsyncStop() function. This function resets all the flags associated with Asynchronous Mode and disables external interrupt due to DREADY line going high.

MPLlibrary provides two functions in MPL_Async.c file to read samples from MPL3115A2 in Asynchronous Mode. The first one, MPLAsyncReadIfReady(…), updates the MPLData structure specified as a parameter only if the new sample is available; otherwise the structure remain unchanged and respective return code (MPL_NRDY -  data sample is not ready) provided to the caller. If the new sample is not ready, the function compares the current time-stamp with the time-stamp of last successful read operation – if the time since last read exceeds maximum sample acquisition time, a DREADY interrupt is triggered to restore the cycle. We will get into more details of this when we review the ISR routines shortly. As the call to this function is non-blocking, this function is being used in the quad control loop to avoid slowing down the loop with the wait for altitude sample.

The second function, MPLAsyncReadWhenReady(…), is very similar to the former one with the exception that it waits until the next sample is available. I usually use this function in some test projects; it also can be used in situations when obtaining the next altitude sample prior to proceeding further may be important for the application.

To actually read the sample data from the library both MPLAsyncReadIfReady(…) and MPLAsyncReadWhenReady(…) functions call library internal function _MPLAsyncRead(…). This function establishes a Critical Section (to avoid potential corruption of data by the interrupt routine), within which retrieves the number of samples accumulated _MPL_Ready by the library, the sum of accumulated samples _MPL_Data, and the time-stamp of the last sample _MPL_DataTS. When these values are retrieved and stored in local variables, both the sample counter _MPL_Ready and the sum of sample _MPL_Data values are set to zero, which indicates that samples asynchronously retrieved by the library are consumed. This concludes the Critical Section.

The altitude values then converted from long integer in sensor units to a floating-point value representing altitude in meters. If more than one sample summed up in _MPL_Data, as indicated by _MPL_Ready > 1, the resulting value of altitude averaged over collected samples. Finally the value of altitude is adjusted by the altitude of the “ground” (if it was set earlier) to convert it from the altitude over the sea level to altitude over ground.

So what is the difference between this function and synchronous read function MPLReadSample(…) that we discussed in the earlierpost? The synchronous read function MPLReadSample(…) ties up the main execution thread for the whole duration of the read operation. Contrary to that, the asynchronous read function MPLAsyncReadWhenReady(…) after reading current sample immediately triggers acquisition of the next sample asynchronously and then returns control to the main thread, which may do some processing of the sample while the next one is being acquired asynchronously. Thus at the next call to MPLAsyncReadWhenReady(…) if the next sample already acquired there would not be any wait or, if sample is not yet ready, the wait will be reduced by the time the main execution thread spent processing previous sample.

Now we are ready to look at the interrupt routines implemented in MLP_ISR.c code file. The first one, MPL_Interrupt(), is the true interrupt routine linked to the DREADY line. When the interrupt is enabled (in MPLAsyncStart() routine) and the DREADY line goes from low to high MCU interrupts the main execution thread and, subject to interrupt priority, transfers control to MPL_Interrupt() function. Alternatively interrupt could be forced from the main execution thread by setting interrupt flag MPL_IF in code, which, under some circumstances discussed above, could be done in MPLAsyncStart() or Asynchronous Mode read routines.

MPL_Interrupt() function first and foremost resets the interrupt flag MPL_IF – otherwise upon return it will be called again by MCU indefinitely. Second, it captures the status of the DREADY line to distinguish between natural and forced interrupts. Then it prepares the I2CAsyncRqst structure and attempts to subscribe for I2C processing through the call to I2CAsyncStart(…). If subscription is successful – request could be initiated right away or put in a queue by I2CAsyncStart(…), MPL_Interrupt() function resets the state of the State Machine implemented in the _MPLCallBack(…) function and returns. If I2C subscription is successful, I2C interrupts will be routed by the I2C ISR (as discussed in the earlier post) to the _MPLCallBack(…) function.

The core of communication is implemented in the _MPLCallBack(…) function – let’s look at how it works. Basically the whole _MPLCallBack(…) function is a big SWITCH statement with each case corresponding to a single state in the communication process, starting with the initial state 0 corresponding to the first interrupt generated by the I2C module after the START condition. Each CASE block performs a simple operation like setting a flag or reading/writing one byte from/to I2C read/write registers following the sequencing protocol required to construct a complete message for MPL3115A2. At the end of each CASE block the state is usually advanced to the next one with a few exceptions.

State 5 may advance state either to the state 6 or to the state 7 depending on the counter of the bytes read from the sensor. We need to read 5 bytes from the sensor to reset the DREADY line – 3 bytes representing the altitude and 2 bytes representing the temperature; we ignore the temperature measurement, but we still have to read it. Thus, at State 5 if there are more bytes to read we advance to State 6; if all 5 bytes are read we advance to State 7.

State 6 is another exception – after preparing I2C bus for read of the next byte it moves the state back to State 5 to read the byte and make decision on further protocol steps based upon the number of bytes read from the sensor in this session.

Finally, at State 18 the message protocol is completed and there is no need to advance state – State 18 is the final one. When _MPLCallBack(…) function reaches State 18 it terminates current I2C operation by calling I2CAsyncStop(…). If the status of the DREADY line was high as captured in variable _MPL_PortLvl by the MPL_Interrupt() function, then the value read from the sensor represent the new sample. Bytes read from the sensor are interpreted according to the specified data format and converted into a long integer representing measurement of altitude in sensor units (LSB corresponds to 6.25 centimeters).

If this is the first sample in a series of samples that were not yet consumed by the client, the just calculated value of the altitude (in sensor units) is stored in the library variable _MPL_Data. If this is not the first sample (_MPL_Ready > 0), the new value is added to the value stored in _MPL_Data. The latter case takes place when the frequency of reading asynchronously collected samples is less than the frequency at which sensor produces samples.

There is a special case in this logic – if the number of samples summed up in the _MPL_Data variable exceeds 512, the series is ignored – the new value of altitude is stored in the _MPL_Data variable and _MPL_Ready is set to one. _MPLCallBack(…) function does this to avoid overflow of the long integer _MPL_Data with the sum of multiple samples. However, this is almost an impossible situation in a control loop – even if we set OSR to 0, the shortest sample acquisition time would be 6 milliseconds, so to accumulate 512 samples would take over 3 seconds, which is a too large interval for any reasonable control loop.

This concludes discussion of the components responsible for asynchronous acquisition of altitude samples from MPL3115A2. In the next post we will look at a sample project implementing asynchronous read of altitude samples and will try to evaluate some timing characteristics.