Revision 1:045edcf091f3, committed 2014-05-16

Comitter:

joe4465

Date:

Fri May 16 14:18:05 2014 +0000

Parent:

0:0010a5abcc31

Child:

2:b3b771c8f7d1

Commit message:

first commit

Changed in this revision

--- a/FreeIMU/FreeIMU.cpp	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/FreeIMU.cpp	Fri May 16 14:18:05 2014 +0000
@@ -95,6 +95,11 @@
 
 }
 
+void FreeIMU::sample(bool sampling)
+{
+    accgyro->sample(sampling);
+}
+
 /**
  * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
 */
@@ -427,21 +432,6 @@
     r[2] = -val[4]; //Roll
 }
 
-void FreeIMU::stop_sampling()
-{
-    accgyro->stop_sampling();
-    //magn->stop_sampling();
-    //baro->stop_sampling();
-}
-
-void FreeIMU::start_sampling()
-{
-    accgyro->start_sampling();
-    //magn->start_sampling();
-    //baro->start_sampling();
-}
-
-
 const float def_sea_press = 1013.25;
 
 /**

--- a/FreeIMU/FreeIMU.h	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/FreeIMU.h	Fri May 16 14:18:05 2014 +0000
@@ -121,6 +121,7 @@
     void init(bool fastmode);
     
     void init(int accgyro_addr, bool fastmode);
+    void sample(bool sampling);
     
     #ifndef CALIBRATION_H
     void calLoad();
@@ -136,8 +137,6 @@
     void getYawPitchRollRad(float * ypr);
     void gravityCompensateAcc(float * acc, float * q);
     float getRawPressure();
-    void start_sampling();
-    void stop_sampling();
     
     float getBaroAlt();
     float getBaroAlt(float sea_press);

--- a/FreeIMU/HMC58X3/HMC58X3.cpp	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/HMC58X3/HMC58X3.cpp	Fri May 16 14:18:05 2014 +0000
@@ -326,10 +326,6 @@
     _thread.signal_set(0x1);
 }
 
-void HMC58X3::stop_sampling(){
-    _thread.signal_set(0x1);
-}
-
 bool magn_valid = false;
 
 void HMC58X3::samplingthread()

--- a/FreeIMU/HMC58X3/HMC58X3.h	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/HMC58X3/HMC58X3.h	Fri May 16 14:18:05 2014 +0000
@@ -110,7 +110,6 @@
     
     void samplingthread();
     void start_sampling();
-    void stop_sampling();
 
     static const int I2C_ADDRESS = 0x3D;
     char HMC58X3_R_IDA;

--- a/FreeIMU/MPU6050/MPU6050.cpp	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/MPU6050/MPU6050.cpp	Fri May 16 14:18:05 2014 +0000
@@ -1922,24 +1922,23 @@
 }
 
 void MPU6050::mpu_sample_func(){
-    i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
+    if(_sampling == true)
+    {
+        i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
+    }
 }
 
 void MPU6050::start_sampling(){
+    _sampling = true;
     mpu_cmd = MPU6050_RA_ACCEL_XOUT_H;
     mpu_sampling.attach_us(this, &MPU6050::mpu_sample_func, 2000);
-    printf("Started sampling accgyro\r\n");
 }
 
-void MPU6050::stop_sampling(){
-    printf("Stopping sampling accgyro\r\n");
-    //mpu_sampling.detach();
-    mpu_sampling.attach_us(this, &MPU6050::empty_function, 2000);
-    printf("Stopped sampling accgyro\r\n");
+void MPU6050::sample(bool sampling)
+{
+    _sampling = sampling;
 }
 
-void MPU6050::empty_function(){}
-
 /** Get 3-axis accelerometer readings.
  * These registers store the most recent accelerometer measurements.
  * Accelerometer measurements are written to these registers at the Sample Rate

--- a/FreeIMU/MPU6050/MPU6050.h	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/MPU6050/MPU6050.h	Fri May 16 14:18:05 2014 +0000
@@ -413,14 +413,14 @@
         
         void mpu_sample_func();
         volatile int16_t ax_cache, ay_cache, az_cache, gx_cache, gy_cache, gz_cache;
+        bool _sampling;
+        void sample(bool sampling);
         
         Ticker mpu_sampling;
         char mpu_cmd;
         uint8_t mpu_buffer[14];
         
         void start_sampling();
-        void stop_sampling();
-        void empty_function();
 
         void initialize();
         bool testConnection();

--- a/FreeIMU/MS561101BA/MS561101BA.cpp	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/MS561101BA/MS561101BA.cpp	Fri May 16 14:18:05 2014 +0000
@@ -117,10 +117,6 @@
     _thread.signal_set(0x1);
 }
 
-void MS561101BA::stop_sampling(){
-    _thread.signal_set(0x1);
-}
-
 int MS561101BA::rawTemperature(){
     return tempCache;
 }

--- a/FreeIMU/MS561101BA/MS561101BA.h	Fri May 09 10:04:36 2014 +0000
+++ b/FreeIMU/MS561101BA/MS561101BA.h	Fri May 16 14:18:05 2014 +0000
@@ -62,7 +62,6 @@
     float getTemperature();
     int32_t getDeltaTemp();
     void start_sampling(uint8_t OSR);
-    void stop_sampling();
     int rawTemperature();
     int rawPressure();
     int readPROM();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MovingAverageFilter.lib	Fri May 16 14:18:05 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/joe4465/code/MovingAverageFilter/#21b70641f866

--- a/flightController.h	Fri May 09 10:04:36 2014 +0000
+++ b/flightController.h	Fri May 16 14:18:05 2014 +0000
@@ -7,22 +7,22 @@
 float map(float x, float in_min, float in_max, float out_min, float out_max);
 void GetAttitude();
 void Task500Hz(void const *n);
-void Task50Hz();
 void Task10Hz();
 
 //Variables
 float _gyroRate[3] ={}; // Yaw, Pitch, Roll
-float _rcConstrainedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
 float _yawTarget = 0;
 int _notFlying = 0; 
 float _altitude = 0;
-int _50HzIterator = 0;
 int _10HzIterator = 0;
 
+//Timers
+RtosTimer *_updateTimer;
+
 // A thread to monitor the serial ports
 void FlightControllerThread(void const *args) 
 {  
-    //Rtos Timers
+    //Update Timer
     _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
     int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
     _updateTimer->start(updateTime);
@@ -49,8 +49,7 @@
 //Zeros values
 void GetAttitude()
 {
-    //Take off zero values to account for any angle inbetween the PCB and quadcopter chassis
-    _ypr[0] = _ypr[0] - _zeroValues[0];
+    //Take off zero values to account for any angle inbetween the PCB level and ground
     _ypr[1] = _ypr[1] - _zeroValues[1];
     _ypr[2] = _ypr[2] - _zeroValues[2];
     
@@ -63,10 +62,10 @@
 
 void Task500Hz(void const *n)
 {
-    _50HzIterator++;
-    if(_50HzIterator % 10 == 0)
+    _10HzIterator++;
+    if(_10HzIterator % 50 == 0)
     {
-        Task50Hz();
+        Task10Hz();
     }
 
     //Get IMU data
@@ -83,13 +82,13 @@
         _rollRatePIDController->setProcessValue(_gyroRate[2]);
         
         //Update rate PID set point with desired rate from RC
-        _yawRatePIDController->setSetPoint(_rcConstrainedCommands[0]);
-        _pitchRatePIDController->setSetPoint(_rcConstrainedCommands[1]);
-        _rollRatePIDController->setSetPoint(_rcConstrainedCommands[2]);
+        _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
+        _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
+        _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
         
         //Compute rate PID outputs
         _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
-        _ratePIDControllerOutputs[1] = -_pitchRatePIDController->compute();
+        _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
         _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
     }
     //Stability mode
@@ -102,18 +101,18 @@
         
         //Update stab PID set point with desired angle from RC
         _yawStabPIDController->setSetPoint(_yawTarget);
-        _pitchStabPIDController->setSetPoint(_rcConstrainedCommands[1]);
-        _rollStabPIDController->setSetPoint(_rcConstrainedCommands[2]);
+        _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
+        _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
         
         //Compute stab PID outputs
         _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
-        _stabPIDControllerOutputs[1] = -_pitchStabPIDController->compute();
+        _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
         _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
         
-        //if pilot commanding yaw
-        if(abs(_rcConstrainedCommands[0]) > 5)
+        //If pilot commanding yaw
+        if(abs(_rcMappedCommands[0]) > 0)
         {  
-            _stabPIDControllerOutputs[0] = _rcConstrainedCommands[0];  //Feed to rate PID (overwriting stab PID output)
+            _stabPIDControllerOutputs[0] = _rcMappedCommands[0];  //Feed to rate PID (overwriting stab PID output)
             _yawTarget = _ypr[0];
         }
         
@@ -134,30 +133,32 @@
     }
     
     //Testing
-    //_ratePIDControllerOutputs[0] = 0; // yaw
+    _ratePIDControllerOutputs[0] = 0; // yaw
     //_ratePIDControllerOutputs[1] = 0; // pitch
-    //_ratePIDControllerOutputs[2] = 0; // roll
+    _ratePIDControllerOutputs[2] = 0; // roll
 
     //Calculate motor power if flying
-    if(_rcCommands[3] > 0 && _armed == true)
+    if(_rcMappedCommands[3] > 0.1 && _armed == true)
     {
         //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
-        _motorPower[0] = Constrain((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[1] = Constrain((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[2] = Constrain((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[3] = Constrain((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[0] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[1] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[2] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[3] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
     
         
         //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
-        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1300); //Reduced to 1300 to limit power for testing
-        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1300);
-        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1300);
-        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1300);
+        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1500); //Reduced to 1500 to limit power for testing
+        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1500);
+        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1500);
+        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1500);
     }
 
     //Not flying
     else if(_armed == true)
     {
+        _yawTarget = _ypr[0];
+        
         //Set motors to armed state
         _motorPower[0] = MOTORS_ARMED;
         _motorPower[1] = MOTORS_ARMED;
@@ -195,40 +196,10 @@
     _motor4.pulsewidth_us(_motorPower[3]);
 }
 
-void Task50Hz()
-{
-    //Get RC control values
-    
-    //Constrain
-    //Rate mode
-    if(_rate == true && _stab == false)
-    {
-        _rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
-        _rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
-        _rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
-        _rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
-    }
-    //Stability  
-    else
-    {
-        _rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
-        _rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
-        _rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
-        _rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
-    }
-    
-    //10Hz Task
-    _10HzIterator++;
-    if(_10HzIterator % 5 == 0)
-    {
-        Task10Hz();
-    }
-}
-
 //Print data
 void Task10Hz()
 {
     int batt = 0;
     _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f>",
-    _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
+    _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
 }
\ No newline at end of file

--- a/hardware.h	Fri May 09 10:04:36 2014 +0000
+++ b/hardware.h	Fri May 16 14:18:05 2014 +0000
@@ -22,16 +22,16 @@
 #define IMU_ROLL_RATE_MIN -360
 #define IMU_PITCH_RATE_MAX 360
 #define IMU_PITCH_RATE_MIN -360
-#define RC_YAW_RATE_MAX 360
-#define RC_YAW_RATE_MIN -360
-#define RC_ROLL_RATE_MAX 360
-#define RC_ROLL_RATE_MIN -360
-#define RC_PITCH_RATE_MAX 360
-#define RC_PITCH_RATE_MIN -360
-#define RC_ROLL_ANGLE_MAX 25
-#define RC_ROLL_ANGLE_MIN -25
-#define RC_PITCH_ANGLE_MAX 25
-#define RC_PITCH_ANGLE_MIN -25
+#define RC_YAW_RATE_MAX 90
+#define RC_YAW_RATE_MIN -90
+#define RC_ROLL_RATE_MAX 90
+#define RC_ROLL_RATE_MIN -90
+#define RC_PITCH_RATE_MAX 90
+#define RC_PITCH_RATE_MIN -90
+#define RC_ROLL_ANGLE_MAX 45
+#define RC_ROLL_ANGLE_MIN -45
+#define RC_PITCH_ANGLE_MAX 45
+#define RC_PITCH_ANGLE_MIN -45
 #define RC_THRUST_MAX 1
 #define RC_THRUST_MIN 0
 #define MOTORS_OFF 0
@@ -42,6 +42,8 @@
 #define RATE_PID_CONTROLLER_OUTPUT_MIN -1
 #define UPDATE_FREQUENCY 500
 #define MOTOR_PWM_FREQUENCY 500
+#define RCMIN -127
+#define RCMAX 127
 
 //Shared Variables
 float _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD;
@@ -51,7 +53,7 @@
 float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
 float _motorPower [4] = {0,0,0,0};
 float _ypr[3] = {0,0,0}; // Yaw, pitch, roll
-float _rcCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
+float _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
 bool _armed = false;
 bool _rate = false;
 bool _stab = true;
@@ -74,9 +76,6 @@
 Thread *_serialPortMonitorThread;
 Thread *_flightControllerThread;
 
-//Timers
-RtosTimer *_updateTimer;
-
 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
 // M1  M2
 //  \  /
@@ -98,7 +97,7 @@
 Serial _wirelessSerial(p9, p10);
 
 //PPM
-DigitalIn _ppm(p7);
+InterruptIn _PPMSignal(p7);
 
 //Battery monitor
 DigitalIn _batteryMonitor(p8);

--- a/main.cpp	Fri May 09 10:04:36 2014 +0000
+++ b/main.cpp	Fri May 16 14:18:05 2014 +0000
@@ -8,12 +8,136 @@
 #include "statusLights.h"
 #include "serialPortMonitor.h"
 #include "flightController.h"
+#include "MAF.h"
 
 //Declarations
 void LoadSettingsFromConfig();
 void InitialisePID();
 void InitialisePWM();
 void Setup();
+void SignalRise();
+
+//VERY MESSY NNED TO TIDY
+
+//PPM STUFF
+Timer _PPMTimer;
+const int _numberOfPPMChannels=8; // This is the number of channels in your PPM signal
+unsigned char _currentPPMChannel=0; //This will point to the current channel in PPM frame. 
+int _PPMChannelValues[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channels value is stored until frame is complete.
+unsigned int _PPMChannelTimes[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channel pulse time value is stored until frame is complete.
+float C0s[_numberOfPPMChannels]; // Zeroth order coefficient for converting times to channels
+float C1s[_numberOfPPMChannels]; // First order coefficient for converting times to channels
+int _frameCount=0;
+int i;
+int _timeElapsedBetweenPPMInterrupts =0; //Keeps track of time between PPM interrupts
+int _miniumumSyncTime = 6000; // Minimum time of the sync pulse (us)
+int _shortPulseTime = 800; // If the pulse time for a channel is this short, something is wrong (us)
+int _pulseMin = 1000; // The minimum pulse time for a channel should be this long (us)
+int _pulseMax = 2000; // The maximum pulse time for a channel should be this long (us)
+int _channelOutputMin = RCMIN; // Desired minimum value for outputs
+int _channelOutputMax = RCMAX; // Desired maximum value for outputs
+const int JCCount=4; //Number of joystick channels
+unsigned char JoystickChannels[JCCount]={0,1,2,3}; // List of joystick channels
+char dum1,dum2;
+
+//RC command filters
+MAF _thrustFilter;
+MAF _yawFilter;
+MAF _pitchRateFilter;
+MAF _rollRateFilter;
+MAF _pitchStabFilter;
+MAF _rollStabFilter;
+
+//Here were all the work decoding the PPM signal takes place
+void SignalRise()
+{
+    _timeElapsedBetweenPPMInterrupts = _PPMTimer.read_us(); // Since we are measuring from signal rise to signal rise, note that _timeElapsedBetweenPPMInterrupts includes the fixed separator time as well
+    if (_timeElapsedBetweenPPMInterrupts < _shortPulseTime)
+    { 
+        return; //Channel timing too short; ignore, it's a glitch. Don't move to the next channel
+    }
+    __disable_irq();
+    //_PPMSignal.rise(NULL);
+    _PPMTimer.reset();  
+    if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel != 0))
+    {   
+        //somehow before reaching the end of PPM frame you read   "New" frame signal???
+        //Ok, it happens. Just ignore this frame and start a new one        
+        _currentPPMChannel=0;              
+    }
+    if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel == 0))
+    {
+        // This is good. You've received "New" frame signal as expected
+        __enable_irq();    
+        //_PPMSignal.rise(&SignalRise);   
+        return;
+    }
+ 
+    // Process current channel. This is a correct channel in a correct frame so far
+    _PPMChannelValues[_currentPPMChannel]= C0s[_currentPPMChannel] + C1s[_currentPPMChannel]*_timeElapsedBetweenPPMInterrupts; // Normalize reading (Min: 900us Max: 1900 us). This is my readings, yours can be different
+    _PPMChannelTimes[_currentPPMChannel] = _timeElapsedBetweenPPMInterrupts;
+    _currentPPMChannel++;
+    
+    if (_currentPPMChannel==_numberOfPPMChannels ) 
+    {  
+        // great!, you've a complete correct frame. Set CanUpdate and start a new frame
+        _frameCount++;
+        _currentPPMChannel=0;
+        
+        //Aux Channels
+        float channel5 = map(_PPMChannelValues[4], RCMIN, RCMAX, 0, 1);
+        float channel6 = map(_PPMChannelValues[5], RCMIN, RCMAX, 0, 1);
+        float channel7 = map(_PPMChannelValues[6], RCMIN, RCMAX, 0, 1);
+        float channel8 = map(_PPMChannelValues[7], RCMIN, RCMAX, 0, 1);
+        
+        //Arm
+        if(channel5 > 0.4) 
+        {
+            //Zero gyro
+            _freeIMU.zeroGyro();
+            _armed = true;
+        }
+        else _armed = false;
+        
+        //Mode
+        if(channel6 > 0.4)
+        {
+            _rate = true;
+            _stab = false;
+        }
+        else
+        {
+            _rate = false;
+            _stab = true;
+        }
+        
+        //Filtering
+        float thrust = _thrustFilter.update(map(_PPMChannelValues[2], RCMIN, RCMAX, RC_THRUST_MIN, RC_THRUST_MAX));
+        float yaw = _yawFilter.update(map(_PPMChannelValues[3], RCMIN, RCMAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
+        float rollRate = _rollRateFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
+        float pitchRate = _pitchRateFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX));
+        float rollStab = _rollStabFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
+        float pitchStab = _pitchStabFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX));
+        
+        //Set rc commands
+        //Rate mode
+        if(_rate == true && _stab == false)
+        {       
+            _rcMappedCommands[0] = yaw;
+            _rcMappedCommands[1] = pitchRate;
+            _rcMappedCommands[2] = rollRate;
+            _rcMappedCommands[3] = thrust;
+        }
+        else // Stab mode
+        {
+            _rcMappedCommands[0] = yaw;
+            _rcMappedCommands[1] = pitchStab;
+            _rcMappedCommands[2] = rollStab;
+            _rcMappedCommands[3] = thrust;
+        }            
+    }
+    __enable_irq();
+}
 
 //Loads settings from the config file
 void LoadSettingsFromConfig()
@@ -214,7 +338,7 @@
 //PWM Initialisation
 void InitialisePWM()
 {
-    //
+    //500Hz
     float period = 1.0 / MOTOR_PWM_FREQUENCY;
     _motor1.period(period);
     _motor2.period(period);
@@ -230,7 +354,7 @@
 
 //Setup
 void Setup()
-{
+{   
     //Setup wired serial coms
     _wiredSerial.baud(115200);
     
@@ -241,10 +365,10 @@
     LoadSettingsFromConfig();
     
     //Set initial RC Ccommands
-    _rcCommands[0] = 0;
-    _rcCommands[1] = 0;
-    _rcCommands[2] = 0;
-    _rcCommands[3] = 0;
+    _rcMappedCommands[0] = 0;
+    _rcMappedCommands[1] = 0;
+    _rcMappedCommands[2] = 0;
+    _rcMappedCommands[3] = 0;
 
     //Initialise IMU
     _freeIMU.init(true);
@@ -255,8 +379,32 @@
     //Initialise PWM
     InitialisePWM();
     
+    //Initalise PPM
+    _PPMSignal.mode (PullUp);
+    _PPMSignal.rise(&SignalRise); //Attach SignalRise routine to handle _PPMSignal rise
+    _PPMTimer.start();
+    
+    //Initialize all channels to produce "sane" outputs (depending on your definition of sanity ;-).
+    C1s[0] = 1.0*(_channelOutputMax-_channelOutputMin)/(_pulseMax-_pulseMin);
+    C0s[0] = 1.0*_channelOutputMin - _pulseMin*C1s[0];
+    for (i=1; i<_numberOfPPMChannels; i++)
+    {
+        C1s[i]=C1s[0];
+        C0s[i]=C0s[0];
+    }    
+    
+    // Initialize joystick channels using saved calibration information
+    FILE *fpi = fopen("/local/coefs.txt", "r");  // Open coefficient file on the local file system for reading
+    for(i=0; i<JCCount; i++)
+    {
+        fscanf(fpi, "%f%c%f%c",&C0s[JoystickChannels[i]],&dum1,&C1s[JoystickChannels[i]],&dum2);
+    }
+    fclose(fpi);
+    _PPMTimer.reset();
+    
     //Start new line
     _wiredSerial.printf("\n\r");
+    _wiredSerial.printf("Finished initialising\n\r");
     
     // Start threads
     if(_initialised == true)

--- a/serialPortMonitor.h	Fri May 09 10:04:36 2014 +0000
+++ b/serialPortMonitor.h	Fri May 16 14:18:05 2014 +0000
@@ -101,19 +101,19 @@
             break;
         //Set yaw
         case 'd':
-            _rcCommands[0] = value; //Yaw
+             //_rcMappedCommands[0] = value; //Yaw
             break;
         //Set pitch
         case 'e':
-            _rcCommands[1] = value; //Pitch
+            //_rcMappedCommands[1] = value; //Pitch
             break;
         //Set roll
         case 'f':
-            _rcCommands[2] = value; //Roll
+            //_rcMappedCommands[2] = value; //Roll
             break;
         //Set thrust
         case 'g':
-            _rcCommands[3] = value; //Thrust
+            //_rcMappedCommands[3] = value; //Thrust
             break;
         //Set PID values
         case 'h':
@@ -124,96 +124,92 @@
         case 'i':
             _yawRatePIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'j':
             _yawRatePIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'k':
             _pitchRatePIDControllerP = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'l':
             _pitchRatePIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'm':
             _pitchRatePIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'n':
             _rollRatePIDControllerP = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'o':
             _rollRatePIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'p':
             _rollRatePIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'q':
             _yawStabPIDControllerP = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'r':
             _yawStabPIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 's':
             _yawStabPIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 't':
             _pitchStabPIDControllerP = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'u':
             _pitchStabPIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'v':
             _pitchStabPIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'w':
             _rollStabPIDControllerP = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'x':
             _rollStabPIDControllerI = value;
             UpdatePID();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         case 'y':
             _rollStabPIDControllerD = value;
             UpdatePID();
-            //WriteSettingsToConfig();
-            break;
-        //Get Settings
-        case 'z':
-            //_wiredSerial.printf("%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f\n", _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
+            WriteSettingsToConfig();
             break;
         //Zero pitch and roll
         case '1':
             ZeroPitchRoll();
-            //WriteSettingsToConfig();
+            WriteSettingsToConfig();
             break;
         default:
             break;
@@ -227,147 +223,149 @@
 {
     _wiredSerial.printf("Writing settings to config file\n\r");
     
-    //Pause timer to avoid conflicts
-    //_updateTimer->stop();
-    //_freeIMU.stop_sampling();
-    
-    //Write values
-    ConvertToCharArray(_yawRatePIDControllerP);
-    if (!_configFile.setValue("_yawRatePIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
-    }
-    
-    ConvertToCharArray(_yawRatePIDControllerI);
-    if (!_configFile.setValue("_yawRatePIDControllerI", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
-    }
-    
-    ConvertToCharArray(_yawRatePIDControllerD);
-    if (!_configFile.setValue("_yawRatePIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
-    }
-    
-    ConvertToCharArray(_pitchRatePIDControllerP);
-    if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
-    }
-    
-    ConvertToCharArray(_pitchRatePIDControllerI);
-    if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
-    }
-    
-    ConvertToCharArray(_pitchRatePIDControllerD);
-    if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
-    }
-    
-    ConvertToCharArray(_rollRatePIDControllerP);
-    if (!_configFile.setValue("_rollRatePIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
-    }
-    
-    ConvertToCharArray(_rollRatePIDControllerI);
-    if (!_configFile.setValue("_rollRatePIDControllerI", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
-    }
-    
-    ConvertToCharArray(_rollRatePIDControllerD);
-    if (!_configFile.setValue("_rollRatePIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
-    }
-
-    ConvertToCharArray(_yawStabPIDControllerP);
-    if (!_configFile.setValue("_yawStabPIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
-    }
-    
-    ConvertToCharArray(_yawStabPIDControllerI);
-    if (!_configFile.setValue("_yawStabPIDControllerI", _str))
+    if(_rcMappedCommands[3] < 0) //Not flying
     {
-        _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
-    }
-    
-    ConvertToCharArray(_yawStabPIDControllerD);
-    if (!_configFile.setValue("_yawStabPIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
-    }
-    
-    ConvertToCharArray(_pitchStabPIDControllerP);
-    if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
-    }
-    
-    ConvertToCharArray(_pitchStabPIDControllerI);
-    if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
-    }
-    
-    ConvertToCharArray(_pitchStabPIDControllerD);
-    if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
-    }
-    
-    ConvertToCharArray(_rollStabPIDControllerP);
-    if (!_configFile.setValue("_rollStabPIDControllerP", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
-    }
+        _freeIMU.sample(false);
+        
+        //Write values
+        ConvertToCharArray(_yawRatePIDControllerP);
+        if (!_configFile.setValue("_yawRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_yawRatePIDControllerI);
+        if (!_configFile.setValue("_yawRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_yawRatePIDControllerD);
+        if (!_configFile.setValue("_yawRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerP);
+        if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerI);
+        if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerD);
+        if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerP);
+        if (!_configFile.setValue("_rollRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerI);
+        if (!_configFile.setValue("_rollRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerD);
+        if (!_configFile.setValue("_rollRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
+        }
     
-    ConvertToCharArray(_rollStabPIDControllerI);
-    if (!_configFile.setValue("_rollStabPIDControllerI", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
-    }
+        ConvertToCharArray(_yawStabPIDControllerP);
+        if (!_configFile.setValue("_yawStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_yawStabPIDControllerI);
+        if (!_configFile.setValue("_yawStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_yawStabPIDControllerD);
+        if (!_configFile.setValue("_yawStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerP);
+        if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerI);
+        if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerD);
+        if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerP);
+        if (!_configFile.setValue("_rollStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerI);
+        if (!_configFile.setValue("_rollStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerD);
+        if (!_configFile.setValue("_rollStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
+        }
     
-    ConvertToCharArray(_rollStabPIDControllerD);
-    if (!_configFile.setValue("_rollStabPIDControllerD", _str))
-    {
-        _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
-    }
-
-    ConvertToCharArray(_zeroValues[1]);
-    if (!_configFile.setValue("_zeroPitch", _str))
-    {
-        _wiredSerial.printf("Failed to write value for zero pitch\n\r");
+        ConvertToCharArray(_zeroValues[1]);
+        if (!_configFile.setValue("_zeroPitch", _str))
+        {
+            _wiredSerial.printf("Failed to write value for zero pitch\n\r");
+        }
+        
+        ConvertToCharArray(_zeroValues[2]);
+        if (!_configFile.setValue("_zeroRoll", _str))
+        {
+            _wiredSerial.printf("Failed to write value for zero roll\n\r");
+        }
+        
+        if (!_configFile.write("/local/config.cfg"))
+        {
+            _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
+        }
+        else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
+        
+        _freeIMU.sample(true);
     }
-    
-    ConvertToCharArray(_zeroValues[2]);
-    if (!_configFile.setValue("_zeroRoll", _str))
-    {
-        _wiredSerial.printf("Failed to write value for zero roll\n\r");
-    }
-    
-    if (!_configFile.write("/local/config.cfg"))
+    else
     {
-        _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
+        _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
     }
-    else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
-    
-    //Start timer
-    //int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
-    //_updateTimer->start(updateTime);
-    //_freeIMU.start_sampling();
 }
 
 //Converts float to char array
 void ConvertToCharArray(float number)
 {
-    sprintf(_str, "%1.6f", number );  
+    sprintf(_str, "%1.8f", number );  
 }
 
 //Converts integer to char array
@@ -379,14 +377,12 @@
 //Updates PID tunings
 void UpdatePID()
 {
-    float updateTime = 1.0 / UPDATE_FREQUENCY;
-    
-    _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
-    _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
-    _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
-    _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
-    _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
-    _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
+    _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD);
+    _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD);
+    _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD);
+    _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD);
+    _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD);
+    _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
 }
 
 //Zero gyro, zero yaw and arm
@@ -395,17 +391,6 @@
     //Zero gyro
     _freeIMU.zeroGyro();
     
-    //Zero Yaw
-    int totalYaw = 0;
-    float ypr[3] = {0,0,0}; // Yaw, pitch, roll
-    for(int i = 0; i < 500; i++)
-    {
-        _freeIMU.getYawPitchRoll(ypr);
-        totalYaw += ypr[0];
-    }
-    
-    _zeroValues[0] = totalYaw/500;
-    
     //Set armed to true
     _armed = true;   
 }
@@ -414,8 +399,8 @@
 void ZeroPitchRoll()
 {  
     //Zero pitch and roll
-    int totalPitch = 0;
-    int totalRoll = 0;
+    float totalPitch = 0;
+    float totalRoll = 0;
     float ypr[3] = {0,0,0}; // Yaw, pitch, roll
     for(int i = 0; i < 500; i++)
     {
@@ -426,6 +411,6 @@
     
     _zeroValues[1] = totalPitch/500;
     _zeroValues[2] = totalRoll/500;
-    
-    //WriteSettingsToConfig();
+    printf("Pitch %f\r\n", _zeroValues[1]);
+    printf("Roll %f\r\n", _zeroValues[2]);
 }