Accelerometer and Magnetometer Calibration: Part 3 - Implementation in Matlab

Matlab Implementation

In this part I will be providing the source code for an implementation of the Gauss-Newton solver in Matlab. As you will see, it is a quite simple algorithm that easily can be turned into C(++)/C#/Java without much hassle. However I will not (for starters) provide this algorithm in any other language because I don't have the time, but in the future I plan on having a C, C++ and C# version that uses matrix simplifications to minimize memory consumption for simple integration into any project.

Structure
The structure is composed of three main scripts (the solver) and two scripts as an example. The code for all scripts are at the end of this post, I only explain their function here:

• nonlin_solve.m - The main nonlinear solver that implements the Gauss-Newton solver.
• yHAT.m - Creates $\hat{y}(\boldsymbol{\theta})$ for nonlin_solve.m.
• gradH.m - Creates  $\mathbf{H}(\boldsymbol{\theta})$ for nonlin_solve.m.
• test_solver.m - An example of an accelerometer calibration.
• create_meas.m - Creates a 6-point measurement set for test_ solver.m with gain plus bias errors added.

The script is extremely easy to use, the solver takes nine inputs as: nonlin_solve(x, y, z, bx, by, bz, gx, gy, gz), where x, y and z are the measurement vectors and bx, by, bz, gx, gy and gz are start guesses. To find a good starting guess it is easiest to look at the datasheet, for example an accelerometer might have the value 1024 = 1g, then the starting guess for the gain is 1024. For the bias I always start at zero, you can see my starting guess in the test_solver.m script. If you are unsure, it won't be a problem, the solver will still converge even with a substantial error in the starting guess. For example, try changing the starting guess from 1024 to 1 in test_solver.m, that is three orders of magnitude (!!!) error and it will still converge however it will need more iterations to do so.

Example run of the test_solver.m:

Loop aborted at n = 6 of 100, requested precision reached!

-: Sensor Bias :-
X-axis: 125.6636
  Error: 0.66356 (0.53%) 

Y-axis: -249.7382
  Error: 0.26185 (0.1%) 

Z-axis: 99.9163
  Error: -0.083738 (0.084%) 

MSE: 0.17196 


-: Sensor Gain :-
X-axis: 1079.4757
  Error: -0.52428 (0.049%) 

Y-axis: 1149.8909
  Error: -0.10913 (0.0095%) 

Z-axis: 920.6441
  Error: 0.6441 (0.07%) 

MSE: 0.23388

From this it is quite evident that the solver works well. But to get a feel for it you should play with it yourself! Please give it a try and come with some comments, I am always looking for improvements to this script.
   This is it for this series! I hope you have learned something about estimating parameters and how to calibrate sensors. It has been an interesting problem to try and solve. And thank you for reading!

Matlab Code

test_solver.m

clear
clc

% Set bias and gains for the solver to find
theta = [125; % mx
         -250; % my
         100; % mz
         1080; % gx
         1150; % gy
         920]; % gz

% Create measurements based on the theta
% Std. dev. of 5 and 50 measurements of each axis
[x, y, z] = create_meas(theta(1), theta(2), theta(3), ...
                           theta(4), theta(5), theta(6), 5, 50);
                       
% For nice printing
format shortG

% Run the measurements through the solver to find theta_hat                      
theta_p = nonlin_solve(x, y, z, 0, 0, 0, 1024, 1024, 1024);

% Calculate the errors
err = (theta_p - theta);
MSE_m = sum(err(1:3).^2)/3;
MSE_g = sum(err(4:6).^2)/3;

% Some nice write-out
fprintf(['-: Sensor Bias :-\n'])
fprintf(['X-axis: ', num2str(theta_p(1)), '\n\t Error: ', ...
    num2str(err(1)),' (', num2str(abs(err(1)*100/theta(1)), 2),'%%) \n'])
fprintf(['\nY-axis: ', num2str(theta_p(2)), '\n\t Error: ', ...
    num2str(err(2)),' (', num2str(abs(err(2)*100/theta(2)), 2),'%%) \n'])
fprintf(['\nZ-axis: ', num2str(theta_p(3)), '\n\t Error: ', ...
    num2str(err(3)),' (', num2str(abs(err(3)*100/theta(3)), 2),'%%) \n'])
