Merge branch 'master' of github.com:PX4/Firmware into px4io-i2c

55 files changed, 3177 insertions, 691 deletions

diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile
index 4d531867c..734af7cc1 100644..100755
--- a/apps/attitude_estimator_ekf/Makefile
+++ b/apps/attitude_estimator_ekf/Makefile
@@ -47,8 +47,6 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/rtGetInf.c \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
-		   codegen/diag.c \
-		   codegen/power.c \
 		   codegen/cross.c 
 
 
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 1c3b9976b..7ca77e513 100644..100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -431,6 +431,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
 					att.yawspeed = x_aposteriori[2];
+					att.rollacc = x_aposteriori[3];
+					att.pitchacc = x_aposteriori[4];
+					att.yawacc = x_aposteriori[5];
+
 					//att.yawspeed =z_k[2] ;
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 91fec9ccb..7d3812abd 100644..100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,34 +44,20 @@
 /* Extended Kalman Filter covariances */
 
 /* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q0, 1e1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
 /* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f);
-/* accelerometer process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f);
-/* magnetometer process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
 /* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
-/* magnetometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
+
 /* offsets in roll, pitch and yaw of sensor plane and body */
 PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
@@ -80,28 +66,16 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
-	h->q0 	=	param_find("EKF_ATT_Q0");
-	h->q1 	=	param_find("EKF_ATT_Q1");
-	h->q2 	=	param_find("EKF_ATT_Q2");
-	h->q3 	=	param_find("EKF_ATT_Q3");
-	h->q4 	=	param_find("EKF_ATT_Q4");
-	h->q5 	=	param_find("EKF_ATT_Q5");
-	h->q6 	=	param_find("EKF_ATT_Q6");
-	h->q7 	=	param_find("EKF_ATT_Q7");
-	h->q8 	=	param_find("EKF_ATT_Q8");
-	h->q9 	=	param_find("EKF_ATT_Q9");
-	h->q10 	=	param_find("EKF_ATT_Q10");
-	h->q11 	=	param_find("EKF_ATT_Q11");
+	h->q0 	=	param_find("EKF_ATT_V2_Q0");
+	h->q1 	=	param_find("EKF_ATT_V2_Q1");
+	h->q2 	=	param_find("EKF_ATT_V2_Q2");
+	h->q3 	=	param_find("EKF_ATT_V2_Q3");
+	h->q4 	=	param_find("EKF_ATT_V2_Q4");
 
-	h->r0 	=	param_find("EKF_ATT_R0");
-	h->r1 	=	param_find("EKF_ATT_R1");
-	h->r2 	=	param_find("EKF_ATT_R2");
-	h->r3 	=	param_find("EKF_ATT_R3");
-	h->r4 	=	param_find("EKF_ATT_R4");
-	h->r5 	=	param_find("EKF_ATT_R5");
-	h->r6 	=	param_find("EKF_ATT_R6");
-	h->r7 	=	param_find("EKF_ATT_R7");
-	h->r8 	=	param_find("EKF_ATT_R8");
+	h->r0 	=	param_find("EKF_ATT_V2_R0");
+	h->r1 	=	param_find("EKF_ATT_V2_R1");
+	h->r2 	=	param_find("EKF_ATT_V2_R2");
+	h->r3 	=	param_find("EKF_ATT_V2_R3");
 
 	h->roll_off  =	param_find("ATT_ROLL_OFFS");
 	h->pitch_off =	param_find("ATT_PITCH_OFFS");
@@ -117,23 +91,11 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
 	param_get(h->q4, &(p->q[4]));
-	param_get(h->q5, &(p->q[5]));
-	param_get(h->q6, &(p->q[6]));
-	param_get(h->q7, &(p->q[7]));
-	param_get(h->q8, &(p->q[8]));
-	param_get(h->q9, &(p->q[9]));
-	param_get(h->q10, &(p->q[10]));
-	param_get(h->q11, &(p->q[11]));
 
 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
 	param_get(h->r3, &(p->r[3]));
-	param_get(h->r4, &(p->r[4]));
-	param_get(h->r5, &(p->r[5]));
-	param_get(h->r6, &(p->r[6]));
-	param_get(h->r7, &(p->r[7]));
-	param_get(h->r8, &(p->r[8]));
 
 	param_get(h->roll_off, &(p->roll_off));
 	param_get(h->pitch_off, &(p->pitch_off));
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index b315d5fe8..09817d58e 100644..100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -50,8 +50,8 @@ struct attitude_estimator_ekf_params {
 };
 
 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
-	param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
+	param_t r0, r1, r2, r3;
+	param_t q0, q1, q2, q3, q4;
 	param_t roll_off, pitch_off, yaw_off;
 };
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 04f6ea267..9e97ad11a 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

@@ -15,8 +15,6 @@
 #include "cross.h"

 #include "eye.h"

 #include "mrdivide.h"

-#include "diag.h"

-#include "power.h"

 

 /* Type Definitions */

 

@@ -50,7 +48,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
       b_u1 = -1;

     }

 

-    y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1);

+    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

   } else if (u1 == 0.0F) {

     if (u0 > 0.0F) {

       y = RT_PIF / 2.0F;

@@ -60,7 +58,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
       y = 0.0F;

     }

   } else {

-    y = (real32_T)atan2f(u0, u1);

+    y = (real32_T)atan2(u0, u1);

   }

 

   return y;

@@ -71,80 +69,88 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
  */

 void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

   real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

-  P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T

+  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T

   eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

   P_aposteriori[144])

 {

-  real32_T a[12];

-  real32_T b_a[12];

-  int32_T i;

-  real32_T Q[144];

+  real32_T wak[3];

   real32_T O[9];

   real_T dv0[9];

-  real32_T c_a[9];

-  real32_T d_a[9];

+  real32_T a[9];

+  int32_T i;

+  real32_T b_a[9];

   real32_T x_n_b[3];

   real32_T b_x_aposteriori_k[3];

-  real32_T m_n_b[3];

   real32_T z_n_b[3];

-  real32_T x_apriori[12];

+  real32_T c_a[3];

+  real32_T d_a[3];

   int32_T i0;

+  real32_T x_apriori[12];

   real_T dv1[144];

   real32_T A_lin[144];

+  static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

+

   real32_T b_A_lin[144];

+  real32_T b_q[144];

+  real32_T c_A_lin[144];

+  real32_T d_A_lin[144];

+  real32_T e_A_lin[144];

   int32_T i1;

   real32_T P_apriori[144];

-  real32_T f0;

-  real32_T R[81];

   real32_T b_P_apriori[108];

-  static const int8_T iv0[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

+  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

 

   real32_T K_k[108];

-  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

+  real32_T fv0[81];

+  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

     0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,

     0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

 

-  real32_T fv0[81];

+  real32_T b_r[81];

+  real32_T fv1[81];

+  real32_T f0;

   real32_T c_P_apriori[36];

-  static const int8_T iv2[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+  static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

 

-  real32_T fv1[36];

-  static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,

+  real32_T fv2[36];

+  static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

 

-  real32_T S_k[36];

+  real32_T c_r[9];

+  real32_T b_K_k[36];

   real32_T d_P_apriori[72];

-  static const int8_T iv4[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     1, 0, 0, 0 };

 

-  real32_T b_K_k[72];

-  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

+  real32_T c_K_k[72];

+  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

     0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 0, 0, 0 };

 

-  real32_T b_r[6];

-  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

-    0, 0, 0, 1 };

-

+  real32_T b_z[6];

   static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     0, 0, 0, 1 };

 

-  real32_T fv2[6];

-  real32_T b_z[6];

+  static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

+    0, 0, 0, 1 };

+

+  real32_T fv3[6];

+  real32_T c_z[6];

 

   /*  Extended Attitude Kalmanfilter */

   /*  */

@@ -160,44 +166,31 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   /* coder.varsize('udpIndVect', [9,1], [1,0]) */

   /* udpIndVect=find(updVect); */

   /* process and measurement noise covariance matrix */

-  /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */

-  power(q, a);

-  for (i = 0; i < 12; i++) {

-    b_a[i] = a[i] * dt;

-  }

-

-  diag(b_a, Q);

-

-  /*   Q=[1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /*       0,     1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /*       0,     0,      1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /*       0,     0,      0,      1e-10,   0,      0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */

-  /*       0,     0,      0,      0,      1e-10,   0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */

-  /*       0,     0,      0,      0,      0,      1e-10,   0,      0,      0,      0,      0,      0; */

-  /*       0,     0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0,      0; */

-  /*       0,     0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0; */

-  /*       0,     0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0; */

-  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0; */

-  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0; */

-  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1]; */

+  /* Q = diag(q.^2*dt); */

   /* observation matrix */

-  /* 'attitudeKalmanfilter:44' wx=  x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:45' wy=  x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:46' wz=  x_aposteriori_k(3); */

-  /* 'attitudeKalmanfilter:48' wox=  x_aposteriori_k(4); */

-  /* 'attitudeKalmanfilter:49' woy=  x_aposteriori_k(5); */

-  /* 'attitudeKalmanfilter:50' woz=  x_aposteriori_k(6); */

-  /* 'attitudeKalmanfilter:52' zex=  x_aposteriori_k(7); */

-  /* 'attitudeKalmanfilter:53' zey=  x_aposteriori_k(8); */

-  /* 'attitudeKalmanfilter:54' zez=  x_aposteriori_k(9); */

-  /* 'attitudeKalmanfilter:56' mux=  x_aposteriori_k(10); */

-  /* 'attitudeKalmanfilter:57' muy=  x_aposteriori_k(11); */

-  /* 'attitudeKalmanfilter:58' muz=  x_aposteriori_k(12); */

-  /* 'attitudeKalmanfilter:61' wk =[wx; */

-  /* 'attitudeKalmanfilter:62'      wy; */

-  /* 'attitudeKalmanfilter:63'      wz]; */

-  /* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */

-  /* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

+  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */

+  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */

+  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */

+  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */

+  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */

+  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */

+  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */

+  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */

+  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */

+  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */

+  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */

+  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */

+  /* % prediction section */

+  /* body angular accelerations */

+  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */

+  wak[0] = x_aposteriori_k[3];

+  wak[1] = x_aposteriori_k[4];

+  wak[2] = x_aposteriori_k[5];

+

+  /* body angular rates */

+  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */

+  /* derivative of the prediction rotation matrix */

+  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

   O[0] = 0.0F;

   O[1] = -x_aposteriori_k[2];

   O[2] = x_aposteriori_k[1];

@@ -208,69 +201,73 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   O[7] = x_aposteriori_k[0];

   O[8] = 0.0F;

 

-  /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

+  /* prediction of the earth z vector */

+  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

   eye(dv0);

   for (i = 0; i < 9; i++) {

-    c_a[i] = (real32_T)dv0[i] + O[i] * dt;

+    a[i] = (real32_T)dv0[i] + O[i] * dt;

   }

 

-  /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

+  /* prediction of the magnetic vector */

+  /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

   eye(dv0);

   for (i = 0; i < 9; i++) {

-    d_a[i] = (real32_T)dv0[i] + O[i] * dt;

+    b_a[i] = (real32_T)dv0[i] + O[i] * dt;

   }

 

-  /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */

-  /* 'attitudeKalmanfilter:71'     -zez,0,zex; */

-  /* 'attitudeKalmanfilter:72'     zey,-zex,0]'; */

-  /* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */

-  /* 'attitudeKalmanfilter:74'     -muz,0,mux; */

-  /* 'attitudeKalmanfilter:75'     zey,-mux,0]'; */

-  /* 'attitudeKalmanfilter:79' E=eye(3); */

-  /* 'attitudeKalmanfilter:80' Es=[1,0,0; */

-  /* 'attitudeKalmanfilter:81'     0,1,0; */

-  /* 'attitudeKalmanfilter:82'     0,0,0]; */

-  /* 'attitudeKalmanfilter:83' Z=zeros(3); */

-  /* 'attitudeKalmanfilter:84' x_apriori=[wk;wok;zek;muk]; */

-  x_n_b[0] = x_aposteriori_k[6];

-  x_n_b[1] = x_aposteriori_k[7];

-  x_n_b[2] = x_aposteriori_k[8];

-  b_x_aposteriori_k[0] = x_aposteriori_k[9];

-  b_x_aposteriori_k[1] = x_aposteriori_k[10];

-  b_x_aposteriori_k[2] = x_aposteriori_k[11];

-  x_apriori[0] = x_aposteriori_k[0];

-  x_apriori[1] = x_aposteriori_k[1];

-  x_apriori[2] = x_aposteriori_k[2];

-  x_apriori[3] = x_aposteriori_k[3];

-  x_apriori[4] = x_aposteriori_k[4];

-  x_apriori[5] = x_aposteriori_k[5];

+  /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */

+  /* 'attitudeKalmanfilter:66'     -zez,0,zex; */

+  /* 'attitudeKalmanfilter:67'     zey,-zex,0]'; */

+  /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */

+  /* 'attitudeKalmanfilter:69'     -muz,0,mux; */

+  /* 'attitudeKalmanfilter:70'     zey,-mux,0]'; */

+  /* 'attitudeKalmanfilter:74' E=eye(3); */

+  /* 'attitudeKalmanfilter:76' Z=zeros(3); */

+  /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */

+  x_n_b[0] = x_aposteriori_k[0];

+  x_n_b[1] = x_aposteriori_k[1];

+  x_n_b[2] = x_aposteriori_k[2];

+  b_x_aposteriori_k[0] = x_aposteriori_k[6];

+  b_x_aposteriori_k[1] = x_aposteriori_k[7];

+  b_x_aposteriori_k[2] = x_aposteriori_k[8];

