Revision 8:981f7e2365b6, committed 2018-07-31

Comitter:

Anaesthetix

Date:

Tue Jul 31 20:36:57 2018 +0000

Parent:

7:d86c41443f6d

Commit message:

Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mahony/MahonyAHRS.cpp	Tue Jul 31 20:36:57 2018 +0000
@@ -0,0 +1,288 @@
+//=============================================================================================
+// MahonyAHRS.c
+//=============================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+// Algorithm paper:
+// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
+//
+//=============================================================================================
+
+//-------------------------------------------------------------------------------------------
+// Header files
+
+#include "MahonyAHRS.h"
+#include <math.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define DEFAULT_SAMPLE_FREQ 1500.0f  // sample frequency in Hz
+#define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
+#define twoKiDef    (2.0f * 0.1f)   // 2 * integral gain
+
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Mahony::Mahony()
+{
+    twoKp = twoKpDef;   // 2 * proportional gain (Kp)
+    twoKi = twoKiDef;   // 2 * integral gain (Ki)
+    q0 = 1.0f;
+    q1 = 0.0f;
+    q2 = 0.0f;
+    q3 = 0.0f;
+    integralFBx = 0.0f;
+    integralFBy = 0.0f;
+    integralFBz = 0.0f;
+    anglesComputed = 0;
+    invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
+}
+
+void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+    float recipNorm;
+    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+    float hx, hy, bx, bz;
+    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
+    float halfex, halfey, halfez;
+    float qa, qb, qc;
+
+    // Use IMU algorithm if magnetometer measurement invalid
+    // (avoids NaN in magnetometer normalisation)
+    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+        updateIMU(gx, gy, gz, ax, ay, az);
+        return;
+    }
+
+    // Convert gyroscope degrees/sec to radians/sec
+    gx *= 0.0174533f;
+    gy *= 0.0174533f;
+    gz *= 0.0174533f;
+
+    // Compute feedback only if accelerometer measurement valid
+    // (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;
+
+        // Normalise magnetometer measurement
+        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+        bx = sqrtf(hx * hx + hy * hy);
+        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
+
+        // Estimated direction of gravity and magnetic field
+        halfvx = q1q3 - q0q2;
+        halfvy = q0q1 + q2q3;
+        halfvz = q0q0 - 0.5f + q3q3;
+        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+
+        // Error is sum of cross product between estimated direction
+        // and measured direction of field vectors
+        halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
+        halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
+        halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
+
+        // Compute and apply integral feedback if enabled
+        if(twoKi > 0.0f) {
+            // integral error scaled by Ki
+            integralFBx += twoKi * halfex * invSampleFreq;
+            integralFBy += twoKi * halfey * invSampleFreq;
+            integralFBz += twoKi * halfez * invSampleFreq;
+            gx += integralFBx;  // apply integral feedback
+            gy += integralFBy;
+            gz += integralFBz;
+        } else {
+            integralFBx = 0.0f; // prevent integral windup
+            integralFBy = 0.0f;
+            integralFBz = 0.0f;
+        }
+
+        // Apply proportional feedback
+        gx += twoKp * halfex;
+        gy += twoKp * halfey;
+        gz += twoKp * halfez;
+    }
+
+    // Integrate rate of change of quaternion
+    gx *= (0.5f * invSampleFreq);       // pre-multiply common factors
+    gy *= (0.5f * invSampleFreq);
+    gz *= (0.5f * invSampleFreq);
+    qa = q0;
+    qb = q1;
+    qc = q2;
+    q0 += (-qb * gx - qc * gy - q3 * gz);
+    q1 += (qa * gx + qc * gz - q3 * gy);
+    q2 += (qa * gy - qb * gz + q3 * gx);
+    q3 += (qa * gz + qb * gy - qc * gx);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+    anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
+{
+    float recipNorm;
+    float halfvx, halfvy, halfvz;
+    float halfex, halfey, halfez;
+    float qa, qb, qc;
+
+    // Convert gyroscope degrees/sec to radians/sec
+    gx *= 0.0174533f;
+    gy *= 0.0174533f;
+    gz *= 0.0174533f;
+
+    // Compute feedback only if accelerometer measurement valid
+    // (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;
+
+        // Estimated direction of gravity
+        halfvx = q1 * q3 - q0 * q2;
+        halfvy = q0 * q1 + q2 * q3;
+        halfvz = q0 * q0 - 0.5f + q3 * q3;
+
+        // Error is sum of cross product between estimated
+        // and measured direction of gravity
+        halfex = (ay * halfvz - az * halfvy);
+        halfey = (az * halfvx - ax * halfvz);
+        halfez = (ax * halfvy - ay * halfvx);
+
+        // Compute and apply integral feedback if enabled
+        if(twoKi > 0.0f) {
+            // integral error scaled by Ki
+            integralFBx += twoKi * halfex * invSampleFreq;
+            integralFBy += twoKi * halfey * invSampleFreq;
+            integralFBz += twoKi * halfez * invSampleFreq;
+            gx += integralFBx;  // apply integral feedback
+            gy += integralFBy;
+            gz += integralFBz;
+        } else {
+            integralFBx = 0.0f; // prevent integral windup
+            integralFBy = 0.0f;
+            integralFBz = 0.0f;
+        }
+
+        // Apply proportional feedback
+        gx += twoKp * halfex;
+        gy += twoKp * halfey;
+        gz += twoKp * halfez;
+    }
+
+    // Integrate rate of change of quaternion
+    gx *= (0.5f * invSampleFreq);       // pre-multiply common factors
+    gy *= (0.5f * invSampleFreq);
+    gz *= (0.5f * invSampleFreq);
+    qa = q0;
+    qb = q1;
+    qc = q2;
+    q0 += (-qb * gx - qc * gy - q3 * gz);
+    q1 += (qa * gx + qc * gz - q3 * gy);
+    q2 += (qa * gy - qb * gz + q3 * gx);
+    q3 += (qa * gz + qb * gy - qc * gx);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+    anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+/*
+float Mahony::invSqrt(float x)
+{
+    float halfx = 0.5f * x;
+    float y = x;
+    long i = *(long*)&y;
+    i = 0x5f3759df - (i>>1);
+    y = *(float*)&i;
+    y = y * (1.5f - (halfx * y * y));
+    y = y * (1.5f - (halfx * y * y));
+    return y;
+}
+*/
+/*
+float Mahony::invSqrt(float x){
+   unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
+   float tmp = *(float*)&i;
+   return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
+}
+*/
+
+float Mahony::invSqrt(float x)
+{
+    float temp = 1/(sqrt(x));
+    return temp;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void Mahony::computeAngles()
+{
+    roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+    pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+    yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    anglesComputed = 1;
+}
+
+
+//============================================================================================
+// END OF CODE
+//============================================================================================
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mahony/MahonyAHRS.h	Tue Jul 31 20:36:57 2018 +0000
@@ -0,0 +1,67 @@
+//=============================================================================================
+// MahonyAHRS.h
+//=============================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef MahonyAHRS_h
+#define MahonyAHRS_h
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+
+class Mahony {
+private:
+    float twoKp;        // 2 * proportional gain (Kp)
+    float twoKi;        // 2 * integral gain (Ki)
+    float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
+    float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
+   // float invSampleFreq;
+    float roll, pitch, yaw;
+    char anglesComputed;
+    static float invSqrt(float x);
+    void computeAngles();
+
+//-------------------------------------------------------------------------------------------
+// Function declarations
+
+public:
+    Mahony();
+    float invSampleFreq;
+    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    float getRoll() {
+        if (!anglesComputed) computeAngles();
+        return roll * 57.29578f;
+    }
+    float getPitch() {
+        if (!anglesComputed) computeAngles();
+        return pitch * 57.29578f;
+    }
+    float getYaw() {
+        if (!anglesComputed) computeAngles();
+        return yaw * 57.29578f + 180.0f;
+    }
+    float getRollRadians() {
+        if (!anglesComputed) computeAngles();
+        return roll;
+    }
+    float getPitchRadians() {
+        if (!anglesComputed) computeAngles();
+        return pitch;
+    }
+    float getYawRadians() {
+        if (!anglesComputed) computeAngles();
+        return yaw;
+    }
+};
+
+#endif
\ No newline at end of file

--- a/calccomp.h	Tue Jul 17 14:56:05 2018 +0000
+++ b/calccomp.h	Tue Jul 31 20:36:57 2018 +0000
@@ -1,11 +1,17 @@
 // Coded by: Erik van de Coevering
 
-// Axis are mixed up, will fix soon.
 #include "mbed.h"
+#include "MAfilter.h"
 
 DigitalOut led1(LED1); // for stick arming (leds are active low)
 
 extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z;
+float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y;
+MAfilter10 ma_errorx, ma_errory;
+static signed int m1 = 0;
+static signed int m2 = 0;
+static signed int m3 = 0;
+static signed int m4 = 0;
 
 void calccomp(int* ctrl, float* angles, int* motor) //ctrl: 0-rud 1-elev 2-throttle 3-aileron      angles: 0-ax 1-ay 2-az 3-gx 4-gy 5-gz
 {
@@ -21,14 +27,9 @@
     int rud = 0;
     int zcomp = 0;
     int throttle = 0;
-    static signed int m1 = 0;
-    static signed int m2 = 0;
-    static signed int m3 = 0;
-    static signed int m4 = 0;
     static bool armed = false;
     float xcntrl = 0;
     float ycntrl = 0;
-    float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz;
     float error_z = 0;
     float error_z1 = 0;
 
@@ -47,14 +48,16 @@
     m3 = throttle;
     m4 = throttle;
 
-    // Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45
+    // Current values used on a 250 size mini racer (still needs tuning): P: 2.7, I: 2.0, D: 0.4
     // Calc PID values and prevent integral windup on KIx
-    KPx = (angles[0] - xcntrl) * KP_y;
-    KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001);
+    error_x = angles[0] - xcntrl;
+    KPx = error_x * KP_y;
+    KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon.
 
-    if(KIx > 50) KIx = 50;
-    if(KIx < -50) KIx = -50;
-    KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y;
+    if(KIx > 10) KIx = 10;
+    if(KIx < -10) KIx = -10;
+
+    KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive
 
     xcomp = KPx + KIx + KDx;
     xcomp2 = xcomp*-1;
@@ -66,12 +69,13 @@
     m4 = m4 - xcomp2;
 
     // Calc PID values and prevent integral windup on KIy
-    KPy = (angles[1] + ycntrl) * KP_x;
-    KIy = KIy + ((angles[1] + ycntrl) * KI_x * 0.001);
+    error_y = angles[1] + ycntrl;
+    KPy = error_y * KP_x;
+    KIy = KIy + (error_y * 0.001f * KI_x);
 
-    if(KIy > 50) KIy = 50;
-    if(KIy < -50) KIy = -50;
-    KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x;
+    if(KIy > 10) KIy = 10;
+    if(KIy < -10) KIy = -10;
+    KDy = (angles[4] + (error_y)) * KD_x;
 
     ycomp = KPy + KIy + KDy;
     ycomp = ycomp*-1;
@@ -89,13 +93,14 @@
     KPz = error_z * KP_z;
     KIz = KIz + (error_z * 0.001f * KI_z);
 
-    if(KIz > 80) KIz = 80;
-    if(KIz < -80) KIz = -80;
+    if(KIz > 20) KIz = 20;
+    if(KIz < -20) KIz = -20;
 
     KDz = (error_z - error_z1) * KD_z;
+
     error_z1 = error_z;
 
-    zcomp = (KPz + KIz + KDz) * -1.0f;
+    zcomp = (KPz + KDz) * -1.0f;
 
     //Mix yaw
     m1 = m1 - zcomp;
@@ -128,4 +133,4 @@
     motor[1] = m2;
     motor[2] = m3;
     motor[3] = m4;
-}
\ No newline at end of file
+}