fprintf(['\nMSE: ', num2str((MSE_m)),' \n'])



fprintf(['\n\n-: Sensor Gain :-\n'])
fprintf(['X-axis: ', num2str(theta_p(4)), '\n\t Error: ', ...
    num2str(err(4)),' (', num2str(abs(err(4)*100/theta(4)), 2),'%%) \n'])
fprintf(['\nY-axis: ', num2str(theta_p(5)), '\n\t Error: ', ...
    num2str(err(5)),' (', num2str(abs(err(5)*100/theta(5)), 2),'%%) \n'])
fprintf(['\nZ-axis: ', num2str(theta_p(6)), '\n\t Error: ', ...
    num2str(err(6)),' (', num2str(abs(err(6)*100/theta(6)), 2),'%%) \n'])
fprintf(['\nMSE: ', num2str((MSE_g)),' \n'])

create_meas.m

function [ax, ay, az] = create_meas(mx, my, mz, gx, gy, gz, stddev, meas)
    % Create measurements on each side of the sensor:
    % x, y and z axises (six measurement positions).
    A = [ ones(meas,1)   zeros(meas,1)  zeros(meas,1);
         -ones(meas,1)   zeros(meas,1)  zeros(meas,1);
          zeros(meas,1)  ones(meas,1)   zeros(meas,1);
          zeros(meas,1) -ones(meas,1)   zeros(meas,1);
          zeros(meas,1)  zeros(meas,1)  ones(meas,1)
          zeros(meas,1)  zeros(meas,1) -ones(meas,1)];
    
    % Multiply with the gain to get "sensor accurate data"
    A(:,1) = A(:,1)*gx;
    A(:,2) = A(:,2)*gy;
    A(:,3) = A(:,3)*gz;
    
    % Add the biases of each axis
    A(:,1) = A(:,1) + mx;
    A(:,2) = A(:,2) + my;
    A(:,3) = A(:,3) + mz;
    
    s = size(A);
    
    R = normrnd(0, stddev, s(1), s(2));
    A = A + R;
    
    ax = round(A(:,1));
    ay = round(A(:,2));
    az = round(A(:,3));
end

nonlin_solve.m