+  z_n_b[0] = x_aposteriori_k[9];

+  z_n_b[1] = x_aposteriori_k[10];

+  z_n_b[2] = x_aposteriori_k[11];

   for (i = 0; i < 3; i++) {

-    m_n_b[i] = 0.0F;

+    c_a[i] = 0.0F;

     for (i0 = 0; i0 < 3; i0++) {

-      m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];

+      c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];

     }

 

-    z_n_b[i] = 0.0F;

+    d_a[i] = 0.0F;

     for (i0 = 0; i0 < 3; i0++) {

-      z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0];

+      d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];

     }

 

-    x_apriori[i + 6] = m_n_b[i];

+    x_apriori[i] = x_n_b[i] + dt * wak[i];

+  }

+

+  for (i = 0; i < 3; i++) {

+    x_apriori[i + 3] = wak[i];

+  }

+

+  for (i = 0; i < 3; i++) {

+    x_apriori[i + 6] = c_a[i];

   }

 

   for (i = 0; i < 3; i++) {

-    x_apriori[i + 9] = z_n_b[i];

+    x_apriori[i + 9] = d_a[i];

   }

 

-  /* 'attitudeKalmanfilter:86' A_lin=[ Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:87'         Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:88'         EZ, Z,  O,  Z */

-  /* 'attitudeKalmanfilter:89'         MA, Z,  Z,  O]; */

-  /* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */

+  /* 'attitudeKalmanfilter:81' A_lin=[ Z,  E,  Z,  Z */

+  /* 'attitudeKalmanfilter:82'     Z,  Z,  Z,  Z */

+  /* 'attitudeKalmanfilter:83'     EZ, Z,  O,  Z */

+  /* 'attitudeKalmanfilter:84'     MA, Z,  Z,  O]; */

+  /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */

   b_eye(dv1);

   for (i = 0; i < 12; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 12 * i] = 0.0F;

+      A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];

     }

 

     for (i0 = 0; i0 < 3; i0++) {

@@ -339,7 +336,164 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
     }

   }

 

-  /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

+  /* 'attitudeKalmanfilter:88' Qtemp=[ q(1),     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:89'         0,     q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:90'         0,     0,      q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:91'         0,     0,      0,      q(2),   0,      0,     0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:92'         0,     0,      0,      0,      q(2),   0,     0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:93'         0,     0,      0,      0,      0,      q(2),   0,      0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:94'         0,     0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:95'         0,     0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0; */

+  /* 'attitudeKalmanfilter:96'         0,     0,      0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0; */

+  /* 'attitudeKalmanfilter:97'         0,     0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0,      0; */

+  /* 'attitudeKalmanfilter:98'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0; */

+  /* 'attitudeKalmanfilter:99'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4)]; */

+  /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */

+  /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

+  b_q[0] = q[0];

+  b_q[12] = 0.0F;

+  b_q[24] = 0.0F;

+  b_q[36] = 0.0F;

+  b_q[48] = 0.0F;

+  b_q[60] = 0.0F;

+  b_q[72] = 0.0F;

+  b_q[84] = 0.0F;

+  b_q[96] = 0.0F;

+  b_q[108] = 0.0F;

+  b_q[120] = 0.0F;

+  b_q[132] = 0.0F;

+  b_q[1] = 0.0F;

+  b_q[13] = q[0];

+  b_q[25] = 0.0F;

+  b_q[37] = 0.0F;

+  b_q[49] = 0.0F;

+  b_q[61] = 0.0F;

+  b_q[73] = 0.0F;

+  b_q[85] = 0.0F;

+  b_q[97] = 0.0F;

+  b_q[109] = 0.0F;

+  b_q[121] = 0.0F;

+  b_q[133] = 0.0F;

+  b_q[2] = 0.0F;

+  b_q[14] = 0.0F;

+  b_q[26] = q[0];

+  b_q[38] = 0.0F;

+  b_q[50] = 0.0F;

+  b_q[62] = 0.0F;

+  b_q[74] = 0.0F;

+  b_q[86] = 0.0F;

+  b_q[98] = 0.0F;

+  b_q[110] = 0.0F;

+  b_q[122] = 0.0F;

+  b_q[134] = 0.0F;

+  b_q[3] = 0.0F;

+  b_q[15] = 0.0F;

+  b_q[27] = 0.0F;

+  b_q[39] = q[1];

+  b_q[51] = 0.0F;

+  b_q[63] = 0.0F;

+  b_q[75] = 0.0F;

+  b_q[87] = 0.0F;

+  b_q[99] = 0.0F;

+  b_q[111] = 0.0F;

+  b_q[123] = 0.0F;

+  b_q[135] = 0.0F;

+  b_q[4] = 0.0F;

+  b_q[16] = 0.0F;

+  b_q[28] = 0.0F;

+  b_q[40] = 0.0F;

+  b_q[52] = q[1];

+  b_q[64] = 0.0F;

+  b_q[76] = 0.0F;

+  b_q[88] = 0.0F;

+  b_q[100] = 0.0F;

+  b_q[112] = 0.0F;

+  b_q[124] = 0.0F;

+  b_q[136] = 0.0F;

+  b_q[5] = 0.0F;

+  b_q[17] = 0.0F;

+  b_q[29] = 0.0F;

+  b_q[41] = 0.0F;

+  b_q[53] = 0.0F;

+  b_q[65] = q[1];

+  b_q[77] = 0.0F;

+  b_q[89] = 0.0F;

+  b_q[101] = 0.0F;

+  b_q[113] = 0.0F;

+  b_q[125] = 0.0F;

+  b_q[137] = 0.0F;

+  b_q[6] = 0.0F;

+  b_q[18] = 0.0F;

+  b_q[30] = 0.0F;

+  b_q[42] = 0.0F;

+  b_q[54] = 0.0F;

+  b_q[66] = 0.0F;

+  b_q[78] = q[2];

+  b_q[90] = 0.0F;

+  b_q[102] = 0.0F;

+  b_q[114] = 0.0F;

+  b_q[126] = 0.0F;

+  b_q[138] = 0.0F;

+  b_q[7] = 0.0F;

+  b_q[19] = 0.0F;

+  b_q[31] = 0.0F;

+  b_q[43] = 0.0F;

+  b_q[55] = 0.0F;

+  b_q[67] = 0.0F;

+  b_q[79] = 0.0F;

+  b_q[91] = q[2];

+  b_q[103] = 0.0F;

+  b_q[115] = 0.0F;

+  b_q[127] = 0.0F;

+  b_q[139] = 0.0F;

+  b_q[8] = 0.0F;

+  b_q[20] = 0.0F;

+  b_q[32] = 0.0F;

+  b_q[44] = 0.0F;

+  b_q[56] = 0.0F;

+  b_q[68] = 0.0F;

+  b_q[80] = 0.0F;

+  b_q[92] = 0.0F;

+  b_q[104] = q[2];

+  b_q[116] = 0.0F;

+  b_q[128] = 0.0F;

+  b_q[140] = 0.0F;

+  b_q[9] = 0.0F;

+  b_q[21] = 0.0F;

+  b_q[33] = 0.0F;

+  b_q[45] = 0.0F;

+  b_q[57] = 0.0F;

+  b_q[69] = 0.0F;

+  b_q[81] = 0.0F;

+  b_q[93] = 0.0F;

+  b_q[105] = 0.0F;

+  b_q[117] = q[3];

+  b_q[129] = 0.0F;

+  b_q[141] = 0.0F;

+  b_q[10] = 0.0F;

+  b_q[22] = 0.0F;

+  b_q[34] = 0.0F;

+  b_q[46] = 0.0F;

+  b_q[58] = 0.0F;

+  b_q[70] = 0.0F;

+  b_q[82] = 0.0F;

+  b_q[94] = 0.0F;

+  b_q[106] = 0.0F;

+  b_q[118] = 0.0F;

+  b_q[130] = q[3];

+  b_q[142] = 0.0F;

+  b_q[11] = 0.0F;

+  b_q[23] = 0.0F;

+  b_q[35] = 0.0F;

+  b_q[47] = 0.0F;

+  b_q[59] = 0.0F;

+  b_q[71] = 0.0F;

+  b_q[83] = 0.0F;

+  b_q[95] = 0.0F;

+  b_q[107] = 0.0F;

+  b_q[119] = 0.0F;

+  b_q[131] = 0.0F;

+  b_q[143] = q[3];

   for (i = 0; i < 12; i++) {

     for (i0 = 0; i0 < 12; i0++) {

       A_lin[i + 12 * i0] = 0.0F;

@@ -347,38 +501,63 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

           i0];

       }

+

+      c_A_lin[i + 12 * i0] = 0.0F;

+      for (i1 = 0; i1 < 12; i1++) {

+        c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];

+      }

     }

-  }

 

-  for (i = 0; i < 12; i++) {

     for (i0 = 0; i0 < 12; i0++) {

-      f0 = 0.0F;

+      d_A_lin[i + 12 * i0] = 0.0F;

       for (i1 = 0; i1 < 12; i1++) {

-        f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

+        d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

       }

 

-      P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0];

+      e_A_lin[i + 12 * i0] = 0.0F;

+      for (i1 = 0; i1 < 12; i1++) {

+        e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

+      }

     }

   }

 

-  /* %update */

-  /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

+  for (i = 0; i < 12; i++) {

+    for (i0 = 0; i0 < 12; i0++) {

+      P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];

+    }

+  }

+

+  /* % update */

+  /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

   if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

-    /* 'attitudeKalmanfilter:103' R=diag(r); */

-    b_diag(r, R);

+    /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */

+    if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

+      /* 'attitudeKalmanfilter:112' r(2)=10000; */

+      r[1] = 10000.0F;

+    }

 

+    /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */

+    /* 'attitudeKalmanfilter:115'         0,r(1),0,0,0,0,0,0,0; */

+    /* 'attitudeKalmanfilter:116'         0,0,r(1),0,0,0,0,0,0; */

+    /* 'attitudeKalmanfilter:117'         0,0,0,r(2),0,0,0,0,0; */

+    /* 'attitudeKalmanfilter:118'         0,0,0,0,r(2),0,0,0,0; */

+    /* 'attitudeKalmanfilter:119'         0,0,0,0,0,r(2),0,0,0; */

+    /* 'attitudeKalmanfilter:120'         0,0,0,0,0,0,r(3),0,0; */

+    /* 'attitudeKalmanfilter:121'         0,0,0,0,0,0,0,r(3),0; */

+    /* 'attitudeKalmanfilter:122'         0,0,0,0,0,0,0,0,r(3)]; */

     /* observation matrix */

-    /* 'attitudeKalmanfilter:106' H_k=[  E,     Z,      Z,    Z; */

-    /* 'attitudeKalmanfilter:107'         Z,     Z,      E,    Z; */

-    /* 'attitudeKalmanfilter:108'         Z,     Z,      Z,    E]; */

-    /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */

-    /* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */

-    /* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */

+    /* [zw;ze;zmk]; */

+    /* 'attitudeKalmanfilter:125' H_k=[  E,     Z,      Z,    Z; */

+    /* 'attitudeKalmanfilter:126'         Z,     Z,      E,    Z; */

+    /* 'attitudeKalmanfilter:127'         Z,     Z,      Z,    E]; */

+    /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */

+    /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */

+    /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */

     for (i = 0; i < 12; i++) {

       for (i0 = 0; i0 < 9; i0++) {

         b_P_apriori[i + 12 * i0] = 0.0F;

         for (i1 = 0; i1 < 12; i1++) {

-          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1

+          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1

             + 12 * i0];

         }

       }

@@ -388,53 +567,136 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
       for (i0 = 0; i0 < 12; i0++) {

         K_k[i + 9 * i0] = 0.0F;

         for (i1 = 0; i1 < 12; i1++) {

-          K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0];

+          K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];

         }

       }

-    }

 

-    for (i = 0; i < 9; i++) {

       for (i0 = 0; i0 < 9; i0++) {

-        f0 = 0.0F;

+        fv0[i + 9 * i0] = 0.0F;

         for (i1 = 0; i1 < 12; i1++) {

-          f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];

+          fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];

         }

+      }

+    }

 

-        fv0[i + 9 * i0] = f0 + R[i + 9 * i0];

+    b_r[0] = r[0];

+    b_r[9] = 0.0F;

+    b_r[18] = 0.0F;

+    b_r[27] = 0.0F;

+    b_r[36] = 0.0F;

+    b_r[45] = 0.0F;

+    b_r[54] = 0.0F;

+    b_r[63] = 0.0F;

+    b_r[72] = 0.0F;

+    b_r[1] = 0.0F;

+    b_r[10] = r[0];

+    b_r[19] = 0.0F;

+    b_r[28] = 0.0F;

+    b_r[37] = 0.0F;

+    b_r[46] = 0.0F;

+    b_r[55] = 0.0F;

+    b_r[64] = 0.0F;

+    b_r[73] = 0.0F;

+    b_r[2] = 0.0F;

+    b_r[11] = 0.0F;

+    b_r[20] = r[0];

+    b_r[29] = 0.0F;

+    b_r[38] = 0.0F;

+    b_r[47] = 0.0F;

+    b_r[56] = 0.0F;

+    b_r[65] = 0.0F;

+    b_r[74] = 0.0F;

+    b_r[3] = 0.0F;

+    b_r[12] = 0.0F;

+    b_r[21] = 0.0F;

+    b_r[30] = r[1];

+    b_r[39] = 0.0F;

+    b_r[48] = 0.0F;

+    b_r[57] = 0.0F;

+    b_r[66] = 0.0F;

+    b_r[75] = 0.0F;

+    b_r[4] = 0.0F;

+    b_r[13] = 0.0F;