--- a/main.cpp	Tue Jul 17 14:56:05 2018 +0000
+++ b/main.cpp	Tue Jul 31 20:36:57 2018 +0000
@@ -2,6 +2,7 @@
 
 #include "mbed.h"
 #include "MadgwickAHRS.h"
+#include "MahonyAHRS.h"
 #include "MAfilter.h"
 #include "MPU9250_SPI.h"
 #include "calccomp.h"
@@ -22,15 +23,16 @@
 SPI spi(p11, p12, p13);
 mpu9250_spi mpu9250(spi, p14);
 
-Madgwick filter;
+//Madgwick filter;
+Mahony filter;
 
 Timer timer;
 Timer tijd;
 Timer t;
 Timer print;
 
-MAfilter10 maGX, maGY, maGZ;
 LPfilter8 lp1, lp2, lp3;
+MAfilter10 MAgx, MAgy, MAgz;
 
 IAP iap;
 
@@ -56,321 +58,328 @@
 
 char mcommand;
 
-void rx0() {
-	trx0.start();
+void rx0()
+{
+    trx0.start();
 }
 
-void rx1() {
-	trx1.start();
-	trx0.stop();
-	if(trx0.read_us() > 900 && trx0.read_us() < 2200) rx_data[0] = trx0.read_us();
-	trx0.reset();
+void rx1()
+{
+    trx1.start();
+    trx0.stop();
+    if(trx0.read_us() > 900 && trx0.read_us() < 2200) rx_data[0] = trx0.read_us();
+    trx0.reset();
 }
 
-void rx2() {
-	trx2.start();
-	trx1.stop();
-	if(trx1.read_us() > 900 && trx1.read_us() < 2200) rx_data[1] = trx1.read_us();
-	trx1.reset();
+void rx2()
+{
+    trx2.start();
+    trx1.stop();
+    if(trx1.read_us() > 900 && trx1.read_us() < 2200) rx_data[1] = trx1.read_us();
+    trx1.reset();
 }
 
-void rx3() {
-	trx3.start();
-	trx2.stop();
-	if(trx2.read_us() > 900 && trx2.read_us() < 2200) rx_data[2] = trx2.read_us();
-	trx2.reset();
+void rx3()
+{
+    trx3.start();
+    trx2.stop();
+    if(trx2.read_us() > 900 && trx2.read_us() < 2200) rx_data[2] = trx2.read_us();
+    trx2.reset();
 }
 
-void rx4() {
-	trx4.start();
-	trx3.stop();
-	if(trx3.read_us() > 900 && trx3.read_us() < 2200) rx_data[3] = trx3.read_us();
-	trx3.reset();
+void rx4()
+{
+    trx4.start();
+    trx3.stop();
+    if(trx3.read_us() > 900 && trx3.read_us() < 2200) rx_data[3] = trx3.read_us();
+    trx3.reset();
 }
 
-void rx5() {
-	trx5.start();
-	trx4.stop();
-	if(trx4.read_us() > 900 && trx4.read_us() < 2200) rx_data[4] = trx4.read_us();
-	trx4.reset();
+void rx5()
+{
+    trx5.start();
+    trx4.stop();
+    if(trx4.read_us() > 900 && trx4.read_us() < 2200) rx_data[4] = trx4.read_us();
+    trx4.reset();
 }
 
-void rx6() {
-	trx5.stop();
-	if(trx5.read_us() > 900 && trx5.read_us() < 2200) rx_data[5] = trx5.read_us();
-	trx5.reset();
-	rxd = true;
+void rx6()
+{
+    trx5.stop();
+    if(trx5.read_us() > 900 && trx5.read_us() < 2200) rx_data[5] = trx5.read_us();
+    trx5.reset();
+    rxd = true;
 }
 
 
 int main()
 {
-	ch_sw = 1;
-	led1 = 1;
-	led2 = 0;
-	pc1->baud(230400);
-	pc1->setTimeout(1);
-	pc.baud(230400);
-	spi.frequency(4000000);
-	
-	
-	//IAP variables
-	char* addr = sector_start_adress[TARGET_SECTOR];
-	char  mem[ MEM_SIZE ];    //  memory, it should be aligned to word boundary
-	char 	PIDsetup = 0;
-	int		r;
-	int 	tempval;
-	
-	//IMU variables
-	float angles[6] = {0};
-  float time = 0;
-	float samplef = 0;
-	float gyrodata[3] = {0};
-	float acceldata[3] = {0};
-	float angles_temp[2] = {0};
-	float roll, pitch;
-	float tempcomp[6] = {0};
-	float temp = 0;
-	float temp2 = 0;
-	float temp3 = 0;
-	bool exit = true;
-	float noise = 0;
-	int count = 0;
-	
-	float filtertest = 1.0;
-		
-		    //Rx variables
-	int motor[4] = {0};
-		
-		// Init pwm
-	motor1.period_ms(10);
-  motor1.pulsewidth_us(1000);
-  motor2.pulsewidth_us(1000);
-  motor3.pulsewidth_us(1000);
-  motor4.pulsewidth_us(1000);
-	
-	filter.begin(1500);
-	
-	pc.putc('c');
-	PIDsetup = pc1->getc();
-	if(PIDsetup == 'c') {
-		while(1) {
-			PIDsetup = pc1->getc();
-			if(PIDsetup == 'R') {
-            for(int i=0; i<18; i++) {
-                pc1->putc(addr[i]);
-                wait_ms(1);
+    ch_sw = 1;
+    led1 = 1;
+    led2 = 0;
+    pc1->baud(230400);
+    pc1->setTimeout(1);
+    pc.baud(230400);
+    spi.frequency(4000000);
+
+
+    //IAP variables
+    char* addr = sector_start_adress[TARGET_SECTOR];
+    char  mem[ MEM_SIZE ];    //  memory, it should be aligned to word boundary
+    char 	PIDsetup = 0;
+    int		r;
+    int 	tempval;
+
+    //IMU variables
+    float angles[6] = {0};
+    float time = 0;
+    float samplef = 0;
+    float gyrodata[3] = {0};
+    float acceldata[3] = {0};
+    float angles_temp[2] = {0};
+    float roll, pitch;
+    float tempcomp[6] = {0};
+    float temp = 0;
+    float temp2 = 0;
+    float temp3 = 0;
+    bool exit = true;
+    float noise = 0;
+    int count = 0;
+
+    //Rx variables
+    int motor[4] = {0};
+
+    // Init pwm
+    motor1.period_ms(10);
+    motor1.pulsewidth_us(1000);
+    motor2.pulsewidth_us(1000);
+    motor3.pulsewidth_us(1000);
+    motor4.pulsewidth_us(1000);
+
+    filter.begin(1500);
+
+    pc.putc('c');
+    PIDsetup = pc1->getc();
+    if(PIDsetup == 'c') {
+        while(1) {
+            PIDsetup = pc1->getc();
+            if(PIDsetup == 'R') {
+                for(int i=0; i<18; i++) {
+                    pc1->putc(addr[i]);
+                    wait_ms(1);
+                }
+            }
+
+            if(PIDsetup == 'W') {
+                for(int i=0; i<18; i++) {
+                    mem[i] = pc1->getc();
+                }
+                iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+                r   = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+                iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+                r   = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
             }
         }
+    }
 
-        if(PIDsetup == 'W') {
-            for(int i=0; i<18; i++) {
-                mem[i] = pc1->getc();
+
+    if(PIDsetup == 'W') {
+        mpu9250.init2(1,BITS_DLPF_CFG_188HZ);
+        pc1->setTimeout(0.01);
+
+        rx_rud.rise(&rx0);
+        rx_elev.rise(&rx1);
+        rx_thr.rise(&rx2);
+        rx_ail.rise(&rx3);
+        rx_p1.rise(&rx4);
+        rx_p2.rise(&rx5);
+        rx_p2.fall(&rx6);
+        mcommand = 'a';
+
+        while(exit) {
+            wait_ms(1);
+            if(mcommand == '5') {
+                motor1.pulsewidth_us(rx_data[2] - 20);
+                motor2.pulsewidth_us(1000);
+                motor3.pulsewidth_us(1000);
+                motor4.pulsewidth_us(1000);
+            } else if(mcommand == '6') {
+                motor1.pulsewidth_us(1000);
+                motor2.pulsewidth_us(rx_data[2] - 20);
+                motor3.pulsewidth_us(1000);
+                motor4.pulsewidth_us(1000);
+            } else if(mcommand == '3') {
+                motor1.pulsewidth_us(1000);
+                motor2.pulsewidth_us(1000);
+                motor3.pulsewidth_us(rx_data[2] - 20);
+                motor4.pulsewidth_us(1000);
+            } else if(mcommand == '4') {
+                motor1.pulsewidth_us(1000);
+                motor2.pulsewidth_us(1000);
+                motor3.pulsewidth_us(1000);
+                motor4.pulsewidth_us(rx_data[2] - 20);
             }
-            iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
-            r   = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
-            iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
-            r   = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+            if(mcommand == 'E') exit = 0;
+
+            if(rxd) {
+                led2 = !led2;
+                rxd = false;
+            }
+
+            mpu9250.read_all();
+            if(mpu9250.accelerometer_data[0] >= 0) noise = noise*0.99 + 0.01*mpu9250.accelerometer_data[0];
+
+            if(count>100) {
+                count = 0;
+                pc.printf("%.2f\n", noise);
+                mcommand = pc1->getc();
+            }
+            count++;
         }
-			}
-		}
-	
-	
-		if(PIDsetup == 'W') {
-			mpu9250.init2(1,BITS_DLPF_CFG_188HZ);
-			pc1->setTimeout(0.01);
-			
-			rx_rud.rise(&rx0);
-			rx_elev.rise(&rx1);
-			rx_thr.rise(&rx2);
-			rx_ail.rise(&rx3);
-			rx_p1.rise(&rx4);
-			rx_p2.rise(&rx5);
-			rx_p2.fall(&rx6);
-			mcommand = 'a';
-			
-			while(exit) {
-				wait_ms(1);
-				if(mcommand == '5') {
-					motor1.pulsewidth_us(rx_data[2] - 20);
-					motor2.pulsewidth_us(1000);
-					motor3.pulsewidth_us(1000);
-					motor4.pulsewidth_us(1000);
-				}
-				else if(mcommand == '6') {
-					motor1.pulsewidth_us(1000);
-					motor2.pulsewidth_us(rx_data[2] - 20);
-					motor3.pulsewidth_us(1000);
-					motor4.pulsewidth_us(1000);
-				}
-				else if(mcommand == '3') {
-					motor1.pulsewidth_us(1000);
-					motor2.pulsewidth_us(1000);
-					motor3.pulsewidth_us(rx_data[2] - 20);
-					motor4.pulsewidth_us(1000);
-				}
-				else if(mcommand == '4') {
-					motor1.pulsewidth_us(1000);
-					motor2.pulsewidth_us(1000);
-					motor3.pulsewidth_us(1000);
-					motor4.pulsewidth_us(rx_data[2] - 20);
-				}
-				if(mcommand == 'E') exit = 0;
-				
-				if(rxd) {
-					led2 = !led2;
-					rxd = false;
-				}
-				
-				mpu9250.read_all();
-				if(mpu9250.accelerometer_data[0] >= 0) noise = noise*0.99 + 0.01*mpu9250.accelerometer_data[0];
-				
-				if(count>100) {
-					count = 0;
-					pc.printf("%.2f\n", noise);
-					mcommand = pc1->getc();
-				}
-				count++;
-			}
-		}
-		
-		tempval = addr[0];
+    }
+
+    tempval = addr[0];
     tempval = tempval + (addr[1] << 8);
     KP_x = ((float)tempval) / 100;
-		tempval = addr[2];
+    tempval = addr[2];
     tempval = tempval + (addr[3] << 8);
     KI_x = ((float)tempval) / 100;
-		tempval = addr[4];
+    tempval = addr[4];
     tempval = tempval + (addr[5] << 8);
     KD_x = ((float)tempval) / 100;
-		
-		tempval = addr[6];
+
+    tempval = addr[6];
     tempval = tempval + (addr[7] << 8);
     KP_y = ((float)tempval) / 100;
-		tempval = addr[8];
+    tempval = addr[8];
     tempval = tempval + (addr[9] << 8);
     KI_y = ((float)tempval) / 100;
-		tempval = addr[10];
+    tempval = addr[10];
     tempval = tempval + (addr[11] << 8);
     KD_y = ((float)tempval) / 100;
-		
-		tempval = addr[12];
+
+    tempval = addr[12];
     tempval = tempval + (addr[13] << 8);
     KP_z = ((float)tempval) / 100;
-		tempval = addr[14];
+    tempval = addr[14];
     tempval = tempval + (addr[15] << 8);
     KI_z = ((float)tempval) / 100;
-		tempval = addr[16];
+    tempval = addr[16];
     tempval = tempval + (addr[17] << 8);
     KD_z = ((float)tempval) / 100;
-		
-		mpu9250.init(1,BITS_DLPF_CFG_188HZ);
-		/*
-		pc.printf("%.2f		%.2f		%.2f\r\n", KP_x, KI_x, KD_x);
-		pc.printf("%.2f		%.2f		%.2f\r\n", KP_y, KI_y, KD_y);
-		pc.printf("%.2f		%.2f		%.2f\r\n", KP_z, KI_z, KD_z);
-		*/
-		rx_rud.rise(&rx0);
-		rx_elev.rise(&rx1);
-		rx_thr.rise(&rx2);
-		rx_ail.rise(&rx3);
-		rx_p1.rise(&rx4);
-		rx_p2.rise(&rx5);
-		rx_p2.fall(&rx6);
-		
-		t.start();
-		
+
+    mpu9250.init(1,BITS_DLPF_CFG_188HZ);
+    /*
+    pc.printf("%.2f		%.2f		%.2f\r\n", KP_x, KI_x, KD_x);
+    pc.printf("%.2f		%.2f		%.2f\r\n", KP_y, KI_y, KD_y);
+    pc.printf("%.2f		%.2f		%.2f\r\n", KP_z, KI_z, KD_z);
+    */
+    rx_rud.rise(&rx0);
+    rx_elev.rise(&rx1);
+    rx_thr.rise(&rx2);
+    rx_ail.rise(&rx3);
+    rx_p1.rise(&rx4);
+    rx_p2.rise(&rx5);
+    rx_p2.fall(&rx6);
+
+    t.start();
+
     while(1) {
-				print.start();
-				t.stop();
-				time = (float)t.read();
-				t.reset();
-				t.start();
-				filter.invSampleFreq = time;
-				samplef = 1/time;
-			
-				mpu9250.read_all();
-				if(mpu9250.Temperature < 6.0f) temp = 6.0f;
-				else if(mpu9250.Temperature > 60.0f) temp = 60.0f;
-				else temp = mpu9250.Temperature;
-				temp2 = temp*temp;
-				temp3 = temp2*temp;
-				
-				// Temperature compensation
-				tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179;
-				tempcomp[1] = 0.0005727*temp - 0.01488;
-				tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp;
-				tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 3.018;
-				tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618;
-				tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939;
-			
-				// Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool)
-				acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]);
-				acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]);
-				acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1);
-				
-				// 10-term moving average to remove some noise
-				gyrodata[0] = maGX.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1);
-				gyrodata[1] = maGY.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1);
-				gyrodata[2] = maGZ.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1);
-			
-				// Madgwick's quaternion algorithm
-				filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0],
-                                 acceldata[1], acceldata[2]);
-			
-				roll = filter.getRoll();
-				pitch = filter.getPitch();
-				
-				angles[3] = gyrodata[1];
-				angles[4] = gyrodata[0];
-				angles[5] = gyrodata[2];
-				
-				//Simple first order complementary filter -> TODO: CHECK STEP RESPONSE
-				angles[0] = 0.99f*(angles[0] + (gyrodata[1]*time)) + 0.01f*pitch;
-				angles[1] = 0.99f*(angles[1] + (gyrodata[0]*time)) + 0.01f*roll;
+        print.start();
+        t.stop();
+        time = (float)t.read();
+        t.reset();
+        t.start();
+        filter.invSampleFreq = time;
+        samplef = 1/time;
+
+        mpu9250.read_all();
+        if(mpu9250.Temperature < 6.0f) temp = 6.0f;
+        else if(mpu9250.Temperature > 60.0f) temp = 60.0f;
+        else temp = 0.999*temp + 0.001*mpu9250.Temperature;
+        temp2 = temp*temp;
+        temp3 = temp2*temp;
+
+        // Temperature compensation
+        tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179;
+        tempcomp[1] = 0.0005727*temp - 0.01488;
+        tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp;
+        tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 1.4;
+        tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618;
+        tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939;
+
+        // Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool)
+        acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]);
+        acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]);
+        acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1);
+
+        // 10-term moving average to remove some noise
+        gyrodata[0] = MAgx.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1);
+        gyrodata[1] = MAgy.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1);
+        gyrodata[2] = MAgz.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1);
+
+        // Madgwick's quaternion algorithm
+        filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0],
+                         acceldata[1], acceldata[2]);
+
+        //	filter.update(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], acceldata[1], acceldata[2], mpu9250.Magnetometer[0], mpu9250.Magnetometer[1], mpu9250.Magnetometer[2]);
+        //
+        roll = filter.getRoll();
+        pitch = filter.getPitch();
+
+        angles[3] = gyrodata[1];
+        angles[4] = gyrodata[0];
+        angles[5] = gyrodata[2];
 