function theta = nonlin_solve(x, y, z, bx, by, bz, gx, gy, gz)
    % Formula for Gauss-Newton solver:
    % theta(k+1) = theta(k) + inv(H'*H)*H'*[y - yHAT(theta_k)]
    
    theta = [bx;
             by;
             bz;
             gx;
             gy;
             gz];
    
         
    for i = 1:100
        H = gradH(x, y, z, theta(1), theta(2), theta(3), ...
                  theta(4), theta(5), theta(6));

        yH = yHAT(x, y, z, theta(1), theta(2), theta(3), ...
                  theta(4), theta(5), theta(6));
              
        hTh = (H'*H);
        step = hTh\H'*(1 - yH);
        theta = theta + step;
        
        if max(abs(step)) < 1e-7
            fprintf(['Loop aborted at n = ', num2str(i), ...
                     ' of 100, requested precision reached!\n\n'])
            break
        end
    end
end

yHAT.m

function a = yHAT(ax, ay, az, mx, my, mz, gx, gy, gz)
    % Calculates the acceleration
    a = (ax - mx).^2./gx.^2 + (ay - my).^2./gy.^2 + (az - mz).^2./gz.^2;
end

gradH.m

function H = gradH(ax, ay, az, mx, my, mz, gx, gy, gz)
    % Derivative of a = (ax - mx)^2/gx^2 + (ay - my)^2/gy^2 + (az - mz)^2/gz^2
    % = d/dtheta (ax - mx)^2/gx^2 + (ay - my)^2/gy^2 + (az - mz)^2/gz^2 = 
    % = [ -(2*ax - 2*mx)/gx^2, -(2*ay - 2*my)/gy^2, -(2*az - 2*mz)/gz^2,
    % -(2*(ax - mx)^2)/gx^3, -(2*(ay - my)^2)/gy^3, -(2*(az - mz)^2)/gz^3]
    
    % H = [da/dmx da/dmy da/dmz da/dgx da/dgy da/dgz]
    
    H = -2.*[(ax - mx)./gx.^2,     (ay - my)./gy.^2,    ...
             (az - mz)./gz.^2,     ((ax - mx).^2)./gx.^3,  ...
             ((ay - my).^2)./gy.^3,  ((az - mz).^2)./gz.^3];
end

Accelerometer and Magnetometer Calibration: Part 2 - Least-Square Solving Methods

Linear versus Nonlinear

When trying to solve an over-defined (more data-points than there is unknowns) it is a popular approach to use the Least-Squares method. This works by finding the best fit of a model that minimizes the mean square error of each measurement. What is important to note is that this is for linear problems and usually performs bad when the problem is nonlinear, what is nonlinear is when you have a product/division of parameters or when the variable is not a polynomial. Looking directly at the model equation,

\begin{equation*}\left (\frac{x - b_x}{g_x} \right )^2 + \left (\frac{y - b_y}{g_y} \right )^2 + \left (\frac{z - b_z}{g_z} \right )^2 = 1, \end{equation*}

it is difficult to determine if the system is linear or nonlinear. But if we expand the squares and see what we end up with it's much easier to determine:

\begin{equation*} \frac{x^2}{g_x^2} + \frac{y^2}{g_y^2} + \frac{z^2}{g_z^2} - \frac{2b_xx}{g_x^2} - \frac{2b_yy}{g_y^2} - \frac{2b_zz}{g_z^2} + \frac{b_x^2}{g_x^2} + \frac{b_y^2}{g_y^2} + \frac{b_z^2}{g_z^2} = 1 \end{equation*}

Now we can see that in six different places we have parameter divided by parameter, hence this is a nonlinear case. If you don't want the gain estimation then it would be a simple linear case, but now we want to know all errors in the sensors hence we will need to use a nonlinear case.

    There is a method to do least-squares on ellipses called "Direct Least Square Fitting of Ellipses", however I will not go into how this is done.

Gauss-Newton Linearization Method

The method of choice to solve this nonlinear problem is going to be the Gauss-Newton linearization method, this method works like Least-Squares but uses the derivative of the function in regards to the parameters to create an iterative successive approximation routine.

   The first thing you do when using this method is to create a column vector, usually denoted $\boldsymbol{\theta}$, that contains the parameters we want to know:

\begin{equation*}\boldsymbol{\theta} = \begin{bmatrix}b_x & b_y & b_z & g_x & g_y & g_z \end{bmatrix}^T \end{equation*}

On top of this we need the function we want to estimate, denoted $\hat{y}(\boldsymbol{\theta})$ where $\mathbf{m} = \begin{bmatrix} x & y & z \end{bmatrix}$ is the measurement vector. Note that the = 1 from the original equation has disappeared, this comes from the fact that we want it to be 1 and now we are calculating it based on the estimated parameters.

\begin{equation*}\hat{y}(\boldsymbol{\theta}) = \left (\frac{x - b_x}{g_x} \right )^2 + \left (\frac{y - b_y}{g_y} \right )^2 + \left (\frac{z - b_z}{g_z} \right )^2 \end{equation*}

As stated before, this method uses the derivative of the function with regard to the parameters. In effect this method works a little like the gradient decent method, you need the gradient of the function in regard to the parameters:

\begin{equation*}
\mathbf{H}(\boldsymbol{\theta}) = \frac{\partial \hat{y}(\boldsymbol{\theta})}{\partial \boldsymbol{\theta}} = -2 \begin{bmatrix}
\frac{x - b_x}{g_x^2} \\
\frac{y - b_y}{g_y^2} \\
\frac{z - b_z}{g_z^2} \\
\frac{(x - b_x)^2}{g_x^3} \\
\frac{(y - b_y)^2}{g_y^3} \\
\frac{(z - b_z)^2}{g_z^3}
\end{bmatrix}^T
\end{equation*}

The gradient will be a $m \times 6$ matrix where m is the number of measurement points. For each measurement set the gradient is calculated and you end up with as many derivatives as measurements. This will make it possible to see if we are coming closer to the real solution or if we are diverging from it. 

   Now we have the prerequisites to the Gauss-Newton linearization method! The iterative solution is calculated with the following equation (how this equation is derived is not something I will go through here):

\begin{equation} \boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k + \left ( \mathbf{H}^T (\boldsymbol{\theta}_k) \mathbf{H}(\boldsymbol{\theta}_k) \right )^{-1} \mathbf{H}^T (\boldsymbol{\theta}_k) \left ( \mathbf{y} - \mathbf{\hat{y}}(\boldsymbol{\theta}_k) \right ) \end{equation} Where $\mathbf{y}$ is what we want our function to be, which in our case is $\mathbf{y} = 1$ and note that $\mathbf{\hat{y}}$ is a vector - not a scalar. Now it's just to follow the following steps:

1. Set the iteration number $k = 0$
2. Make an initial guess, $ \boldsymbol{\theta}_0 $ of the parameter vector.
3. Compute the gradient $ \mathbf{H}(\boldsymbol{\theta}_k) $ and the modeled response by $ \hat{y}(\boldsymbol{\theta}_k) $.
4. Update the parameter estimates according to Eq. (1).
5. Check your stop criteria $\left \| \boldsymbol{\theta}_k - \boldsymbol{\theta}_{k-1} \right \| < \varepsilon$, where $\varepsilon$ is a small number, I choose $\varepsilon = 10^{-7}$.
6. If any of the stop criteria is met, stop the algorithm, otherwise increase the iteration counter and start from point 3.

How many measurement points are needed?

One of the big questions are how many data points are needed for this to work? The answer requires some testing, but I have found that as low as 5 measurements per axis and direction is about where the minimum is (total of 30 measurements). To get the best data set it is recommended to do the so-called "six-point measurement" which means that the system will be put in 6 known positions corresponding to the faces of a cube. With that said, I use 50 (total of 300) measurements and with that I get within 0.2 % of the real value in simulations. Which value you choose is up to you.

   How to do this with the magnetometer is a bit trickier. The same method can be used, which works, but is sub optimal. I have tried holding the system facing North and slowly rotating it along the E-W axis and after that rotating it around the N-S axis. This seems to work as well but some more testing is required. However even with the six-point measurement magnetometer calibration is within 0.5%, which is more than good enough!

Accelerometer and Magnetometer Calibration: Part 1 - Theory

Introduction

In this small series of posts I'll be going into the art of calibrating sensors and mainly the calibration of 3-axis accelerometers and 3-axis magnetometers. The question that arises might be why there even is a need of calibration and the best answer is that all sensors have manufacturing differences, differences in the PCB design, components close by creating distortions and so on, and to counter these effects calibration is mainly used. An example of how an ideal measurements of accelerometer and magnetometer looks versus measurements corrupted with gain errors and bias errors is shown bellow in Figure 1. The errors have been  some what over emphasized. This is the result you'd expect if, as an example, you put a magnetometer on a table and rotated it one turn while keeping it flat to the table.

Figure 1: Ideal sensor measurements versus corrupted measurements. Crosses denote measurement points and the red circle is the center of the ellipsoid.

This really rises three questions: 1) how to model the ideal case, 2) which errors can occur and 3) how to model the case with errors. All these problem have quite simple solutions however by just looking at the data it can be hard to identify them.
     I will warn the average person, there will be a lot of math and I will not explain the background theories. I will assume that you know about vector and matrix algebra, derivatives and how to calculate with ellipses.