+    b_r[22] = 0.0F;

+    b_r[31] = 0.0F;

+    b_r[40] = r[1];

+    b_r[49] = 0.0F;

+    b_r[58] = 0.0F;

+    b_r[67] = 0.0F;

+    b_r[76] = 0.0F;

+    b_r[5] = 0.0F;

+    b_r[14] = 0.0F;

+    b_r[23] = 0.0F;

+    b_r[32] = 0.0F;

+    b_r[41] = 0.0F;

+    b_r[50] = r[1];

+    b_r[59] = 0.0F;

+    b_r[68] = 0.0F;

+    b_r[77] = 0.0F;

+    b_r[6] = 0.0F;

+    b_r[15] = 0.0F;

+    b_r[24] = 0.0F;

+    b_r[33] = 0.0F;

+    b_r[42] = 0.0F;

+    b_r[51] = 0.0F;

+    b_r[60] = r[2];

+    b_r[69] = 0.0F;

+    b_r[78] = 0.0F;

+    b_r[7] = 0.0F;

+    b_r[16] = 0.0F;

+    b_r[25] = 0.0F;

+    b_r[34] = 0.0F;

+    b_r[43] = 0.0F;

+    b_r[52] = 0.0F;

+    b_r[61] = 0.0F;

+    b_r[70] = r[2];

+    b_r[79] = 0.0F;

+    b_r[8] = 0.0F;

+    b_r[17] = 0.0F;

+    b_r[26] = 0.0F;

+    b_r[35] = 0.0F;

+    b_r[44] = 0.0F;

+    b_r[53] = 0.0F;

+    b_r[62] = 0.0F;

+    b_r[71] = 0.0F;

+    b_r[80] = r[2];

+    for (i = 0; i < 9; i++) {

+      for (i0 = 0; i0 < 9; i0++) {

+        fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];

       }

     }

 

-    mrdivide(b_P_apriori, fv0, K_k);

+    mrdivide(b_P_apriori, fv1, K_k);

 

-    /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */

+    /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */

     for (i = 0; i < 9; i++) {

       f0 = 0.0F;

       for (i0 = 0; i0 < 12; i0++) {

-        f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];

+        f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];

       }

 

-      c_a[i] = z[i] - f0;

+      O[i] = z[i] - f0;

     }

 

     for (i = 0; i < 12; i++) {

       f0 = 0.0F;

       for (i0 = 0; i0 < 9; i0++) {

-        f0 += K_k[i + 12 * i0] * c_a[i0];

+        f0 += K_k[i + 12 * i0] * O[i0];

       }

 

       x_aposteriori[i] = x_apriori[i] + f0;

     }

 

-    /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

+    /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

     b_eye(dv1);

     for (i = 0; i < 12; i++) {

       for (i0 = 0; i0 < 12; i0++) {

         f0 = 0.0F;

         for (i1 = 0; i1 < 9; i1++) {

-          f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];

+          f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];

         }

 

-        Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

+        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

       }

     }

 

@@ -442,60 +704,72 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
       for (i0 = 0; i0 < 12; i0++) {

         P_aposteriori[i + 12 * i0] = 0.0F;

         for (i1 = 0; i1 < 12; i1++) {

-          P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];

+          P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12

+            * i0];

         }

       }

     }

   } else {

-    /* 'attitudeKalmanfilter:118' else */

-    /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

+    /* 'attitudeKalmanfilter:138' else */

+    /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

     if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

-      /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */

-      c_diag(*(real32_T (*)[3])&r[0], O);

-

+      /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */

+      /* 'attitudeKalmanfilter:142'             0,r(1),0; */

+      /* 'attitudeKalmanfilter:143'             0,0,r(1)]; */

       /* observation matrix */

-      /* 'attitudeKalmanfilter:123' H_k=[  E,     Z,      Z,    Z]; */

-      /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

-      /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

-      /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

+      /* 'attitudeKalmanfilter:146' H_k=[  E,     Z,      Z,    Z]; */

+      /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

+      /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

+      /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

       for (i = 0; i < 12; i++) {

         for (i0 = 0; i0 < 3; i0++) {

           c_P_apriori[i + 12 * i0] = 0.0F;

           for (i1 = 0; i1 < 12; i1++) {

             c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-              iv2[i1 + 12 * i0];

+              iv3[i1 + 12 * i0];

           }

         }

       }

 

       for (i = 0; i < 3; i++) {

         for (i0 = 0; i0 < 12; i0++) {

-          fv1[i + 3 * i0] = 0.0F;

+          fv2[i + 3 * i0] = 0.0F;

           for (i1 = 0; i1 < 12; i1++) {

-            fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *

+            fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *

               i0];

           }

         }

-      }

 

-      for (i = 0; i < 3; i++) {

         for (i0 = 0; i0 < 3; i0++) {

-          f0 = 0.0F;

+          O[i + 3 * i0] = 0.0F;

           for (i1 = 0; i1 < 12; i1++) {

-            f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];

+            O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];

           }

+        }

+      }

 

-          c_a[i + 3 * i0] = f0 + O[i + 3 * i0];

+      c_r[0] = r[0];

+      c_r[3] = 0.0F;

+      c_r[6] = 0.0F;

+      c_r[1] = 0.0F;

+      c_r[4] = r[0];

+      c_r[7] = 0.0F;

+      c_r[2] = 0.0F;

+      c_r[5] = 0.0F;

+      c_r[8] = r[0];

+      for (i = 0; i < 3; i++) {

+        for (i0 = 0; i0 < 3; i0++) {

+          a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];

         }

       }

 

-      b_mrdivide(c_P_apriori, c_a, S_k);

+      b_mrdivide(c_P_apriori, a, b_K_k);

 

-      /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */

+      /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */

       for (i = 0; i < 3; i++) {

         f0 = 0.0F;

         for (i0 = 0; i0 < 12; i0++) {

-          f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];

+          f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];

         }

 

         x_n_b[i] = z[i] - f0;

@@ -504,22 +778,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
       for (i = 0; i < 12; i++) {

         f0 = 0.0F;

         for (i0 = 0; i0 < 3; i0++) {

-          f0 += S_k[i + 12 * i0] * x_n_b[i0];

+          f0 += b_K_k[i + 12 * i0] * x_n_b[i0];

         }

 

         x_aposteriori[i] = x_apriori[i] + f0;

       }

 

-      /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

+      /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

       b_eye(dv1);

       for (i = 0; i < 12; i++) {

         for (i0 = 0; i0 < 12; i0++) {

           f0 = 0.0F;

           for (i1 = 0; i1 < 3; i1++) {

-            f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];

+            f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];

           }

 

-          Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

+          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

         }

       }

 

@@ -527,87 +801,134 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         for (i0 = 0; i0 < 12; i0++) {

           P_aposteriori[i + 12 * i0] = 0.0F;

           for (i1 = 0; i1 < 12; i1++) {

-            P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *

-              i0];

+            P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +

+              12 * i0];

           }

         }

       }

     } else {

-      /* 'attitudeKalmanfilter:133' else */

-      /* 'attitudeKalmanfilter:134' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

+      /* 'attitudeKalmanfilter:156' else */

+      /* 'attitudeKalmanfilter:157' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

       if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

       {

-        /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */

-        d_diag(*(real32_T (*)[6])&r[0], S_k);

+        /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */

+        if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

+          /* 'attitudeKalmanfilter:159' r(2)=10000; */

+          r[1] = 10000.0F;

+        }

 

+        /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */

+        /* 'attitudeKalmanfilter:163'                 0,r(1),0,0,0,0; */

+        /* 'attitudeKalmanfilter:164'                 0,0,r(1),0,0,0; */

+        /* 'attitudeKalmanfilter:165'                 0,0,0,r(2),0,0; */

+        /* 'attitudeKalmanfilter:166'                 0,0,0,0,r(2),0; */

+        /* 'attitudeKalmanfilter:167'                 0,0,0,0,0,r(2)]; */

         /* observation matrix */

-        /* 'attitudeKalmanfilter:138' H_k=[  E,     Z,      Z,    Z; */

-        /* 'attitudeKalmanfilter:139'                     Z,     Z,      E,    Z]; */

-        /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

-        /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-        /* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

+        /* 'attitudeKalmanfilter:170' H_k=[  E,     Z,      Z,    Z; */

+        /* 'attitudeKalmanfilter:171'                 Z,     Z,      E,    Z]; */

+        /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

+        /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

+        /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

         for (i = 0; i < 12; i++) {

           for (i0 = 0; i0 < 6; i0++) {

             d_P_apriori[i + 12 * i0] = 0.0F;

             for (i1 = 0; i1 < 12; i1++) {

               d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                iv4[i1 + 12 * i0];

+                iv5[i1 + 12 * i0];

             }

           }

         }

 

         for (i = 0; i < 6; i++) {

           for (i0 = 0; i0 < 12; i0++) {

-            b_K_k[i + 6 * i0] = 0.0F;

+            c_K_k[i + 6 * i0] = 0.0F;

             for (i1 = 0; i1 < 12; i1++) {

-              b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12

+              c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12

                 * i0];

             }

           }

-        }

 

-        for (i = 0; i < 6; i++) {

           for (i0 = 0; i0 < 6; i0++) {

-            f0 = 0.0F;

+            fv2[i + 6 * i0] = 0.0F;

             for (i1 = 0; i1 < 12; i1++) {

-              f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];

+              fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];

             }

+          }

+        }

 

-            fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0];

+        b_K_k[0] = r[0];

+        b_K_k[6] = 0.0F;

+        b_K_k[12] = 0.0F;

+        b_K_k[18] = 0.0F;

+        b_K_k[24] = 0.0F;

+        b_K_k[30] = 0.0F;

+        b_K_k[1] = 0.0F;

+        b_K_k[7] = r[0];

+        b_K_k[13] = 0.0F;

+        b_K_k[19] = 0.0F;

+        b_K_k[25] = 0.0F;

+        b_K_k[31] = 0.0F;

+        b_K_k[2] = 0.0F;

+        b_K_k[8] = 0.0F;

+        b_K_k[14] = r[0];

+        b_K_k[20] = 0.0F;

+        b_K_k[26] = 0.0F;

+        b_K_k[32] = 0.0F;

+        b_K_k[3] = 0.0F;

+        b_K_k[9] = 0.0F;

+        b_K_k[15] = 0.0F;

+        b_K_k[21] = r[1];

+        b_K_k[27] = 0.0F;

+        b_K_k[33] = 0.0F;

+        b_K_k[4] = 0.0F;

+        b_K_k[10] = 0.0F;

+        b_K_k[16] = 0.0F;

+        b_K_k[22] = 0.0F;

+        b_K_k[28] = r[1];

+        b_K_k[34] = 0.0F;

+        b_K_k[5] = 0.0F;

+        b_K_k[11] = 0.0F;

+        b_K_k[17] = 0.0F;

+        b_K_k[23] = 0.0F;

+        b_K_k[29] = 0.0F;

+        b_K_k[35] = r[1];

+        for (i = 0; i < 6; i++) {

+          for (i0 = 0; i0 < 6; i0++) {

+            c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

           }

         }

 

-        c_mrdivide(d_P_apriori, fv1, b_K_k);

+        c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

 

-        /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */

+        /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */

         for (i = 0; i < 6; i++) {

           f0 = 0.0F;

           for (i0 = 0; i0 < 12; i0++) {

-            f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];

+            f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

           }

 

-          b_r[i] = z[i] - f0;

+          b_z[i] = z[i] - f0;

         }

 

         for (i = 0; i < 12; i++) {

           f0 = 0.0F;

           for (i0 = 0; i0 < 6; i0++) {

-            f0 += b_K_k[i + 12 * i0] * b_r[i0];

+            f0 += c_K_k[i + 12 * i0] * b_z[i0];

           }

 

           x_aposteriori[i] = x_apriori[i] + f0;

         }

 

-        /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

+        /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

         b_eye(dv1);

         for (i = 0; i < 12; i++) {

           for (i0 = 0; i0 < 12; i0++) {

             f0 = 0.0F;

             for (i1 = 0; i1 < 6; i1++) {

-              f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];

+              f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

             }

 

-            Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

+            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

           }

         }

 

@@ -615,98 +936,137 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
           for (i0 = 0; i0 < 12; i0++) {

             P_aposteriori[i + 12 * i0] = 0.0F;

             for (i1 = 0; i1 < 12; i1++) {

-              P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *

-                i0];

+              P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1

+                + 12 * i0];

             }

           }

         }

       } else {

-        /* 'attitudeKalmanfilter:149' else */

-        /* 'attitudeKalmanfilter:150' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

+        /* 'attitudeKalmanfilter:181' else */

+        /* 'attitudeKalmanfilter:182' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

         if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

         {

-          /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */

+          /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */

+          /* 'attitudeKalmanfilter:184'                     0,r(1),0,0,0,0; */

+          /* 'attitudeKalmanfilter:185'                     0,0,r(1),0,0,0; */

+          /* 'attitudeKalmanfilter:186'                     0,0,0,r(3),0,0; */

+          /* 'attitudeKalmanfilter:187'                     0,0,0,0,r(3),0; */

+          /* 'attitudeKalmanfilter:188'                     0,0,0,0,0,r(3)]; */

           /* observation matrix */

-          /* 'attitudeKalmanfilter:154' H_k=[  E,     Z,      Z,    Z; */

-          /* 'attitudeKalmanfilter:155'                     Z,     Z,      Z,    E]; */

-          /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

-          /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

+          /* 'attitudeKalmanfilter:191' H_k=[  E,     Z,      Z,    Z; */

+          /* 'attitudeKalmanfilter:192'                     Z,     Z,      Z,    E]; */

+          /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

+          /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

+          /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

+          for (i = 0; i < 12; i++) {

+            for (i0 = 0; i0 < 6; i0++) {

+              d_P_apriori[i + 12 * i0] = 0.0F;

+              for (i1 = 0; i1 < 12; i1++) {

+                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

+                  iv7[i1 + 12 * i0];

+              }

+            }

+          }

+

           for (i = 0; i < 6; i++) {

             for (i0 = 0; i0 < 12; i0++) {

-              b_K_k[i + 6 * i0] = 0.0F;

+              c_K_k[i + 6 * i0] = 0.0F;

               for (i1 = 0; i1 < 12; i1++) {

-                b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 +

+                c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +

                   12 * i0];

               }

             }

-          }

 