-				// If [VAR] is NaN use previous value
-				if(angles[0] != angles[0]) angles[0] = angles_temp[0];
-				if(angles[1] != angles[1]) angles[1] = angles_temp[1];
-				if(angles[0] == angles[0]) angles_temp[0] = angles[0];
-				if(angles[1] == angles[1]) angles_temp[1] = angles[1];
-				
-				tijd.stop();
-				tijd.reset();
-				tijd.start();
-				
-		/*		
-				if(print.read_ms() > 100) {
-					print.stop();
-					print.reset();
-					print.start();
-					//led2 = !led2;
-				//	pc.printf("X: %.2f		Y: %.2f		%.0f\r\n", angles[0], angles[1], samplef);
-					pc.printf("%.2f		%.0f\r\n", angles[1], samplef);
-				}
-				*/
-				pc.printf("%.1f		%.0f\r\n", angles[0], samplef);
-				if(rxd) {
-					led2 = !led2;
-					rxd = false;
-	//				pc.printf("%d		%d		%d		%d\r\n", motor[0], motor[1], motor[2], motor[3]);
-				}
-				
-				// To change VTX channel/band/power with the remote
-				if(rx_data[5] > 1650) ch_sw = 0;
-				else ch_sw = 1;
-				
-				calccomp(rx_data, angles, motor);
-				
-				motor1.pulsewidth_us(motor[0]);
-				motor2.pulsewidth_us(motor[1]);
-				motor3.pulsewidth_us(motor[2]);
-				motor4.pulsewidth_us(motor[3]);
-				/*
-				motor1.pulsewidth_us(rx_data[2]-20);
-				motor2.pulsewidth_us(rx_data[2]-20);
-				motor3.pulsewidth_us(rx_data[2]-20);
-				motor4.pulsewidth_us(rx_data[2]-20);
-				*/
+        //Simple first order complementary filter -> TODO: CHECK STEP RESPONSE
+        angles[0] = 0.95f*(angles[0] + (gyrodata[1]*time)) + 0.05f*pitch;
+        angles[1] = 0.95f*(angles[1] + (gyrodata[0]*time)) + 0.05f*roll;
+
+//				angles[0] = pitch;
+//				angles[1] = roll;
+
+        // If [VAR] is NaN use previous value
+        if(angles[0] != angles[0]) angles[0] = angles_temp[0];
+        if(angles[1] != angles[1]) angles[1] = angles_temp[1];
+        if(angles[0] == angles[0]) angles_temp[0] = angles[0];
+        if(angles[1] == angles[1]) angles_temp[1] = angles[1];
+
+        tijd.stop();
+        tijd.reset();
+        tijd.start();
+
+        /*
+        		if(print.read_ms() > 100) {
+        			print.stop();
+        			print.reset();
+        			print.start();
+        			//led2 = !led2;
+        		//	pc.printf("X: %.2f		Y: %.2f		%.0f\r\n", angles[0], angles[1], samplef);
+        			pc.printf("%.2f		%.0f\r\n", angles[1], samplef);
+        		}
+        		*/
+        pc.printf("%.0f		%.0f\r\n", pitch, roll);
+        if(rxd) {
+            led2 = !led2;
+            rxd = false;
+            //			pc.printf("%d		%d		%d		%d\r\n", rx_data[0], rx_data[1], rx_data[2], rx_data[3]);
+        }
+
+        // To change VTX channel/band/power with the remote
+        if(rx_data[5] > 1650) ch_sw = 0;
+        else ch_sw = 1;
+
+        calccomp(rx_data, angles, motor);
+
+        motor1.pulsewidth_us(motor[0]);
+        motor2.pulsewidth_us(motor[1]);
+        motor3.pulsewidth_us(motor[2]);
+        motor4.pulsewidth_us(motor[3]);
+        /*
+        motor1.pulsewidth_us(rx_data[2]-20);
+        motor2.pulsewidth_us(rx_data[2]-20);
+        motor3.pulsewidth_us(rx_data[2]-20);
+        motor4.pulsewidth_us(rx_data[2]-20);
+        */
     }
 }