Modeling

1) How to model the ideal case?
Before we start trying to make a model we need to figure out what we are measuring. Both the magnetic field and the gravitational field are, in each point on Earth, a vector that has a direction and a magnitude and what we are measuring is the x, y and z components of these two vectors respectively. In the ideal case we assume that the magnitude is constant for all directions, there is no shifting (bias) and with this we can create an ideal measurement model. A vector with constant magnitude is most simply modeled as a sphere with the base of the vector in its center and the tip being able to touch any point, this gives the following equation of a sphere with the radius R:

\begin{equation*} x^2 + y^2 + z^2 = R^2 \end{equation*}  This is how we would want our measurements to look, but unfortunately this is usually not the case as we will see in the next part.

2) Which errors can occur?

Before we start looking at different errors we have to make some assumptions. We assume that the errors are uncorrelated, meaning that for example errors in the x-axis does not effect the errors in the y- or z-axis and that the noise is Gaussian (White). With this we can start identifying the different errors from the right picture in Figure 1. In this figure we can see that the center of the ellipse has been shifted, we will call this bias errors, and that the circle is not round any more but elliptical, we will call this gain errors, plus on top of this there is some random noise.

3) How to model the errors?
To model this let start with the radius of the circle. If you remember the old trigonometry classes, the way to represent a sphere with gains on x, y and z is called an ellipsoid and the general equation for an ellipse with its center in the origin have the equation:

\begin{equation*} \left (\frac{x}{a} \right )^2 + \left (\frac{y}{b} \right )^2 + \left (\frac{z}{c} \right )^2 = 1 \end{equation*}

Here I have included the $R^2$ in the a, b and c constants (more information on ellipsoids) and with this equation we can represent gains in each of the axises in the sensor.

     Now, lets start looking at the offset of the center of the ellipsoid. If we look back to our old equation solving classes then we know that to shift a function $f(x)$ a distance $b$ to the right we can write that as $f(x-b)$. Remember that we assumed the error to be uncorrelated and knowing this we can rewrite the ellipsoid equation to incorporate bias. In this equation I have renamed the gains from $a, b, c$ to $g_x, g_y, g_z$ respectively and I call the biases $b_x, b_y, b_z$:

\begin{equation*}\left (\frac{x - b_x}{g_x} \right )^2 + \left (\frac{y - b_y}{g_y} \right )^2 + \left (\frac{z - b_z}{g_z} \right )^2 = 1 \end{equation*}

This is the complete measurement model of both the accelerometer and the magnetometer, where $x, y, z$ are the measurements corrupted by gain ($g_x, g_y, g_z$) and bias ($b_x, b_y, b_x$) errors respectively.
    Using this equation I will, in the next part, show how to use Least-Square solving methods for finding the gains ($g_x, g_y, g_z$) and biases ($b_x, b_y, b_x$) respectively plus how to handle the random noise on top of the model.

LaTeX support added to the blog!

After a long time of  nothing I have finished my Master Thesis! With it finished, I have finally added LaTeX Math support to the blog and this is a post to try it out! I have a few math-heavy posts coming up and to do it all with images just isn't doable. I will have a few Nonlinear Kalman Filter implementations and how to attack the problem of GPS navigation when there aren't enough bits in a floating point number. But this is at a later time!

Trying some inline math $ \theta(k+1) = \theta(k) + \Delta t \cdot \dot{\theta}(k) $ explaining an simple integrator...

And some centered equations:

\begin{equation}
x = a + b
\end{equation}
\begin{equation*}
x = \begin{bmatrix}
a & b \\
c & d
\end{bmatrix}
\end{equation*}
Seems to be working quite good! :D

ECC -13 acceptance and new article to CDC -13

I just realized that have not written about the newest development in my articles. My article, "Full Quaternion Based  Attitude Control for a Quadrotor", was accepted to ECC 2013 in Zurich, Switzerland! So this summer I will go there and present my results to the people there. This is really huge for me, I have still not finished my master thesis and I was able to publish in one of the biggest conferences. I'm really looking forward to this and I hope the response will be very good.

     One of the critiques on the article was the lack of experimental results but a new article has been submitted to Conference on Decision and Control (CDC) -13 with the experimental results. I can't reveal much because of confidentiality but I can say this much; the controller works perfectly and very close to the simulation results! So now it's just to wait and see if it will be accepted. Wish me luck!

What's up and new boards

There has been some time since the last update but here are some updates. Sadly a lot of time has gone to my master thesis so not as much time as I have wanted has gone to the KFly Project but that does not mean it has been standing still.

     I'm currently writing a post on the difference between different Kalman Filters such as the normal linear one, the Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF) and finally the Cubature Kalman Filter (CKF). I will also go into  how you tune these filters, what a "square-root" implementation is and what an adaptive filter is and how it works. I'm also thinking of adding how a Square-Root Adaptive Cubature Kalman Filter (SR-ACKF) implementation I'm working on works and it's benefits over a few other implementations.

     Some hardware changes has also been done and three new boards have been built. A friend of mine has been porting TauLabs (OpenPilot) for my board and yesterday we tried some code. We ended up bricking two of the boards so the JTAG programmer could not find them. In a small act of desperation we took an STM32F4Discovery and used the onboard ST-Link programmer via SWD and we were able to flash the boards again!

     The hardware changes we made because of a poor choice of ESD protection, the chip chosen was so small it was virtually impossible to get soldered properly (0.25  mm pitch UDF chip). Because of this I changed it to an SO-70 chip and now the board is ESD safe and working!