-          for (i = 0; i < 3; i++) {

-            b_r[i << 1] = r[i];

-            b_r[1 + (i << 1)] = r[6 + i];

-          }

-

-          for (i = 0; i < 6; i++) {

             for (i0 = 0; i0 < 6; i0++) {

-              f0 = 0.0F;

+              fv2[i + 6 * i0] = 0.0F;

               for (i1 = 0; i1 < 12; i1++) {

-                f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];

+                fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *

+                  i0];

               }

-

-              S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)];

             }

           }

 

-          /* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-          for (i = 0; i < 12; i++) {

+          b_K_k[0] = r[0];

+          b_K_k[6] = 0.0F;

+          b_K_k[12] = 0.0F;

+          b_K_k[18] = 0.0F;

+          b_K_k[24] = 0.0F;

+          b_K_k[30] = 0.0F;

+          b_K_k[1] = 0.0F;

+          b_K_k[7] = r[0];

+          b_K_k[13] = 0.0F;

+          b_K_k[19] = 0.0F;

+          b_K_k[25] = 0.0F;

+          b_K_k[31] = 0.0F;

+          b_K_k[2] = 0.0F;

+          b_K_k[8] = 0.0F;

+          b_K_k[14] = r[0];

+          b_K_k[20] = 0.0F;

+          b_K_k[26] = 0.0F;

+          b_K_k[32] = 0.0F;

+          b_K_k[3] = 0.0F;

+          b_K_k[9] = 0.0F;

+          b_K_k[15] = 0.0F;

+          b_K_k[21] = r[2];

+          b_K_k[27] = 0.0F;

+          b_K_k[33] = 0.0F;

+          b_K_k[4] = 0.0F;

+          b_K_k[10] = 0.0F;

+          b_K_k[16] = 0.0F;

+          b_K_k[22] = 0.0F;

+          b_K_k[28] = r[2];

+          b_K_k[34] = 0.0F;

+          b_K_k[5] = 0.0F;

+          b_K_k[11] = 0.0F;

+          b_K_k[17] = 0.0F;

+          b_K_k[23] = 0.0F;

+          b_K_k[29] = 0.0F;

+          b_K_k[35] = r[2];

+          for (i = 0; i < 6; i++) {

             for (i0 = 0; i0 < 6; i0++) {

-              d_P_apriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                  iv7[i1 + 12 * i0];

-              }

+              c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

             }

           }

 

-          c_mrdivide(d_P_apriori, S_k, b_K_k);

+          c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

 

-          /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */

+          /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */

           for (i = 0; i < 3; i++) {

-            b_r[i] = z[i];

+            b_z[i] = z[i];

           }

 

           for (i = 0; i < 3; i++) {

-            b_r[i + 3] = z[i + 6];

+            b_z[i + 3] = z[i + 6];

           }

 

           for (i = 0; i < 6; i++) {

-            fv2[i] = 0.0F;

+            fv3[i] = 0.0F;

             for (i0 = 0; i0 < 12; i0++) {

-              fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

+              fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];

             }

 

-            b_z[i] = b_r[i] - fv2[i];

+            c_z[i] = b_z[i] - fv3[i];

           }

 

           for (i = 0; i < 12; i++) {

             f0 = 0.0F;

             for (i0 = 0; i0 < 6; i0++) {

-              f0 += b_K_k[i + 12 * i0] * b_z[i0];

+              f0 += c_K_k[i + 12 * i0] * c_z[i0];

             }

 

             x_aposteriori[i] = x_apriori[i] + f0;

           }

 

-          /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

+          /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

           b_eye(dv1);

           for (i = 0; i < 12; i++) {

             for (i0 = 0; i0 < 12; i0++) {

               f0 = 0.0F;

               for (i1 = 0; i1 < 6; i1++) {

-                f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

+                f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];

               }

 

-              Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

+              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

             }

           }

 

@@ -714,19 +1074,19 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
             for (i0 = 0; i0 < 12; i0++) {

               P_aposteriori[i + 12 * i0] = 0.0F;

               for (i1 = 0; i1 < 12; i1++) {

-                P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12

-                  * i0];

+                P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *

+                  P_apriori[i1 + 12 * i0];

               }

             }

           }

         } else {

-          /* 'attitudeKalmanfilter:165' else */

-          /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */

+          /* 'attitudeKalmanfilter:202' else */

+          /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */

           for (i = 0; i < 12; i++) {

             x_aposteriori[i] = x_apriori[i];

           }

 

-          /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */

+          /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */

           memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));

         }

       }

@@ -734,54 +1094,54 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   }

 

   /* % euler anglels extraction */

-  /* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

+  /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

   for (i = 0; i < 3; i++) {

     x_n_b[i] = -x_aposteriori[i + 6];

   }

 

   rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);

 

-  /* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

+  /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

   rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&

-           x_aposteriori[9]), m_n_b);

+           x_aposteriori[9]), wak);

 

-  /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */

+  /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */

   for (i = 0; i < 3; i++) {

-    x_n_b[i] = m_n_b[i];

+    x_n_b[i] = wak[i];

   }

 

-  cross(z_n_b, x_n_b, m_n_b);

+  cross(z_n_b, x_n_b, wak);

 

-  /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */

+  /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */

   for (i = 0; i < 3; i++) {

-    x_n_b[i] = m_n_b[i];

+    x_n_b[i] = wak[i];

   }

 

-  rdivide(x_n_b, norm(m_n_b), m_n_b);

+  rdivide(x_n_b, norm(wak), wak);

 

-  /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */

-  cross(m_n_b, z_n_b, x_n_b);

+  /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */

+  cross(wak, z_n_b, x_n_b);

 

-  /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */

+  /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */

   for (i = 0; i < 3; i++) {

     b_x_aposteriori_k[i] = x_n_b[i];

   }

 

   rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);

 

-  /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

+  /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

   for (i = 0; i < 3; i++) {

     Rot_matrix[i] = x_n_b[i];

-    Rot_matrix[3 + i] = m_n_b[i];

+    Rot_matrix[3 + i] = wak[i];

     Rot_matrix[6 + i] = z_n_b[i];

   }

 

-  /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */

+  /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

+  /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */

+  /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

+  /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */

   eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);

+  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

   eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

 }

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
index c93d7f4bb..afa63c1a9 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

@@ -29,6 +29,6 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

+extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

 #endif

 /* End of code generation (attitudeKalmanfilter.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
index 67b6fa422..7d620d7fa 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_initialize'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
index 610924d75..8b599be66 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_initialize'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
index c89602723..7f9727419 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_terminate'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
index a19630775..da84a5024 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_terminate'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
index eba83d0d9..30fd1e75e 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c
index 42cd70971..84ada9002 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ b/apps/attitude_estimator_ekf/codegen/cross.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'cross'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h
index c90d6b3a5..e727f0684 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ b/apps/attitude_estimator_ekf/codegen/cross.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'cross'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c
deleted file mode 100644
index 7c28e873a..000000000
--- a/apps/attitude_estimator_ekf/codegen/diag.c
+++ /dev/null
@@ -1,78 +0,0 @@
-/*

- * diag.c

- *

- * Code generation for function 'diag'

- *

- * C source code generated on: Tue Oct 16 15:27:58 2012

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "diag.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_diag(const real32_T v[9], real32_T d[81])

-{

-  int32_T j;

-  memset(&d[0], 0, 81U * sizeof(real32_T));

-  for (j = 0; j < 9; j++) {

-    d[j + 9 * j] = v[j];

-  }

-}

-

-/*

- *

- */

-void c_diag(const real32_T v[3], real32_T d[9])

-{

-  int32_T j;

-  for (j = 0; j < 9; j++) {

-    d[j] = 0.0F;

-  }

-

-  for (j = 0; j < 3; j++) {

-    d[j + 3 * j] = v[j];

-  }

-}

-

-/*

- *

- */

-void d_diag(const real32_T v[6], real32_T d[36])

-{

-  int32_T j;

-  memset(&d[0], 0, 36U * sizeof(real32_T));

-  for (j = 0; j < 6; j++) {

-    d[j + 6 * j] = v[j];

-  }

-}

-

-/*

- *

- */

-void diag(const real32_T v[12], real32_T d[144])

-{

-  int32_T j;

-  memset(&d[0], 0, 144U * sizeof(real32_T));

-  for (j = 0; j < 12; j++) {

-    d[j + 12 * j] = v[j];

-  }

-}

-

-/* End of code generation (diag.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h
deleted file mode 100644
index 98b411c2d..000000000
--- a/apps/attitude_estimator_ekf/codegen/diag.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*

- * diag.h

- *

- * Code generation for function 'diag'

- *

- * C source code generated on: Tue Oct 16 15:27:58 2012

- *

- */

-

-#ifndef __DIAG_H__

-#define __DIAG_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_diag(const real32_T v[9], real32_T d[81]);

-extern void c_diag(const real32_T v[3], real32_T d[9]);

-extern void d_diag(const real32_T v[6], real32_T d[36]);

-extern void diag(const real32_T v[12], real32_T d[144]);

-#endif

-/* End of code generation (diag.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c
index dbd44c6a8..b89ab58ef 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ b/apps/attitude_estimator_ekf/codegen/eye.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'eye'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h
index 325fd23ec..94fbe7671 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ b/apps/attitude_estimator_ekf/codegen/eye.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'eye'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c
index 0a540775a..a810f22e4 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'mrdivide'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h
index d286eda5a..2d3b0d51f 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'mrdivide'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c
index deb71dc60..0c418cc7b 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ b/apps/attitude_estimator_ekf/codegen/norm.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'norm'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h
index 3459cbdb5..60cf77b57 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ b/apps/attitude_estimator_ekf/codegen/norm.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'norm'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c
deleted file mode 100644
index fc602fb5c..000000000
--- a/apps/attitude_estimator_ekf/codegen/power.c
+++ /dev/null
@@ -1,84 +0,0 @@
-/*

- * power.c

- *

- * Code generation for function 'power'

- *

- * C source code generated on: Tue Oct 16 15:27:58 2012

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "power.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  real32_T f1;

-  real32_T f2;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else {

-    f1 = (real32_T)fabs(u0);

-    f2 = (real32_T)fabs(u1);

-    if (rtIsInfF(u1)) {

-      if (f1 == 1.0F) {

-        y = ((real32_T)rtNaN);

-      } else if (f1 > 1.0F) {

-        if (u1 > 0.0F) {

-          y = ((real32_T)rtInf);

-        } else {

-          y = 0.0F;

-        }

-      } else if (u1 > 0.0F) {

-        y = 0.0F;

-      } else {

-        y = ((real32_T)rtInf);

-      }

-    } else if (f2 == 0.0F) {

-      y = 1.0F;

-    } else if (f2 == 1.0F) {

-      if (u1 > 0.0F) {

-        y = u0;

-      } else {

-        y = 1.0F / u0;

-      }

-    } else if (u1 == 2.0F) {

-      y = u0 * u0;

-    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {

-      y = (real32_T)sqrt(u0);

-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {

-      y = ((real32_T)rtNaN);

-    } else {

-      y = (real32_T)pow(u0, u1);

-    }

-  }

-

-  return y;

-}

-

-/*

- *

- */

-void power(const real32_T a[12], real32_T y[12])

-{

-  int32_T k;

-  for (k = 0; k < 12; k++) {

-    y[k] = rt_powf_snf(a[k], 2.0F);

-  }

-}

-

-/* End of code generation (power.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h
deleted file mode 100644
index 40a0d9353..000000000
--- a/apps/attitude_estimator_ekf/codegen/power.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * power.h

- *

- * Code generation for function 'power'

- *

- * C source code generated on: Tue Oct 16 15:27:58 2012

- *

- */

-

-#ifndef __POWER_H__

-#define __POWER_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void power(const real32_T a[12], real32_T y[12]);

-#endif

-/* End of code generation (power.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c
index 11e4e6f1f..d035dae5e 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/rdivide.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'rdivide'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h
index 4e2430374..4bbebebe2 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/rdivide.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'rdivide'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
index d20fa13fc..34164d104 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
index 83a355873..145373cd0 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
index d357102af..d84ca9573 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
index 415927709..65fdaa96f 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h
index 608391608..356498363 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
index 69069562b..303d1d9d2 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
index 9686964ae..bd56b30d9 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
index 77fd3ec7a..9a5c96267 100644..100755
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Tue Oct 16 15:27:58 2012

+ * C source code generated on: Sat Jan 19 15:25:29 2013

  *

  */

 

@@ -26,8 +26,8 @@
  *   Number of bits:     char:   8    short:   16    int:  32

  *                       long:  32      native word size:  32

  *   Byte ordering: LittleEndian

- *   Signed integer division rounds to: Undefined

- *   Shift right on a signed integer as arithmetic shift: off

+ *   Signed integer division rounds to: Zero

+ *   Shift right on a signed integer as arithmetic shift: on

  *=======================================================================*/

 

 /*=======================================================================* 

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 3a6fecf74..6b1bc0f9b 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1678,12 +1678,13 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 
 			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph / 100.0f;
-			float vdop_m = gps_position.epv / 100.0f;
+			float hdop_m = gps_position.eph_m;
+			float vdop_m = gps_position.epv_m;
 
 			/* check if gps fix is ok */
 			// XXX magic number
-			float dop_threshold_m = 2.0f;
+			float hdop_threshold_m = 4.0f;
+			float vdop_threshold_m = 8.0f;
 
 			/*
 			 * If horizontal dilution of precision (hdop / eph)
@@ -1694,10 +1695,12 @@ int commander_thread_main(int argc, char *argv[])
 			 * the system is currently not armed, set home
 			 * position to the current position.
 			 */
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m)
-				&& (vdop_m < dop_threshold_m)
+
+			if (gps_position.fix_type == GPS_FIX_TYPE_3D
+				&& (hdop_m < hdop_threshold_m)
+				&& (vdop_m < vdop_threshold_m)
 				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp < 2000000)
+				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
 				&& !current_status.flag_system_armed) {
 				warnx("setting home position");
 
@@ -1706,11 +1709,11 @@ int commander_thread_main(int argc, char *argv[])
 				home.lon = gps_position.lon;
 				home.alt = gps_position.alt;
 
-				home.eph = gps_position.eph;
-				home.epv = gps_position.epv;
+				home.eph_m = gps_position.eph_m;
+				home.epv_m = gps_position.epv_m;
 
-				home.s_variance = gps_position.s_variance;
-				home.p_variance = gps_position.p_variance;
+				home.s_variance_m_s = gps_position.s_variance_m_s;
+				home.p_variance_m = gps_position.p_variance_m;
 
 				/* announce new home position */
 				if (home_pub > 0) {
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index 03da35a0a..d9637b4a7 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -322,11 +322,24 @@ void BlockMultiModeBacksideAutopilot::update()
 			_att.roll, _att.pitch, _att.yaw,
 			_att.rollspeed, _att.pitchspeed, _att.yawspeed
 		);
-		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
-		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_AIL] = - _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = - _backsideAutopilot.getElevator();
 		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
 		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
 
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
+
+		/* do not limit in HIL */
+		if (!_status.flag_hil_enabled) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							_actuators.control[CH_THR] : _manual.throttle;
+		}
+
+
 	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
 
 		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
@@ -337,11 +350,12 @@ void BlockMultiModeBacksideAutopilot::update()
 			_actuators.control[CH_THR] = _manual.throttle;
 
 		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-			_stabilization.update(
-			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
+
+			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
 			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
 			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = _stabilization.getElevator();
+			_actuators.control[CH_ELV] = - _stabilization.getElevator();
 			_actuators.control[CH_RDR] = _stabilization.getRudder();
 			_actuators.control[CH_THR] = _manual.throttle;
 		}
diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h
new file mode 100644
index 000000000..1dda8ef0b
--- /dev/null
+++ b/apps/drivers/drv_gps.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file GPS driver interface.
+ */
+
+#ifndef _DRV_GPS_H
+#define _DRV_GPS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+
+#define GPS_DEVICE_PATH	"/dev/gps"
+
+typedef enum {
+	GPS_DRIVER_MODE_NONE = 0,
+	GPS_DRIVER_MODE_UBX,
+	GPS_DRIVER_MODE_MTK,
+	GPS_DRIVER_MODE_NMEA,
+} gps_driver_mode_t;
+
+
+/*
+ * ObjDev tag for GPS data.
+ */
+ORB_DECLARE(gps);
+
+/*
+ * ioctl() definitions
+ */
+#define _GPSIOCBASE			(0x2800)            //TODO: arbitrary choice...
+#define _GPSIOC(_n)		(_IOC(_GPSIOCBASE, _n))
+
+#endif /* _DRV_GPS_H */
diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile
new file mode 100644
index 000000000..3859a88a5
--- /dev/null
+++ b/apps/drivers/gps/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# GPS driver
+#
+
+APPNAME		 = gps
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
new file mode 100644
index 000000000..6d7cfcc68
--- /dev/null
+++ b/apps/drivers/gps/gps.cpp
@@ -0,0 +1,536 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gps.cpp
+ * Driver for the GPS on a serial port
+ */
+
+#include <nuttx/clock.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/i2c.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/err.h>
+#include <drivers/drv_gps.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include "ubx.h"
+#include "mtk.h"
+
+
+#define TIMEOUT_5HZ 400
+#define RATE_MEASUREMENT_PERIOD 5000000
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+
+
+class GPS : public device::CDev
+{
+public:
+	GPS(const char* uart_path);
+	~GPS();
+
+	virtual int			init();
+
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void				print_info();
+
+private:
+
+	bool				_task_should_exit;				///< flag to make the main worker task exit
+	int				_serial_fd;					///< serial interface to GPS
+	unsigned			_baudrate;					///< current baudrate
+	char				_port[20];					///< device / serial port path
+	volatile int			_task;						//< worker task
+	bool				_healthy;					///< flag to signal if the GPS is ok
+	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
+	bool				_mode_changed;					///< flag that the GPS mode has changed
+	gps_driver_mode_t		_mode;						///< current mode
+	GPS_Helper			*_Helper;					///< instance of GPS parser
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
+	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	float				_rate;						///< position update rate
+
+
+	/**
+	 * Try to configure the GPS, handle outgoing communication to the GPS
+	 */
+	void				config();
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void			task_main_trampoline(void *arg);
+
+
+	/**
+	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
+	 */
+	void				task_main(void);
+
+	/**
+	 * Set the baudrate of the UART to the GPS
+	 */
+	int				set_baudrate(unsigned baud);
+
+	/**
+	 * Send a reset command to the GPS
+	 */
+	void				cmd_reset();
+
+};
+
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int gps_main(int argc, char *argv[]);
+
+namespace
+{
+
+GPS	*g_dev;
+
+}
+
+
+GPS::GPS(const char* uart_path) :
+	CDev("gps", GPS_DEVICE_PATH),
+	_task_should_exit(false),
+	_healthy(false),
+	_mode_changed(false),
+	_mode(GPS_DRIVER_MODE_UBX),
+	_Helper(nullptr),
+	_report_pub(-1),
+	_rate(0.0f)
+{
+	/* store port name */
+	strncpy(_port, uart_path, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+	memset(&_report, 0, sizeof(_report));
+
+	_debug_enabled = true;
+}
+
+GPS::~GPS()
+{
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
+
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
+	}
+
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
+	g_dev = nullptr;
+
+}
+
+int
+GPS::init()
+{
+	int ret = ERROR;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK)
+		goto out;
+
+	/* start the GPS driver worker task */
+	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+
+	if (_task < 0) {
+		warnx("task start failed: %d", errno);
+		return -errno;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	lock();
+
+	int ret = OK;
+
+	switch (cmd) {
+	case SENSORIOCRESET:
+		cmd_reset();
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+GPS::task_main_trampoline(void *arg)
+{
+	g_dev->task_main();
+}
+
+void
+GPS::task_main()
+{
+	log("starting");
+
+	/* open the serial port */
+	_serial_fd = ::open(_port, O_RDWR);
+
+	if (_serial_fd < 0) {
+		log("failed to open serial port: %s err: %d", _port, errno);
+		/* tell the dtor that we are exiting, set error code */
+		_task = -1;
+		_exit(1);
+	}
+
+	uint64_t last_rate_measurement = hrt_absolute_time();
+	unsigned last_rate_count = 0;
+
+	/* loop handling received serial bytes and also configuring in between */
+	while (!_task_should_exit) {
+
+		if (_Helper != nullptr) {
+			delete(_Helper);
+			/* set to zero to ensure parser is not used while not instantiated */
+			_Helper = nullptr;
+		}
+
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_Helper = new UBX(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_Helper = new MTK(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_NMEA:
+				//_Helper = new NMEA(); //TODO: add NMEA
+				break;
+			default:
+				break;
+		}
+		unlock();
+		if (_Helper->configure(_baudrate) == 0) {
+			unlock();
+			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+//				lock();
+				/* opportunistic publishing - else invalid data would end up on the bus */
+				if (_report_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				} else {
+					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+				}
+
+				last_rate_count++;
+
+				/* measure update rate every 5 seconds */
+				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
+					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+					last_rate_measurement = hrt_absolute_time();
+					last_rate_count = 0;
+				}
+
+				if (!_healthy) {
+					warnx("module found");
+					_healthy = true;
+				}
+			}
+			if (_healthy) {
+				warnx("module lost");
+				_healthy = false;
+				_rate = 0.0f;
+			}
+
+			lock();
+		}
+		lock();
+
+		/* select next mode */
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_mode = GPS_DRIVER_MODE_MTK;
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_UBX;
+				break;
+		//				case GPS_DRIVER_MODE_NMEA:
+		//					_mode = GPS_DRIVER_MODE_UBX;
+		//					break;
+			default:
+				break;
+		}
+
+	}
+	debug("exiting");
+
+	::close(_serial_fd);
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+
+void
+GPS::cmd_reset()
+{
+	//XXX add reset?
+}
+
+void
+GPS::print_info()
+{
+	switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;
+		case GPS_DRIVER_MODE_NMEA:
+			warnx("protocol: NMEA");
+			break;
+		default:
+			break;
+	}
+	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+	if (_report.timestamp_position != 0) {
+		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
+			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("update rate: %6.2f Hz", (double)_rate);
+	}
+
+	usleep(100000);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace gps
+{
+
+GPS	*g_dev;
+
+void	start(const char *uart_path);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *uart_path)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new GPS(uart_path);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+		goto fail;
+	}
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver.
+ */
+void
+stop()
+{
+	delete g_dev;
+	g_dev = nullptr;
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	exit(0);
+}
+
+/**
+ * Print the status of the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+
+int
+gps_main(int argc, char *argv[])
+{
+
+	/* set to default */
+	char* device_name = GPS_DEFAULT_UART_PORT;
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		/* work around getopt unreliability */
+		if (argc > 3) {
+			if (!strcmp(argv[2], "-d")) {
+				device_name = argv[3];
+			} else {
+				goto out;
+			}
+		}
+		gps::start(device_name);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+		gps::stop();
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		gps::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		gps::reset();
+
+	/*
+	 * Print driver status.
+	 */
+	if (!strcmp(argv[1], "status"))
+		gps::info();
+
+out:
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+}
diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp
new file mode 100644
index 000000000..9c1fad569
--- /dev/null
+++ b/apps/drivers/gps/gps_helper.cpp
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <termios.h>
+#include <errno.h>
+#include <systemlib/err.h>
+#include "gps_helper.h"
+
+/* @file gps_helper.cpp */
+
+int
+GPS_Helper::set_baudrate(const int &fd, unsigned baud)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	warnx("try baudrate: %d\n", speed);
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n", baud);
+		return -EINVAL;
+	}
+	struct termios uart_config;
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERROR setting baudrate (tcsetattr)\n");
+		return -1;
+	}
+	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
+	return 0;
+}
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
new file mode 100644
index 000000000..f3d3bc40b
--- /dev/null
+++ b/apps/drivers/gps/gps_helper.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file gps_helper.h */
+
+#ifndef GPS_HELPER_H
+#define GPS_HELPER_H
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+class GPS_Helper
+{
+public:
+	virtual int				configure(unsigned &baud) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
+	int 					set_baudrate(const int &fd, unsigned baud);
+};
+
+#endif /* GPS_HELPER_H */
diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp
new file mode 100644
index 000000000..4762bd503
--- /dev/null
+++ b/apps/drivers/gps/mtk.cpp
@@ -0,0 +1,274 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file mkt.cpp */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "mtk.h"
+
+
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_mtk_revision(0)
+{
+	decode_init();
+}
+
+MTK::~MTK()
+{
+}
+
+int
+MTK::configure(unsigned &baudrate)
+{
+	/* set baudrate first */
+	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
+		return -1;
+
+	baudrate = MTK_BAUDRATE;
+
+	/* Write config messages, don't wait for an answer */
+	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int
+MTK::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+	gps_mtk_packet_t packet;
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* first read whatever is left */
+		if (j < count) {
+			/* pass received bytes to the packet decoder */
+			while (j < count) {
+				if (parse_char(buf[j], packet) > 0) {
+					handle_message(packet);
+					return 1;
+				}
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+					return -1;
+				}
+				j++;
+			}
+			/* everything is read */
+			j = count = 0;
+		}
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+void
+MTK::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = MTK_DECODE_UNINIT;
+}
+int
+MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
+{
+	int ret = 0;
+
+	if (_decode_state == MTK_DECODE_UNINIT) {
+
+		if (b == MTK_SYNC1_V16) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 16;
+		} else if (b == MTK_SYNC1_V19) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 19;
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
+		if (b == MTK_SYNC2) {
+			_decode_state = MTK_DECODE_GOT_CK_B;
+
+		} else {
+			// Second start symbol was wrong, reset state machine
+			decode_init();
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
+		// Add to checksum
+		if (_rx_count < 33)
+			add_byte_to_checksum(b);
+
+		// Fill packet buffer
+		((uint8_t*)(&packet))[_rx_count] = b;
+		_rx_count++;
+
+		/* Packet size minus checksum, XXX ? */
+		if (_rx_count >= sizeof(packet)) {
+			/* Compare checksum */
+			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
+				ret = 1;
+			} else {
+				warnx("MTK Checksum invalid");
+				ret = -1;
+			}
+			// Reset state machine to decode next packet
+			decode_init();
+		}
+	}
+	return ret;
+}
+
+void
+MTK::handle_message(gps_mtk_packet_t &packet)
+{
+	if (_mtk_revision == 16) {
+		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
+		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+	} else if (_mtk_revision == 19) {
+		_gps_position->lat = packet.latitude; // both degrees*1e7
+		_gps_position->lon = packet.longitude; // both degrees*1e7
+	} else {
+		warnx("mtk: unknown revision");
+		_gps_position->lat = 0;
+		_gps_position->lon = 0;
+	}
+	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+	_gps_position->fix_type = packet.fix_type;
+	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
+	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
+	_gps_position->satellites_visible = packet.satellites;
+
+	/* convert time and date information to unix timestamp */
+	struct tm timeinfo; //TODO: test this conversion
+	uint32_t timeinfo_conversion_temp;
+
+	timeinfo.tm_mday = packet.date * 1e-4;
+	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
+	timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
+	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
+
+	timeinfo.tm_hour = packet.utc_time * 1e-7;
+	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
+	timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
+	timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
+	timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
+	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
+	time_t epoch = mktime(&timeinfo);
+
+	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
+
+	return;
+}
+
+void
+MTK::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}
diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h
new file mode 100644
index 000000000..d4e390b01
--- /dev/null
+++ b/apps/drivers/gps/mtk.h
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file mtk.h */
+
+#ifndef MTK_H_
+#define MTK_H_
+
+#include "gps_helper.h"
+
+#define MTK_SYNC1_V16 0xd0
+#define MTK_SYNC1_V19 0xd1
+#define MTK_SYNC2 0xdd
+
+#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
+#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define SBAS_ON	        	"$PMTK313,1*2E\r\n"
+#define WAAS_ON				"$PMTK301,2*2E\r\n"
+#define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
+
+#define MTK_TIMEOUT_5HZ 400
+#define MTK_BAUDRATE 38400
+
+typedef enum {
+	MTK_DECODE_UNINIT = 0,
+	MTK_DECODE_GOT_CK_A = 1,
+	MTK_DECODE_GOT_CK_B = 2
+} mtk_decode_state_t;
+
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint8_t payload; ///< Number of payload bytes
+	int32_t latitude;  ///< Latitude in degrees * 10^7
+	int32_t longitude; ///< Longitude in degrees * 10^7
+	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
+	uint32_t ground_speed; ///< velocity in m/s
+	int32_t heading; ///< heading in degrees * 10^2
+	uint8_t satellites; ///< number of sattelites used
+	uint8_t fix_type;  ///< fix type: XXX correct for that
+	uint32_t date;
+	uint32_t utc_time;
+	uint16_t hdop; ///< horizontal dilution of position (without unit)
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_mtk_packet_t;
+
+#pragma pack(pop)
+
+#define MTK_RECV_BUFFER_SIZE 40
+
+class MTK : public GPS_Helper
+{
+public:
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~MTK();
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
+
+private:
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b, gps_mtk_packet_t &packet);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	void				handle_message(gps_mtk_packet_t &packet);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void				add_byte_to_checksum(uint8_t);
+
+	int					_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	mtk_decode_state_t	_decode_state;
+	uint8_t				_mtk_revision;
+	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+};
+
+#endif /* MTK_H_ */
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
new file mode 100644
index 000000000..74cbc5aaf
--- /dev/null
+++ b/apps/drivers/gps/ubx.cpp
@@ -0,0 +1,745 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file U-Blox protocol implementation */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <drivers/drv_hrt.h>
+
+#include "ubx.h"
+
+#define UBX_CONFIG_TIMEOUT 100
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_waiting_for_ack(false)
+{
+	decode_init();
+}
+
+UBX::~UBX()
+{
+}
+
+int
+UBX::configure(unsigned &baudrate)
+{
+	_waiting_for_ack = true;
+
+	/* try different baudrates */
+	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+	for (int baud_i = 0; baud_i < 5; baud_i++) {
+		baudrate = baudrates_to_try[baud_i];
+		set_baudrate(_fd, baudrate);
+
+		/* Send a CFG-PRT message to set the UBX protocol for in and out
+		 * and leave the baudrate as it is, we just want an ACK-ACK from this
+		 */
+		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
+		/* Set everything else of the packet to 0, otherwise the module wont accept it */
+		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+
+		/* Define the package contents, don't change the baudrate */
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= baudrate;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* Send a CFG-PRT message again, this time change the baudrate */
+
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
+			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		}
+
+		/* no ack is ecpected here, keep going configuring */
+
+		/* send a CFT-RATE message to define update rate */
+		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
+		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+
+		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
+		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
+		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
+		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* send a NAV5 message to set the options for the internal filter */
+		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
+		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+
+		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
+		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
+		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
+		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
+		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
+		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
+		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_MSG;
+
+		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
+		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
+		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
+		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
+		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
+		/* For satelites info 1Hz is enough */
+		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
+
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+
+		_waiting_for_ack = false;
+		return 0;
+	}
+	return -1;
+}
+
+int
+UBX::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* pass received bytes to the packet decoder */
+		while (j < count) {
+			if (parse_char(buf[j]) > 0) {
+				/* return to configure during configuration or to the gps driver during normal work
+				 * if a packet has arrived */
+				 if (handle_message() > 0)
+					return 1;
+			}
+			/* in case we keep trying but only get crap from GPS */
+			if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				return -1;
+			}
+			j++;
+		}
+
+		/* everything is read */
+		j = count = 0;
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
+		}
+	}
+}
+
+int
+UBX::parse_char(uint8_t b)
+{
+	switch (_decode_state) {
+		/* First, look for sync1 */
+		case UBX_DECODE_UNINIT:
+			if (b == UBX_SYNC1) {
+				_decode_state = UBX_DECODE_GOT_SYNC1;
+			}
+			break;
+		/* Second, look for sync2 */
+		case UBX_DECODE_GOT_SYNC1:
+			if (b == UBX_SYNC2) {
+				_decode_state = UBX_DECODE_GOT_SYNC2;
+			} else {
+				/* Second start symbol was wrong, reset state machine */
+				decode_init();
+			}
+			break;
+		/* Now look for class */
+		case UBX_DECODE_GOT_SYNC2:
+			/* everything except sync1 and sync2 needs to be added to the checksum */
+			add_byte_to_checksum(b);
+			/* check for known class */
+			switch (b) {
+				case UBX_CLASS_ACK:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = ACK;
+					break;
+
+				case UBX_CLASS_NAV:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = NAV;
+					break;
+
+//				case UBX_CLASS_RXM:
+//					_decode_state = UBX_DECODE_GOT_CLASS;
+//					_message_class = RXM;
+//					break;
+
+				case UBX_CLASS_CFG:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = CFG;
+					break;
+				default: //unknown class: reset state machine
+					decode_init();
+					break;
+			}
+			break;
+		case UBX_DECODE_GOT_CLASS:
+			add_byte_to_checksum(b);
+			switch (_message_class) {
+				case NAV:
+					switch (b) {
+					case UBX_MESSAGE_NAV_POSLLH:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_POSLLH;
+						break;
+
+					case UBX_MESSAGE_NAV_SOL:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SOL;
+						break;
+
+					case UBX_MESSAGE_NAV_TIMEUTC:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_TIMEUTC;
+						break;
+
+//					case UBX_MESSAGE_NAV_DOP:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = NAV_DOP;
+//						break;
+
+					case UBX_MESSAGE_NAV_SVINFO:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SVINFO;
+						break;
+
+					case UBX_MESSAGE_NAV_VELNED:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_VELNED;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+//				case RXM:
+//					switch (b) {
+//					case UBX_MESSAGE_RXM_SVSI:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = RXM_SVSI;
+//						break;
+//
+//					default: //unknown class: reset state machine, should not happen
+//						decode_init();
+//						break;
+//					}
+//					break;
+
+				case CFG:
+					switch (b) {
+					case UBX_MESSAGE_CFG_NAV5:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = CFG_NAV5;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+
+				case ACK:
+					switch (b) {
+					case UBX_MESSAGE_ACK_ACK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_ACK;
+						break;
+					case UBX_MESSAGE_ACK_NAK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_NAK;
+						break;
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+				default: //should not happen because we set the class
+					warnx("UBX Error, we set a class that we don't know");
+					decode_init();
+//					config_needed = true;
+					break;
+				}
+				break;
+			case UBX_DECODE_GOT_MESSAGEID:
+				add_byte_to_checksum(b);
+				_payload_size = b; //this is the first length byte
+				_decode_state = UBX_DECODE_GOT_LENGTH1;
+				break;
+			case UBX_DECODE_GOT_LENGTH1:
+				add_byte_to_checksum(b);
+				_payload_size += b << 8; // here comes the second byte of length
+				_decode_state = UBX_DECODE_GOT_LENGTH2;
+				break;
+			case UBX_DECODE_GOT_LENGTH2:
+				/* Add to checksum if not yet at checksum byte */
+				if (_rx_count < _payload_size)
+					add_byte_to_checksum(b);
+					_rx_buffer[_rx_count] = b;
+					/* once the payload has arrived, we can process the information */
+					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
+
+						/* compare checksum */
+						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+							return 1;
+						} else {
+							decode_init();
+							return -1;
+							warnx("ubx: Checksum wrong");
+						}
+
+						return 1;
+					} else if (_rx_count < RECV_BUFFER_SIZE) {
+						_rx_count++;
+					} else {
+						warnx("ubx: buffer full");
+						decode_init();
+						return -1;
+					}
+				break;
+		default:
+			break;
+	}
+	return 0; //XXX ?
+}
+
+
+int
+UBX::handle_message()
+{
+	int ret = 0;
+
+	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
+		case NAV_POSLLH: {
+//			printf("GOT NAV_POSLLH MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
+
+				_gps_position->lat = packet->lat;
+				_gps_position->lon = packet->lon;
+				_gps_position->alt = packet->height_msl;
+
+				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
+				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+
+				/* Add timestamp to finish the report */
+				_gps_position->timestamp_position = hrt_absolute_time();
+				/* only return 1 when new position is available */
+				ret = 1;
+			}
+			break;
+		}
+
+		case NAV_SOL: {
+//			printf("GOT NAV_SOL MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+
+				_gps_position->fix_type = packet->gpsFix;
+				_gps_position->s_variance_m_s = packet->sAcc;
+				_gps_position->p_variance_m = packet->pAcc;
+
+				_gps_position->timestamp_variance = hrt_absolute_time();
+			}
+			break;
+		}
+
+//		case NAV_DOP: {
+////		printf("GOT NAV_DOP MESSAGE\n");
+//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
+//
+//			_gps_position->eph_m =  packet->hDOP;
+//			_gps_position->epv =  packet->vDOP;
+//
+//			_gps_position->timestamp_posdilution = hrt_absolute_time();
+//
+//			_new_nav_dop = true;
+//
+//			break;
+//		}
+
+		case NAV_TIMEUTC: {
+//			printf("GOT NAV_TIMEUTC MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+
+				//convert to unix timestamp
+				struct tm timeinfo;
+				timeinfo.tm_year = packet->year - 1900;
+				timeinfo.tm_mon = packet->month - 1;
+				timeinfo.tm_mday = packet->day;
+				timeinfo.tm_hour = packet->hour;
+				timeinfo.tm_min = packet->min;
+				timeinfo.tm_sec = packet->sec;
+
+				time_t epoch = mktime(&timeinfo);
+
+				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+
+				_gps_position->timestamp_time = hrt_absolute_time();
+			}
+			break;
+		}
+
+		case NAV_SVINFO: {
+//			printf("GOT NAV_SVINFO MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
+				const int length_part1 = 8;
+				char _rx_buffer_part1[length_part1];
+				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
+
+				//read checksum
+				const int length_part3 = 2;
+				char _rx_buffer_part3[length_part3];
+				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+
+				//definitions needed to read numCh elements from the buffer:
+				const int length_part2 = 12;
+				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+				char _rx_buffer_part2[length_part2]; //for temporal storage
+
+				uint8_t satellites_used = 0;
+				int i;
+
+				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+
+					/* Get satellite information from the buffer */
+					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
+					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
+
+
+					/* Write satellite information in the global storage */
+					_gps_position->satellite_prn[i] = packet_part2->svid;
+
+					//if satellite information is healthy store the data
+					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+
+					if (!unhealthy) {
+						if ((packet_part2->flags) & 1) { //flags is a bitfield
+							_gps_position->satellite_used[i] = 1;
+							satellites_used++;
+
+						} else {
+							_gps_position->satellite_used[i] = 0;
+						}
+
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+
+					} else {
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+				}
+
+				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
+					/* Unused channels have to be set to zero for e.g. MAVLink */
+					_gps_position->satellite_prn[i] = 0;
+					_gps_position->satellite_used[i] = 0;
+					_gps_position->satellite_snr[i] = 0;
+					_gps_position->satellite_elevation[i] = 0;
+					_gps_position->satellite_azimuth[i] = 0;
+				}
+				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+				/* set timestamp if any sat info is available */
+				if (packet_part1->numCh > 0) {
+					_gps_position->satellite_info_available = true;
+				} else {
+					_gps_position->satellite_info_available = false;
+				}
+				_gps_position->timestamp_satellites = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+		case NAV_VELNED: {
+//			printf("GOT NAV_VELNED MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+
+				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
+				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->vel_ned_valid = true;
+				_gps_position->timestamp_velocity = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+//		case RXM_SVSI: {
+//				printf("GOT RXM_SVSI MESSAGE\n");
+//				const int length_part1 = 7;
+//				char _rx_buffer_part1[length_part1];
+//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
+//
+//				_gps_position->satellites_visible = packet->numVis;
+//				_gps_position->counter++;
+//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
+//
+//			break;
+//		}
+		case ACK_ACK: {
+//			printf("GOT ACK_ACK\n");
+			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
+
+			if (_waiting_for_ack) {
+				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
+					ret = 1;
+				}
+			}
+		}
+		break;
+
+		case ACK_NAK: {
+//			printf("GOT ACK_NAK\n");
+			warnx("UBX: Received: Not Acknowledged");
+			/* configuration obviously not successful */
+			ret = -1;
+			break;
+		}
+
+		default: //we don't know the message
+			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			ret = -1;
+			break;
+		}
+	// end if _rx_count high enough
+	decode_init();
+	return ret; //XXX?
+}
+
+void
+UBX::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = UBX_DECODE_UNINIT;
+	_message_class = CLASS_UNKNOWN;
+	_message_id = ID_UNKNOWN;
+	_payload_size = 0;
+}
+
+void
+UBX::add_byte_to_checksum(uint8_t b)
+{
+	_rx_ck_a = _rx_ck_a + b;
+	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+}
+
+void
+UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
+{
+	uint8_t ck_a = 0;
+	uint8_t ck_b = 0;
+	unsigned i;
+
+	for (i = 0; i < length-2; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+	/* The checksum is written to the last to bytes of a message */
+	message[length-2] = ck_a;
+	message[length-1] = ck_b;
+}
+
+void
+UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+{
+	ssize_t ret = 0;
+
+	/* Calculate the checksum now */
+	add_checksum_to_message(packet, length);
+
+	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+
+	/* Start with the two sync bytes */
+	ret += write(fd, sync_bytes, sizeof(sync_bytes));
+	ret += write(fd, packet, length);
+
+	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
+		warnx("ubx: config write fail");
+}
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
new file mode 100644
index 000000000..e3dd1c7ea
--- /dev/null
+++ b/apps/drivers/gps/ubx.h
@@ -0,0 +1,395 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file U-Blox protocol definitions */
+
+#ifndef UBX_H_
+#define UBX_H_
+
+#include "gps_helper.h"
+
+#define UBX_SYNC1 0xB5
+#define UBX_SYNC2 0x62
+
+/* ClassIDs (the ones that are used) */
+#define UBX_CLASS_NAV 0x01
+//#define UBX_CLASS_RXM 0x02
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+
+/* MessageIDs (the ones that are used) */
+#define UBX_MESSAGE_NAV_POSLLH 0x02
+#define UBX_MESSAGE_NAV_SOL 0x06
+#define UBX_MESSAGE_NAV_TIMEUTC 0x21
+//#define UBX_MESSAGE_NAV_DOP 0x04
+#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_VELNED 0x12
+//#define UBX_MESSAGE_RXM_SVSI 0x20
+#define UBX_MESSAGE_ACK_ACK 0x01
+#define UBX_MESSAGE_ACK_NAK 0x00
+#define UBX_MESSAGE_CFG_PRT 0x00
+#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_CFG_MSG 0x01
+#define UBX_MESSAGE_CFG_RATE 0x08
+
+#define UBX_CFG_PRT_LENGTH 20
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01         /**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0     /**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400      /**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01    /**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01   /**< UBX out */
+
+#define UBX_CFG_RATE_LENGTH 6
+#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
+#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
+
+
+#define UBX_CFG_NAV5_LENGTH 36
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001		/**< only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2          /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+
+#define UBX_CFG_MSG_LENGTH 8
+#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
+// ************
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
+	int32_t lon;					/**<  Longitude * 1e-7, deg */
+	int32_t lat;					/**<  Latitude * 1e-7, deg */
+	int32_t height;					/**<  Height above Ellipsoid, mm */
+	int32_t height_msl;				/**<  Height above mean sea level, mm */
+	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
+	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_posllh_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
+	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
+	int16_t week;					/**< GPS week (GPS time) */
+	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+	uint8_t flags;
+	int32_t ecefX;
+	int32_t ecefY;
+	int32_t ecefZ;
+	uint32_t pAcc;
+	int32_t ecefVX;
+	int32_t ecefVY;
+	int32_t ecefVZ;
+	uint32_t sAcc;
+	uint16_t pDOP;
+	uint8_t reserved1;
+	uint8_t numSV;
+	uint32_t reserved2;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_sol_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
+	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
+	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
+	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
+	uint8_t month; 					/**< Month, range 1..12 (UTC) */
+	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
+	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
+	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
+	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
+	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_timeutc_packet_t;
+
+//typedef struct {
+//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
+//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
+//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
+//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
+//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
+//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
+//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
+//	uint8_t ck_a;
+//	uint8_t ck_b;
+//} gps_bin_nav_dop_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+	uint8_t numCh; 					/**< Number of channels */
+	uint8_t globalFlags;
+	uint16_t reserved2;
+
+} gps_bin_nav_svinfo_part1_packet_t;
+
+typedef struct {
+	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
+	uint8_t svid; 					/**< Satellite ID */
+	uint8_t flags;
+	uint8_t quality;
+	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
+	int8_t elev; 					/**< Elevation in integer degrees */
+	int16_t azim; 					/**< Azimuth in integer degrees */
+	int32_t prRes; 					/**< Pseudo range residual in centimetres */
+
+} gps_bin_nav_svinfo_part2_packet_t;
+
+typedef struct {
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_svinfo_part3_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; // GPS Millisecond Time of Week
+	int32_t velN; //NED north velocity, cm/s
+	int32_t velE; //NED east velocity, cm/s
+	int32_t velD; //NED down velocity, cm/s
+	uint32_t speed; //Speed (3-D), cm/s
+	uint32_t gSpeed; //Ground Speed (2-D), cm/s
+	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
+	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
+	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_velned_packet_t;
+
+//typedef struct {
+//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
+//	int16_t week; 					/**< Measurement GPS week number */
+//	uint8_t numVis;					/**< Number of visible satellites */
+//
+//	//... rest of package is not used in this implementation
+//
+//} gps_bin_rxm_svsi_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_ack_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_nak_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t portID;
+	uint8_t res0;
+	uint16_t res1;
+	uint32_t mode;
+	uint32_t baudRate;
+	uint16_t inProtoMask;
+	uint16_t outProtoMask;
+	uint16_t flags;
+	uint16_t pad;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_prt_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t measRate;
+	uint16_t navRate;
+	uint16_t timeRef;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_rate_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t mask;
+	uint8_t dynModel;
+	uint8_t fixMode;
+	int32_t fixedAlt;
+	uint32_t fixedAltVar;
+	int8_t minElev;
+	uint8_t drLimit;
+	uint16_t pDop;
+	uint16_t tDop;
+	uint16_t pAcc;
+	uint16_t tAcc;
+	uint8_t staticHoldThresh;
+	uint8_t dgpsTimeOut;
+	uint32_t reserved2;
+	uint32_t reserved3;
+	uint32_t reserved4;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_nav5_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t msgClass_payload;
+	uint8_t msgID_payload;
+	uint8_t rate[6];
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_msg_packet_t;
+
+
+// END the structures of the binary packets
+// ************
+
+typedef enum {
+	UBX_CONFIG_STATE_PRT = 0,
+	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
+	UBX_CONFIG_STATE_RATE,
+	UBX_CONFIG_STATE_NAV5,
+	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
+	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
+	UBX_CONFIG_STATE_MSG_NAV_DOP,
+	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
+	UBX_CONFIG_STATE_MSG_NAV_SOL,
+	UBX_CONFIG_STATE_MSG_NAV_VELNED,
+//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
+	UBX_CONFIG_STATE_CONFIGURED
+} ubx_config_state_t;
+
+typedef enum {
+	CLASS_UNKNOWN = 0,
+	NAV = 1,
+	RXM = 2,
+	ACK = 3,
+	CFG = 4
+} ubx_message_class_t;
+
+typedef enum {
+	//these numbers do NOT correspond to the message id numbers of the ubx protocol
+	ID_UNKNOWN = 0,
+	NAV_POSLLH,
+	NAV_SOL,
+	NAV_TIMEUTC,
+//	NAV_DOP,
+	NAV_SVINFO,
+	NAV_VELNED,
+//	RXM_SVSI,
+	CFG_NAV5,
+	ACK_ACK,
+	ACK_NAK,
+} ubx_message_id_t;
+
+typedef enum {
+	UBX_DECODE_UNINIT = 0,
+	UBX_DECODE_GOT_SYNC1,
+	UBX_DECODE_GOT_SYNC2,
+	UBX_DECODE_GOT_CLASS,
+	UBX_DECODE_GOT_MESSAGEID,
+	UBX_DECODE_GOT_LENGTH1,
+	UBX_DECODE_GOT_LENGTH2
+} ubx_decode_state_t;
+
+//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
+#pragma pack(pop)
+
+#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+
+class UBX : public GPS_Helper
+{
+public:
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~UBX();
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
+
+private:
+
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	int					handle_message(void);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void				add_byte_to_checksum(uint8_t);
+
+	/**
+	 * Add the two checksum bytes to an outgoing message
+	 */
+	void				add_checksum_to_message(uint8_t* message, const unsigned length);
+
+	/**
+	 * Helper to send a config packet
+	 */
+	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+	int 				_fd;
+	struct vehicle_gps_position_s *_gps_position;
+	ubx_config_state_t	_config_state;
+	bool 				_waiting_for_ack;
+	uint8_t				_clsID_needed;
+	uint8_t				_msgID_needed;
+	ubx_decode_state_t	_decode_state;
+	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+	ubx_message_class_t _message_class;
+	ubx_message_id_t	_message_id;
+	unsigned			_payload_size;
+};
+
+#endif /* UBX_H_ */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 55b7cfa85..27e200e40 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -267,6 +267,11 @@ private:
 	 */
 	 int 			self_test();
 
+	/*
+	  set low pass filter frequency
+	 */
+	void _set_dlpf_filter(uint16_t frequency_hz);
+
 };
 
 /**
@@ -379,7 +384,7 @@ MPU6000::init()
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ);
+	_set_dlpf_filter(20);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -418,6 +423,9 @@ MPU6000::init()
 	case MPU6000_REV_D8:
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
+	// default case to cope with new chip revisions, which
+	// presumably won't have the accel scaling bug		
+	default:
 		// Accel scale 8g (4096 LSB/g)
 		write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
 		break;
@@ -485,6 +493,37 @@ MPU6000::probe()
 	return -EIO;
 }
 
+/*
+  set the DLPF filter frequency. This affects both accel and gyro.
+ */
+void
+MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
+{
+	uint8_t filter;
+
+	/* 
+	   choose next highest filter frequency available
+	 */
+	if (frequency_hz <= 5) {
+		filter = BITS_DLPF_CFG_5HZ;
+	} else if (frequency_hz <= 10) {
+		filter = BITS_DLPF_CFG_10HZ;
+	} else if (frequency_hz <= 20) {
+		filter = BITS_DLPF_CFG_20HZ;
+	} else if (frequency_hz <= 42) {
+		filter = BITS_DLPF_CFG_42HZ;
+	} else if (frequency_hz <= 98) {
+		filter = BITS_DLPF_CFG_98HZ;
+	} else if (frequency_hz <= 188) {
+		filter = BITS_DLPF_CFG_188HZ;
+	} else if (frequency_hz <= 256) {
+		filter = BITS_DLPF_CFG_256HZ_NOLPF2;
+	} else {
+		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
+	}
+	write_reg(MPUREG_CONFIG, filter);
+}
+
 ssize_t
 MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -610,8 +649,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