Three new KFly boards assembled and tested!

(On a side note, this was my first post written on a tablet. Yay!)

Another test video

Here is another video taken during testing. Not much to say really. It has a loop where it goes from 0, 60, -60, 0, 30, -30, 0 degrees in 5 second intervals. As seen in the video it is rock solid! Unfortunately I don't have a video when we are striking it with disturbances but, suffice to say, it handles it without any problems. Some offset can be seen after a few movements, this comes from one engine being a bit broken and thus creating a lot of vibrations. This causes some offset in the filter.

As soon as I know if my article was accepted to ECC -13 I will post more theory on the controller with simulation and experimental results. So stay tuned for more info!

First controlled test of the Quaternion controller!

Today was a big day: The first real test of the Quaternion based controller. It was some work to get it to work as expected, as an example a program to calibrate the speed controllers was needed so some time was put into that.

     After guessing the gains of the controller I started it up and it worked! As shown in the video (this is the first actual start up, none before this one) it really works! It is programmed to go from "flat" to about 60 degrees angle and it does it really well. There is a constant offset because of one of the engines being some what broken but it still works! Now it time to start tuning the controller!

Worst bug ever! (in ST's USB CDC driver)

Today a lot has been done for the preparations to do the first controlled "flight test", "flight test" as in a Quadrotor strapped to a frame and trying the rate and attitude controller. However during the development of the controller I noticed a very strange thing happening (it actually started when I sent a lot of data over the USB VCP (Virtual Com Port), after some time the board just stopped doing anything. But it happened very sporadically, from within minutes to hours. But today I finally caught it with he debugger (I've been trying for two weeks)!

     The program got stuck in a function called " DCD_WriteEmptyTxFifo" - one of the USB interrupt handlers and for some reason it got in an infinite loop here. I can directly tell you all that I'm no expert in USB, I have just adapted the ST drivers to fit my purpose, so I started to wonder how I should fix this but Google is you friend. After some research I found this post where one had solved the problem! Apparently ST had a bug in the driver where they had forgotten to clear the interrupt and this was causing the infinite interrupt loop. After adding those lines of code my system stopped to hang randomly!

Now that's one good thorn out of my paw! But I must say I expected ST's drivers to work, not to have a bug like this.

PC Visualization Test - Rotating PCB

Not it's getting really close to start wit the "real" functions in the system. I'm currently testing the Madgwick AHRS algorithm, with success however I realized that his quaternion derivative is at fault. Firstly he uses the left hand derivative (why? fixed by changing the sign of the derivative) and secondly he uses the equation for when the angular rate is measured in the fixed frame, not in the PCB frame. This creates strange effects when rotating a lot but I changed this and the result can be seen in the following videos comparing Madgwick rotation and my fixed rotation.

Madgwick rotation:

Correct rotation:

And when using the correct derivative the following C# program was created to visualize the result using OpenTK:

Now it's really starting to come together!

Updates on the Software - Waging war with interrupt priorities

Much has been done now and after a few long days I have finally got interrupt driven measurements from the sensors to work. Because I want the abstract level of the code to be high there was a lot to fix to get it to work and be "invisible" to the user, but now it works very well.

This is how it is done/implemented:

1. First the sensor sends out an "Data Ready" signal, however this is not on the I2C-bus (MPU6050 or HMC5883L, the barometer does not have an data ready pin).
2. The interrupt is generated and an Binary Semaphore (in FreeRTOS) is set so the task that reads can be unblocked.
3. The task will try to take the I2C-bus by checking the I2C-mutex, if the bus is occupied it will wait, else it will tell the I2C to get the data requested using interrupt driven transfers. When it is done the I2C will execute an Callback-function that will set the I2C-mutex as free again and tell the OS to start using the data.
4. Now the task will wait for the next "Data Ready" interrupt.
   

This guarantees that there will be no collisions on the buss and it is easy to add mote functionality depending on where you want things to happen. 

    The biggest problem when trying to do this was when FreeRTOS always got stuck on a function called "vListInsert", this was tracked down to be a problem in the priority of my interrupts. The interrupts must have a lower logical interrupt priority (higher numerical value) than a define in FreeRTOS, else the thread safe API functions could not be called safely. The USB was given a priority of 5 and the I2C was given a priority of 6 (higher value is lower priority), but then the I2C stopped working. I then found out that the ISR (Interrupt Service Routine) of the USB was taking to long to execute and therefore "breaking" I2C packages when preempting it, this was as simple as swapping their priority. Now all other interrupts must have a priority of 7 - 15 for not to interfere with the I2C and the USB and FreeRTOS has 0-4.

Test print of a Quadrotor

I've been playing around a little with the idea of printing a Quadrotor frame. So I was browsing around on Thingiverse when I stumbled upon this design: http://www.thingiverse.com/thing:34552. I used that as a basis for my own design but changed a few things. I made the spacer from the arms a separate part, this because I didn't have long enough screws so I embedded a M3 spacer inside it so I could screw it from both sides. Also I changed so it could take the motor I have been thinking of using, made it higher to fit a battery in between the plates and added small feet.

And some pictures!

Concept design 

Test with one boom

Test with one boom 

Zoomed on the split spacer with an M3 hex-spacer inside

My 3D printer is alive!

The last couple of weeks I've been building a 3D printer in order to build cases and frames for my PCB. It's coming along really nicely so soon I can start printing frames.

It's an MendelMax 1.5 printer with an QU-BD extruder. Some modifications have been made to the extruder because the original tensioner for the filament was really bad, it had no margin for error in the filament. We made it spring loaded plus a bearing to make it slide better and with less resistance.

Entire assembly 

Where the bearing pushes on the filament

Video when printing a few parts at 50-70mm/s

Preliminary design for a Config software

In order to make it easier to configure the system a simple configuration software has been designed. This is the first preliminary design so it will be subjected to change as the project goes on.

It has functionality to:

• Talk with the bootloader for firmware download
• Set gains and limits in the regulator (this is going to change a lot, some changes has already been made to the regulator)
• Calibrate the input signals to their specific function (Analog, On/Off, 3-state)
• Set the mixing of control signals for different frames, select mixers from a few different presets and the PPM update frequency

Some pictures:

More info: Hardware/Software

Here are some specifications for the hardware and software. This is valid for rev. 2.2. Later revisions  might have small changes.

HW Specs:

• ARM Cortex-M4F STM32F405 CPU w/ FPU and DSP @ 168MHz
• MPU-6050 accelerometer/gyroscope
• HMC5883L magnetometer
• MS5611-01BA01 pressure sensor
• 8 PWM outputs (50/400Hz)
• 4 expansion connectors (3 UARTs & 1 CAN)
• 6 slot RC input connector with support for (C)PPM up to 8 channels in and signal strength

SW Specs:

• Runs FreeRTOS (with FPU support) with an Hardware Abstraction Layer on top for fast application development
• USB Bootloader - no need for an external programmer!
• Quaternion based Uncented/Extended Kalman Filter provides attitude and position estimation
• "Control signal to Motor"-mixing for different types of frames
• Flight control: Rate mode, Attitude mode and more to come as deveopment proceedes

USB serial driver working!

Finally I got the USB serial driver to work! After a whole lot of testing on the STM32F4-Discovery I managed to port the code to my application. Now this can be used to communicate with the chip and work as a down-link for the bootloader.

First test one Discovery-board:

Final result (Swedish):

First post: Some old info about box and PCB soldering

As this is the first post some old information is added to start of with. More info on recent updates will come soon.

A friend of mine used his 3D printer to test making a case for the board, it turned out really well and will help protect the board.

First version of a box for the PCB

Here are some pictures of the soldering of a board. However this is an old version of the board. The new version has a few updates in order to make it easier to solder and so a new case could be used (some components were to close to the edge of the board).

 Bottom

Top