-		/* XXX not implemented */
-		return -EINVAL;
+		_set_dlpf_filter((uint16_t)arg);
+		return OK;
 
 	case ACCELIOCSSCALE:
 		{
@@ -668,8 +707,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
-		/* XXX not implemented */
-		return -EINVAL;
+		_set_dlpf_filter((uint16_t)arg);
+		return OK;
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 7e89dffb2..db851221b 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -190,11 +190,12 @@ void KalmanNav::update()
 	if (!_positionInitialized &&
 	    _attitudeInitialized && // wait for attitude first
 	    gpsUpdate &&
-	    _gps.fix_type > 2 &&
-	    _gps.counter_pos_valid > 10) {
-		vN = _gps.vel_n;
-		vE = _gps.vel_e;
-		vD = _gps.vel_d;
+	    _gps.fix_type > 2
+	    //&& _gps.counter_pos_valid > 10
+	    ) {
+		vN = _gps.vel_n_m_s;
+		vE = _gps.vel_e_m_s;
+		vD = _gps.vel_d_m_s;
 		setLatDegE7(_gps.lat);
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
@@ -259,7 +260,7 @@ void KalmanNav::updatePublications()
 
 	// position publication
 	_pos.timestamp = _pubTimeStamp;
-	_pos.time_gps_usec = _gps.timestamp;
+	_pos.time_gps_usec = _gps.timestamp_position;
 	_pos.valid = true;
 	_pos.lat = getLatDegE7();
 	_pos.lon = getLonDegE7();
@@ -290,7 +291,6 @@ void KalmanNav::updatePublications()
 
 	_att.R_valid = true;
 	_att.q_valid = true;
-	_att.counter = _navFrames;
 
 	// selectively update publications,
 	// do NOT call superblock do-all method
@@ -631,8 +631,8 @@ int KalmanNav::correctPos()
 
 	// residual
 	Vector y(5);
-	y(0) = _gps.vel_n - vN;
-	y(1) = _gps.vel_e - vE;
+	y(0) = _gps.vel_n_m_s - vN;
+	y(1) = _gps.vel_e_m_s - vE;
 	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
 	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
 	y(4) = double(_gps.alt) / 1.0e3 - alt;
@@ -651,9 +651,9 @@ int KalmanNav::correctPos()
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in gps correction\n");
 			// fallback to GPS
-			vN = _gps.vel_n;
-			vE = _gps.vel_e;
-			vD = _gps.vel_d;
+			vN = _gps.vel_n_m_s;
+			vE = _gps.vel_e_m_s;
+			vD = _gps.vel_d_m_s;
 			setLatDegE7(_gps.lat);
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
index 87b6e925c..ba5b593e2 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -196,9 +196,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	/* roll rate (PI) */
 	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
 	/* pitch rate (PI) */
-	actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	/* set pitch minus feedforward throttle compensation (nose pitches up from throttle */
-	actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust;
+	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
 	/* yaw rate (PI) */
 	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index ceb7c2554..b958d5f96 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[])
 		/* 20 Hz / 50 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
-		/* 2 Hz */
+		/* 10 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
+		/* 10 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
 
 	} else if (baudrate >= 115200) {
@@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
-		/* 5 Hz / 100 ms */
+		/* 5 Hz / 200 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+		/* 5 Hz / 200 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 
@@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
+		/* 2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
 
 	} else {
 		/* very low baud rate, limit to 1 Hz / 1000 ms */
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 86732d07c..4d9cd964d 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg)
 			static uint64_t old_timestamp = 0;
 
 			/* gps */
-			hil_gps.timestamp = gps.time_usec;
-			hil_gps.counter = hil_counter++;
+			hil_gps.timestamp_position = gps.time_usec;
+//			hil_gps.counter = hil_counter++;
 			hil_gps.time_gps_usec = gps.time_usec;
 			hil_gps.lat = gps.lat;
 			hil_gps.lon = gps.lon;
 			hil_gps.alt = gps.alt;
-			hil_gps.counter_pos_valid = hil_counter++;
-			hil_gps.eph = gps.eph;
-			hil_gps.epv = gps.epv;
-			hil_gps.s_variance = 100;
-			hil_gps.p_variance = 100;
-			hil_gps.vel = gps.vel;
-			hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
-			hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
-			hil_gps.vel_d = 0.0f;
-			hil_gps.cog = gps.cog;
+//			hil_gps.counter_pos_valid = hil_counter++;
+			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
+			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+			hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
+			hil_gps.p_variance_m = 100; // XXX 100 m variance?
+			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
+			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
+			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
+			hil_gps.vel_d_m_s = 0.0f;
+			hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
 			hil_gps.fix_type = gps.fix_type;
 			hil_gps.satellites_visible = gps.satellites_visible;
 
@@ -511,7 +511,6 @@ handle_message(mavlink_message_t *msg)
 			hil_attitude.yawspeed = hil_state.yawspeed;
 
 			/* set timestamp and notify processes (broadcast) */
-			hil_attitude.counter++;
 			hil_attitude.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
 		}
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index ec5149745..9f85d5801 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l)
 
 	/* GPS position */
 	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
-				     gps.timestamp,
+				     gps.timestamp_position,
 				     gps.fix_type,
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     gps.eph,
-				     gps.epv,
-				     gps.vel,
-				     gps.cog,
+				     (uint16_t)(gps.eph_m * 1e2f), // from m to cm
+				     (uint16_t)(gps.epv_m * 1e2f), // from m to cm
+				     (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
+				     (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
 				     gps.satellites_visible);
 
 	if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
@@ -698,7 +698,7 @@ uorb_receive_start(void)
 
 	/* --- GPS VALUE --- */
 	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink_subs.gps_sub, 1000);	/* 1Hz updates */
+	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */
 
 	/* --- HOME POSITION --- */
 	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c
index 4c021303f..030ac6e23 100644
--- a/apps/px4/tests/test_adc.c
+++ b/apps/px4/tests/test_adc.c
@@ -60,8 +60,10 @@ int test_adc(int argc, char *argv[])
 {
 	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
-		err(1, "can't open ADC device");
+	if (fd < 0) {
+		warnx("ERROR: can't open ADC device");
+		return 1;
+	}
 
 	for (unsigned i = 0; i < 5; i++) {
 		/* make space for a maximum of eight channels */
@@ -82,7 +84,7 @@ int test_adc(int argc, char *argv[])
 		usleep(150000);
 	}
 
-	message("\t ADC test successful.\n");
+	warnx("\t ADC test successful.\n");
 
 errout_with_dev:
 
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
index ad6828f20..9f8c5c9ea 100644
--- a/apps/px4/tests/tests_main.c
+++ b/apps/px4/tests/tests_main.c
@@ -135,6 +135,7 @@ test_all(int argc, char *argv[])
 	unsigned	i;
 	char		*args[2] = {"all", NULL};
 	unsigned int failcount = 0;
+	unsigned int testcount = 0;
 	bool		passed[NTESTS];
 
 	printf("\nRunning all tests...\n\n");
@@ -156,6 +157,7 @@ test_all(int argc, char *argv[])
 				fflush(stdout);
 				passed[i] = true;
 			}
+			testcount++;
 		}
 	}
 
@@ -178,7 +180,7 @@ test_all(int argc, char *argv[])
 		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
 		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
 		printf("\n");
-		printf(" All tests passed (%d of %d)\n", i, i);
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
 
 	} else {
 		printf("  ______   ______     __     __ \n");
@@ -187,7 +189,7 @@ test_all(int argc, char *argv[])
 		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
 		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
 		printf("\n");
-		printf(" Some tests failed (%d of %d)\n", failcount, i);
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
 	}
 
 	printf("\n");
@@ -245,6 +247,7 @@ int test_jig(int argc, char *argv[])
 	unsigned	i;
 	char		*args[2] = {"jig", NULL};
 	unsigned int failcount = 0;
+	unsigned int testcount = 0;
 	bool		passed[NTESTS];
 
 	printf("\nRunning all tests...\n\n");
@@ -264,6 +267,7 @@ int test_jig(int argc, char *argv[])
 				fflush(stdout);
 				passed[i] = true;
 			}
+			testcount++;
 		}
 	}
 
@@ -284,7 +288,7 @@ int test_jig(int argc, char *argv[])
 		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
 		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
 		printf("\n");
-		printf(" All tests passed (%d of %d)\n", i, i);
+		printf(" All tests passed (%d of %d)\n", testcount, testcount);
 	} else {
 		printf("  ______   ______     __     __ \n");
 		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
@@ -292,7 +296,7 @@ int test_jig(int argc, char *argv[])
 		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
 		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
 		printf("\n");
-		printf(" Some tests failed (%d of %d)\n", failcount, i);
+		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
 	}
 	printf("\n");
 
diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h
index dec34b6ab..7e1b82a0f 100644
--- a/apps/uORB/topics/home_position.h
+++ b/apps/uORB/topics/home_position.h
@@ -61,10 +61,10 @@ struct home_position_s
 	int32_t lat;                    /**< Latitude in 1E7 degrees */
 	int32_t lon;                    /**< Longitude in 1E7 degrees */
 	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	uint16_t eph;                   /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
-	uint16_t epv;                   /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
-	float s_variance;               /**< speed accuracy estimate cm/s */
-	float p_variance;               /**< position accuracy estimate cm */
+	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
+	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
+	float s_variance_m_s;               /**< speed accuracy estimate m/s */
+	float p_variance_m;               /**< position accuracy estimate m */
 };
 
 /**
diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h
index 009379920..c31c81d0c 100644..100755
--- a/apps/uORB/topics/vehicle_attitude.h
+++ b/apps/uORB/topics/vehicle_attitude.h
@@ -57,29 +57,26 @@
  */
 struct vehicle_attitude_s {
 
-	/*
-	 * Actual data, this is specific to the type of data which is stored in this struct
-	 * A line containing L0GME will be added by the Python logging code generator to the
-	 * logged dataset.
-	 */
-	uint64_t timestamp;   /**< in microseconds since system start          */
+	uint64_t timestamp;	/**< in microseconds since system start          */
 
 	/* This is similar to the mavlink message ATTITUDE, but for onboard use */
 
-	/* @warning Roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
+	/** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
 
-	float roll;           /**< Roll angle (rad, Tait-Bryan, NED)             LOGME    */
-	float pitch;          /**< Pitch angle (rad, Tait-Bryan, NED)            LOGME    */
-	float yaw;            /**< Yaw angle (rad, Tait-Bryan, NED)              LOGME    */
-	float rollspeed;      /**< Roll angular speed (rad/s, Tait-Bryan, NED)   LOGME    */
-	float pitchspeed;     /**< Pitch angular speed (rad/s, Tait-Bryan, NED)  LOGME    */
-	float yawspeed;       /**< Yaw angular speed (rad/s, Tait-Bryan, NED)    LOGME    */
-	float rate_offsets[3];/**< Offsets of the body angular rates from zero */
-	float R[3][3];        /**< Rotation matrix body to world, (Tait-Bryan, NED)       */
-	float q[4];           /**< Quaternion (NED) */
-	bool R_valid;         /**< Rotation matrix valid */
-	bool q_valid;         /**< Quaternion valid */
-	uint16_t counter;     /**< Counter of all attitude messages (wraps)    */
+	float roll;		/**< Roll angle (rad, Tait-Bryan, NED)				*/
+	float pitch;		/**< Pitch angle (rad, Tait-Bryan, NED)				*/
+	float yaw;		/**< Yaw angle (rad, Tait-Bryan, NED)				*/
+	float rollspeed;	/**< Roll angular speed (rad/s, Tait-Bryan, NED)		*/
+	float pitchspeed;	/**< Pitch angular speed (rad/s, Tait-Bryan, NED)		*/
+	float yawspeed;		/**< Yaw angular speed (rad/s, Tait-Bryan, NED)			*/
+	float rollacc;		/**< Roll angular accelration (rad/s, Tait-Bryan, NED)		*/
+	float pitchacc;		/**< Pitch angular acceleration (rad/s, Tait-Bryan, NED)	*/
+	float yawacc;		/**< Yaw angular acceleration (rad/s, Tait-Bryan, NED)		*/
+	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
+	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
+	float q[4];		/**< Quaternion (NED)						*/
+	bool R_valid;		/**< Rotation matrix valid					*/
+	bool q_valid;		/**< Quaternion valid						*/
 
 };
 
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h
index db529da06..5463a460d 100644
--- a/apps/uORB/topics/vehicle_gps_position.h
+++ b/apps/uORB/topics/vehicle_gps_position.h
@@ -55,35 +55,38 @@
  */
 struct vehicle_gps_position_s
 {
-	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)   */
-	uint32_t counter;			/**< Count of GPS messages */
-	uint64_t time_gps_usec;			/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+	uint64_t timestamp_position;		/**< Timestamp for position information */
+	int32_t lat;						/**< Latitude in 1E7 degrees */
+	int32_t lon;						/**< Longitude in 1E7 degrees */
+	int32_t alt;						/**< Altitude in 1E3 meters (millimeters) above MSL  */
 	
-	int32_t lat;				/**< Latitude in 1E7 degrees //LOGME   */
-	int32_t lon;				/**< Longitude in 1E7 degrees //LOGME   */
-	int32_t alt;				/**< Altitude in 1E3 meters (millimeters) above MSL //LOGME   */
-	uint16_t counter_pos_valid;		/**< is only increased when new lat/lon/alt information was added   */
-	uint16_t eph;				/**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME   */
-	uint16_t epv;				/**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535   */
-	float s_variance;			/**< speed accuracy estimate cm/s */
-	float p_variance;			/**< position accuracy estimate cm */
-	uint16_t vel;				/**< GPS ground speed (m/s * 100). If unknown, set to: 65535   */
-	float vel_n;				/**< GPS ground speed in m/s */
-	float vel_e;				/**< GPS ground speed in m/s */
-	float vel_d;				/**< GPS ground speed in m/s */
-	uint16_t cog;				/**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535   */
-	uint8_t fix_type; 			/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
-	uint8_t satellites_visible;		/**< Number of satellites visible. If unknown, set to 255   */
+	uint64_t timestamp_variance;
+	float s_variance_m_s;				/**< speed accuracy estimate m/s */
+	float p_variance_m;					/**< position accuracy estimate m */
+	uint8_t fix_type; 					/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
-	uint8_t satellite_prn[20]; 		/**< Global satellite ID   */
-	uint8_t satellite_used[20];		/**< 0: Satellite not used, 1: used for localization   */
-	uint8_t satellite_elevation[20]; 	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite   */
-	uint8_t satellite_azimuth[20];		/**< Direction of satellite, 0: 0 deg, 255: 360 deg.   */
-	uint8_t satellite_snr[20];		/**< Signal to noise ratio of satellite   */
-	uint8_t satellite_info_available;	/**< 0 for no info, 1 for info available   */
+	float eph_m;						/**< GPS HDOP horizontal dilution of position in m */
+	float epv_m;						/**< GPS VDOP horizontal dilution of position in m */
 
-	/* flags */
-	float vel_ned_valid;			/**< Flag to indicate if NED speed is valid */
+	uint64_t timestamp_velocity;		/**< Timestamp for velocity informations */
+	float vel_m_s;						/**< GPS ground speed (m/s) */
+	float vel_n_m_s;					/**< GPS ground speed in m/s */
+	float vel_e_m_s;					/**< GPS ground speed in m/s */
+	float vel_d_m_s;					/**< GPS ground speed in m/s */
+	float cog_rad;						/**< Course over ground (NOT heading, but direction of movement) in rad */
+	bool vel_ned_valid;					/**< Flag to indicate if NED speed is valid */
+
+	uint64_t timestamp_time;			/**< Timestamp for time information */
+	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+
+	uint64_t timestamp_satellites;		/**< Timestamp for sattelite information */
+	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
+	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
+	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
+	uint8_t satellite_elevation[20]; 	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t satellite_azimuth[20];		/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t satellite_snr[20];			/**< Signal to noise ratio of satellite   */
+	bool satellite_info_available;	/**< 0 for no info, 1 for info available */
 };
 
 /**