Revision 9:7b194f83e567, committed 2015-02-22

Comitter:

joe4465

Date:

Sun Feb 22 20:10:12 2015 +0000

Parent:

8:5a7efcd0c0dd

Commit message:

Added external magnetometer

Changed in this revision

diff -r 5a7efcd0c0dd -r 7b194f83e567 FreeIMU.lib
--- a/FreeIMU.lib	Tue Feb 10 20:58:12 2015 +0000
+++ b/FreeIMU.lib	Sun Feb 22 20:10:12 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/joe4465/code/FreeIMU/#7d83fc674fb2
+http://developer.mbed.org/users/joe4465/code/FreeIMU_external_magnetometer/#e20c095bfd7a

diff -r 5a7efcd0c0dd -r 7b194f83e567 MODSERIAL.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Sun Feb 22 20:10:12 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68

diff -r 5a7efcd0c0dd -r 7b194f83e567 MaxbotixDriver.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MaxbotixDriver.lib	Sun Feb 22 20:10:12 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/DanielC/code/MaxbotixDriver/#7e65f5077f5a

diff -r 5a7efcd0c0dd -r 7b194f83e567 TinyGPS.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyGPS.lib	Sun Feb 22 20:10:12 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/shimniok/code/TinyGPS/#f522b8bdf987

diff -r 5a7efcd0c0dd -r 7b194f83e567 altitudeMonitor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/altitudeMonitor.h	Sun Feb 22 20:10:12 2015 +0000
@@ -0,0 +1,18 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "hardware.h"
+
+// The altitude monitor thread gets the latest altitude from each sensor and combines into one altitude
+void AltitudeMonitorThread(void const *args) 
+{
+    printf("Altitude monitor thread started\r\n");
+
+    while (true) 
+    {
+        _maxBotixPingAltitude = _maxBotixSensor.read() / 100; // Convert to meters
+        _barometerAltitude = 0;//_freeIMU.getBaroAlt();
+        
+        
+        Thread::wait(100);
+    }
+}

diff -r 5a7efcd0c0dd -r 7b194f83e567 filter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/filter.lib	Sun Feb 22 20:10:12 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/networker/code/filter/#46a72e790df8

diff -r 5a7efcd0c0dd -r 7b194f83e567 flightController.h
--- a/flightController.h	Tue Feb 10 20:58:12 2015 +0000
+++ b/flightController.h	Sun Feb 22 20:10:12 2015 +0000
@@ -8,10 +8,7 @@
 void NotFlying();
 
 //Variables
-float           _yawTarget = 0;
 int             _notFlying = 0; 
-float           _altitude = 0;
-double          _basePower = 0;
 
 //Timers
 RtosTimer       *_flightControllerUpdateTimer;
@@ -184,42 +181,25 @@
 
 void GetAttitude()
 {
-    //Use the 2 spare channels to alter the offset
-    if(_levelOffset == true)
-    {
-        float pitchOffset = /*_channel7MedianFilter->process(*/Map(_rcCommands[6], RC_OUT_MIN, RC_OUT_MAX, -5, 5);//);
-        float rollOffset = /*_channel8MedianFilter->process(*/Map(_rcCommands[7], RC_OUT_MIN, RC_OUT_MAX, -5, 5);//);
-        
-        _zeroValues[1] = _oldZeroValues[1] + pitchOffset;
-        _zeroValues[2] = _oldZeroValues[2] + rollOffset;
-    }
-    else 
-    {
-        _oldZeroValues[1] = _zeroValues[1];
-        _oldZeroValues[2] = _zeroValues[2];
-    }
-    
-    //_zeroValues[1] = _rcCommands[6];
-    //_zeroValues[2] = _rcCommands[7];
     
     //Get raw data from IMU
     _freeIMU.getYawPitchRoll(_ypr);
     _freeIMU.getRate(_gyroRate);
     
-    //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];
+    //Take off zero values to account for any angle inbetween level and ground
+    //_ypr[1] = _ypr[1] - _zeroValues[1];
+    //_ypr[2] = _ypr[2] - _zeroValues[2];
     
     //Swap pitch and roll angle because IMU is mounted at a right angle to the board
-    float yaw = _ypr[0];
     float pitch = _ypr[2];
-    float roll = - _ypr[1]; //Needs to be negative
-    _ypr[0] = yaw;
+    float roll = -_ypr[1];
     _ypr[1] = pitch;
     _ypr[2] = roll;
     
-    //Swap pitch, roll and yaw? rate because IMU is mounted at a right angle to the board
-    yaw = _gyroRate[2];
+    _ypr[0] = _ypr[0];
+    
+    //Swap pitch, roll and yaw rate because IMU is mounted at a right angle to the board
+    float yaw = _gyroRate[2];
     pitch = _gyroRate[0];
     roll = _gyroRate[1];
     _gyroRate[0] = yaw;

diff -r 5a7efcd0c0dd -r 7b194f83e567 hardware.h
--- a/hardware.h	Tue Feb 10 20:58:12 2015 +0000
+++ b/hardware.h	Sun Feb 22 20:10:12 2015 +0000
@@ -4,8 +4,11 @@
 #include "PID.h"
 #include "ConfigFile.h"
 #include "PPM.h"
+#include <sstream>
+#include <TinyGPS.h>
+#include "sonar.h"
+#include "MODSERIAL.h"
 #include "filter.h"
-#include <sstream>
 
 #ifndef HARDWARE_H
 #define HARDWARE_H
@@ -30,16 +33,16 @@
 #define             RC_IN_MIN 1000
 #define             RC_OUT_MAX 1
 #define             RC_OUT_MIN 0
-#define             RC_YAW_RATE_MAX 90
-#define             RC_YAW_RATE_MIN -90
+#define             RC_YAW_RATE_MAX 180
+#define             RC_YAW_RATE_MIN -180
 #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_ROLL_ANGLE_MAX 20
+#define             RC_ROLL_ANGLE_MIN -20
+#define             RC_PITCH_ANGLE_MAX 20
+#define             RC_PITCH_ANGLE_MIN -20
 #define             RC_THRUST_MAX 1
 #define             RC_THRUST_MIN 0
 
@@ -69,12 +72,12 @@
 float               _oldZeroValues[3] = {0,0,0}; //Yaw, pitch, roll
 float               _rcCommands[8] = {0,0,0,0,0,0,0,0};
 float               _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
-float               _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
+double              _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
+bool                _gpsConnected = false;
 bool                _armed = false;
 bool                _rate = false;
 bool                _stab = true;
 bool                _initialised = false;
-bool                _gpsConnected = false;
 float               _motorPower [4] = {0,0,0,0};
 float               _gyroRate[3] = {0,0,0}; // Yaw, Pitch, Roll
 float               _ypr[3] = {0,0,0}; // Yaw, pitch, roll
@@ -83,6 +86,9 @@
 bool                _levelOffset = false;
 int                 _commsMode = 0;
 int                 _batt = 0;
+float               _yawTarget = 0;
+float               _maxBotixPingAltitude = 0;
+float               _barometerAltitude = 0;
     
 //PID controllers
 PID                 *_yawRatePIDController;
@@ -97,6 +103,7 @@
 Thread              *_serialPortMonitorThread;
 Thread              *_flightControllerThread;
 Thread              *_rcCommandMonitorThread;
+Thread              *_altitudeMonitorThread;
 
 //Config file
 LocalFileSystem     local("local");
@@ -104,12 +111,10 @@
 char*               _str = new char[1024];
 
 //RC filters
-//medianFilter        *_yawMedianFilter;
-//medianFilter        *_pitchMedianFilter;
-//medianFilter        *_rollMedianFilter;
-//medianFilter        *_thrustMedianFilter;
-//medianFilter        *_channel7MedianFilter;
-//medianFilter        *_channel8MedianFilter;
+medianFilter        *_yawMedianFilter;
+medianFilter        *_pitchMedianFilter;
+medianFilter        *_rollMedianFilter;
+medianFilter        *_thrustMedianFilter;
 
 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
 // M1  M2
@@ -126,20 +131,18 @@
 PwmOut              _motor4(p24);
 
 //USB serial
-Serial              _wiredSerial(USBTX, USBRX);
+MODSERIAL           _wiredSerial(USBTX, USBRX);
 
 //Wireless Serial
-Serial              _wirelessSerial(p9, p10);
+MODSERIAL           _wirelessSerial(p9, p10);
 
 //GPS Serial
-//Serial            _gpsSerial(p28, p27);
+MODSERIAL           _gps(p13, p14);
+TinyGPS             _tinyGPS;
 
 //PPM in
-PPM *_ppm;
-InterruptIn *_interruptPin = new InterruptIn(p8);
-
-//Battery monitor
-//DigitalIn         _batteryMonitor(p8);
+PPM                 *_ppm;
+InterruptIn         *_interruptPin = new InterruptIn(p8);
 
 //Onboard LED's
 DigitalOut          _led1(LED1);
@@ -147,18 +150,22 @@
 DigitalOut          _led3(LED3);
 DigitalOut          _led4(LED4);
 
-//Outputs
-//DigitalOut          _output1(p11);
-//DigitalOut          _output2(p12);
-//DigitalOut          _output3(p5);
-//DigitalOut          _output4(p6);
-
 //IMU
 FreeIMU             _freeIMU;
 
 //Buzzer
 DigitalOut          _buzzer(p20);
 
+//MaxBotix Ping sensor
+Timer               _maxBotixTimer;
+Sonar               _maxBotixSensor(p15, _maxBotixTimer);
+
+//Unused analog pins
+DigitalOut         _spare1(p16);
+DigitalOut         _spare2(p17);
+DigitalOut         _spare3(p18);
+DigitalOut         _spare4(p19);
+
 //Functions///////////////////////////////////////////////////////////////////////////////////////////////
 //Zero gyro and arm
 void Arm()

diff -r 5a7efcd0c0dd -r 7b194f83e567 main.cpp
--- a/main.cpp	Tue Feb 10 20:58:12 2015 +0000
+++ b/main.cpp	Sun Feb 22 20:10:12 2015 +0000
@@ -9,6 +9,7 @@
 #include "serialPortMonitor.h"
 #include "flightController.h"
 #include "rcCommandMonitor.h"
+#include "altitudeMonitor.h"
 
 //Declarations
 void LoadSettingsFromConfig();
@@ -209,20 +210,16 @@
     //Setup wired serial coms
     _wiredSerial.baud(115200);
     
-    _buzzer = 0;
-    
     printf("\r\n");  
     printf("*********************************************************************************\r\n");
     printf("Starting Setup\r\n");
     printf("*********************************************************************************\r\n");
     
+     //Disable buzzer
+    _buzzer = 0;
+    
     //Setup wireless serial coms
     _wirelessSerial.baud(57600);
-    printf("Setup wireless serial\r\n");
-    
-    //Setup GPS serial comms
-    //_gpsSerial.baud(115200);
-    //printf("Setup GPS serial\r\n");
     
     //Read config file
     LoadSettingsFromConfig();
@@ -232,61 +229,56 @@
     _rcMappedCommands[1] = 0;
     _rcMappedCommands[2] = 0;
     _rcMappedCommands[3] = 0;
-    printf("Setup initial RC commands\r\n");
     
     //Setup RC median filters
-    //_yawMedianFilter = new medianFilter(10);
-    //_pitchMedianFilter = new medianFilter(10);
-    //_rollMedianFilter = new medianFilter(10);
-    //_thrustMedianFilter = new medianFilter(10);
-    //_channel7MedianFilter = new medianFilter(10);
-    //_channel8MedianFilter = new medianFilter(10);
-    printf("Setup RC median filters\r\n");
-    
+    _yawMedianFilter = new medianFilter(5);
+    _pitchMedianFilter = new medianFilter(5);
+    _rollMedianFilter = new medianFilter(5);
+    _thrustMedianFilter = new medianFilter(5);
+
     //Initialise PPM
     _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
-    printf("Setup PPM\r\n");
 
     //Initialise IMU
-    wait(1);
     _freeIMU.init(true);
-    printf("Setup IMU\r\n");
+    
+    //Initialise MaxBotix ping sensor
+    _maxBotixTimer.start();
+    
+    //Initialise GPS
+    _gps.baud(115200);
     
     //Initialise PID
     InitialisePID();
-    printf("Setup PID\r\n");
     
     //Initialise PWM
     InitialisePWM();
-    printf("Setup PWM\r\n");
     
     //Set initialised flag
     _initialised = true;
-    
-    printf("*********************************************************************************\r\n");
-    printf("Finished Setup\r\n");
-    printf("*********************************************************************************\r\n");
        
     // Start threads
     _flightControllerThread = new Thread (FlightControllerThread);
     _flightControllerThread->set_priority(osPriorityRealtime);
     _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
     _rcCommandMonitorThread->set_priority(osPriorityHigh);
+    _altitudeMonitorThread = new Thread (AltitudeMonitorThread);
+    _altitudeMonitorThread->set_priority(osPriorityHigh);
     _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
     _serialPortMonitorThread->set_priority(osPriorityLow);
     _statusThread = new Thread(StatusThread);
     _statusThread->set_priority(osPriorityIdle);
+    
+    Thread::wait(1000);
+    
+    printf("*********************************************************************************\r\n");
+    printf("Finished Setup\r\n");
+    printf("*********************************************************************************\r\n");
 }
 
 int main()
-{
+{   
     Setup(); 
     
-    // Wait here forever
-    Thread::wait(osWaitForever); 
-    
-    /*while(true)
-    {
-        printf("%f\r\n", _ypr[1]);    
-    }*/
+    Thread::wait(osWaitForever);
 }
\ No newline at end of file

diff -r 5a7efcd0c0dd -r 7b194f83e567 medianfilter.lib
--- a/medianfilter.lib	Tue Feb 10 20:58:12 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/networker/code/medianfilter/#9bd415456089

diff -r 5a7efcd0c0dd -r 7b194f83e567 rcCommandMonitor.h
--- a/rcCommandMonitor.h	Tue Feb 10 20:58:12 2015 +0000
+++ b/rcCommandMonitor.h	Sun Feb 22 20:10:12 2015 +0000
@@ -34,10 +34,10 @@
             i = 0;
             
             //Map yaw channel
-            _rcMappedCommands[0] = - /*_yawMedianFilter->process(*/Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);//);
+            _rcMappedCommands[0] = - _yawMedianFilter->process(Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
         
             //Map thust channel
-            _rcMappedCommands[3] = /*_thrustMedianFilter->process(*/Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX);//);
+            _rcMappedCommands[3] = _thrustMedianFilter->process(Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
         
             //Map arm channel.
             if(_rcCommands[4] > 0.5 && _armed == false) Arm();
@@ -62,16 +62,16 @@
             if(_rate == false && _stab == true)//Stability mode
             {
                 //Roll
-                _rcMappedCommands[2] = /*_rollMedianFilter->process(*/Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);//);
+                _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
                 //Pitch
-                _rcMappedCommands[1] = /*_pitchMedianFilter->process(*/-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);//); //Needs to be reverse
+                _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX)); //Needs to be reverse
             }
             else if(_rate == true && _stab == false)//Rate mode
             {
                 //Roll
-                _rcMappedCommands[2] = /*_rollMedianFilter->process(*/Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);//);
+                _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
                 //Pitch
-                _rcMappedCommands[1] = /*_pitchMedianFilter->process(*/-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);//); //Needs to be reverse
+                _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX)); //Needs to be reverse
             }
             else
             {
@@ -84,12 +84,6 @@
             //Transmitter not connected so increase iterator
             i++;
             
-            //Set commands to 0
-            _rcMappedCommands[0] = 0;
-            _rcMappedCommands[1] = 0;
-            _rcMappedCommands[2] = 0;
-            _rcMappedCommands[3] = -1;
-            
             //If connection has been down for 10 loops then assume the connection really is lost
             if(i > 10 && _armed == true) Disarm();
         }

diff -r 5a7efcd0c0dd -r 7b194f83e567 serialPortMonitor.h
--- a/serialPortMonitor.h	Tue Feb 10 20:58:12 2015 +0000
+++ b/serialPortMonitor.h	Sun Feb 22 20:10:12 2015 +0000
@@ -3,15 +3,13 @@
 #include "hardware.h"
 
 //Declarations
-void CheckWirelessSerialCommand();
-//void CheckGPSSerialCommand();
+void CheckSerialCommand();
 void UpdatePID();
+void getGPS();
 
 //Variables
 char _wirelessSerialBuffer[255];
 int _wirelessSerialRxPos = 0;
-//char _gpsSerialBuffer[255];
-//int _gpsSerialRxPos = 0;
 
 // A thread to monitor the serial ports
 void SerialPortMonitorThread(void const *args) 
@@ -20,12 +18,6 @@
     
     while(true)
     {
-        //Print to windows application
-        //int batt = 0;
-        //_wirelessSerial.printf("<%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%d:%d:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f::1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f>",
-        //_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, 0/*_gpsValues[0]*/, 0/* _gpsValues[1]*/, 0/*_gpsValues[2]*/, 0/*_gpsValues[3]*/, 0/*_gpsValues[4]*/, 0/*_gpsConnected*/, _zeroValues[1], _zeroValues[2], _oldZeroValues[1], _oldZeroValues[2]); 
-        //      0               1               2               3            4        5       6       7                   8                       9                                   10                              11  
-        
         //Check comms mode and print correct data back to PC application
         switch(_commsMode)
         {
@@ -55,13 +47,13 @@
             
             //Mapped RC commands   
             case 4:
-                _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f>",
-                _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3]);
+                _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f:RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
+                _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]);
                 break;
             
             //PID Tuning
             case 5:
-                _wirelessSerial.printf("<RYPIDP=%1.2f:RYPIDI=%1.2f:RYPIDD=%1.2f:RPPIDP=%1.2f:RPPIDI=%1.2f:RPPIDD=%1.2f:RRPIDP=%1.2f:RRPIDI=%1.2f:RRPIDD=%1.2f:SYPIDP=%1.2f:SYPIDI=%1.2f:SYPIDD=%1.2f:SPPIDP=%1.2f:SPPIDI=%1.2f:SPPIDD=%1.2f:SRPIDP=%1.2f:SRPIDI=%1.2f:SRPIDD=%1.2f>",
+                _wirelessSerial.printf("<RYPIDP=%1.6f:RYPIDI=%1.6f:RYPIDD=%1.6f:RPPIDP=%1.6f:RPPIDI=%1.6f:RPPIDD=%1.6f:RRPIDP=%1.6f:RRPIDI=%1.6f:RRPIDD=%1.6f:SYPIDP=%1.6f:SYPIDI=%1.6f:SYPIDD=%1.6f:SPPIDP=%1.6f:SPPIDI=%1.6f:SPPIDD=%1.6f:SRPIDP=%1.6f:SRPIDI=%1.6f:SRPIDD=%1.6f>",
                 _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
                 break;
               
@@ -75,23 +67,40 @@
             case 7:
                 _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
                 _zeroValues[0], _zeroValues[1], _zeroValues[2]);
-                break;  
+                break;
             
-            //Raw RC Commands
+            //Rate tuning
             case 8:
-                _wirelessSerial.printf("<RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
-                _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]);
+                //Yaw set point, Yaw actual, Yaw PID output
+                //Pitch set point, Pitch actual, Pitch PID output
+                //Roll set point, Roll actual, Roll PID output
+                _wirelessSerial.printf("<MRCY=%1.2f:RY=%1.2f:RYPID=%1.2f:MRCP=%1.2f:RP=%1.2f:RPPID=%1.2f:MRCR=%1.2f:RR=%1.2f:RRPID=%1.2f>",
+                _rcMappedCommands[0], _gyroRate[0], _ratePIDControllerOutputs[0], _rcMappedCommands[1], _gyroRate[1], _ratePIDControllerOutputs[1], _rcMappedCommands[2], _gyroRate[2], _ratePIDControllerOutputs[2]);
                 break;
                 
+            //Stab tuning  
+            case 9:
+                //Yaw set point, Yaw actual, Yaw PID output
+                //Pitch set point, Pitch actual, Pitch PID output
+                //Roll set point, Roll actual, Roll PID output
+                _wirelessSerial.printf("<MRCY=%1.2f:SY=%1.2f:SYPID=%1.2f:MRCP=%1.2f:SP=%1.2f:SPPID=%1.2f:MRCR=%1.2f:SR=%1.2f:SRPID=%1.2f>",
+                _rcMappedCommands[0], _ypr[0], _stabPIDControllerOutputs[0], _rcMappedCommands[1], _ypr[1], _stabPIDControllerOutputs[1], _rcMappedCommands[2], _ypr[2], _stabPIDControllerOutputs[2]);
+                break;
+            
+            //Altitude    
+            case 10:
+                _wirelessSerial.printf("<GAlt=%1.2f:PAlt=%1.2f:BAlt=%1.2f>",
+                _gpsValues[2], _maxBotixPingAltitude, _barometerAltitude);
+                
             default:
                 break;                  
-        }
+        }  
         
         //Check for wireless serial command
-        if (_wirelessSerial.readable() > 0)
+        while (_wirelessSerial.readable() > 0)
         {
             int c = _wirelessSerial.getc();
-            
+                                                
             switch (c)
             {
                 case 60: // 
@@ -101,7 +110,7 @@
                 case 10: // LF
                 case 13: // CR
                 case 62: // >
-                    CheckWirelessSerialCommand();
+                    CheckSerialCommand();
                     break;
                     
                 default:
@@ -112,44 +121,25 @@
                     }
                     break;
             }
-        } 
-    
+        }                                  
+        
         //Check for GPS serial command
-        /*if (_gpsSerial.readable() > 0)
+        while(_gps.readable() > 0)
         {
-            int c = _gpsSerial.getc();
-            //printf("%c", c);
-            _wiredSerial.putc(c);
-            
-            switch (c)
+            int c = _gps.getc();
+            if(_tinyGPS.encode(c))
             {
-                case 60: // <
-                    _gpsSerialRxPos = 0;
-                    break;
-                
-                case 10: // LF
-                case 13: // CR
-                case 62: // >
-                    CheckGPSSerialCommand();
-                    break;
-                    
-                default:
-                    _gpsSerialBuffer[_gpsSerialRxPos++] = c;
-                    if (_gpsSerialRxPos > 200)
-                    {
-                        _gpsSerialRxPos = 0;
-                        printf("oh no \r\n");
-                    }
-                    break;
+                getGPS();
             }
-        } */
+        }
+        
         Thread::wait(100);
     }
 }
 
 //Checks for a valid command from the serial port and executes it
 //<Command=Value>
-void CheckWirelessSerialCommand()
+void CheckSerialCommand()
 {
     int length = _wirelessSerialRxPos;
     _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
@@ -352,74 +342,22 @@
     return;
 }
 
-//Checks for a valid command from the serial port and executes it
-//<%.2f:%.2f:%.2f:%.2f:%.2f:%d>", latitude, longitude, GnssInfo.altitude.meters(), GnssInfo.course.deg(), GnssInfo.speed.kph(), _connectedFlag);
-/*void CheckGPSSerialCommand()
-{
-    int length = _gpsSerialRxPos;
-    _gpsSerialBuffer[_gpsSerialRxPos] = 0;
-    _gpsSerialRxPos = 0;
-
-    if (length < 1)
-    {
-        return;
-    }
-    
-    printf("command\r\n");
-    
-    //Convert _gpsSerialBuffer to string
-    
-    //Split on :
-    
-    //Check it split into 6 parts
-    
-    //Convert to cirrect format and assign to vars
-    
-    char command = _gpsSerialBuffer[0];
-    double value = 0;
-    if(length > 1)
-    {
-        value = atof((char*)&_gpsSerialBuffer[2]);
-    }
+void getGPS()
+{ 
+    unsigned long fix_age;
+    _tinyGPS.f_get_position(&_gpsValues[0], &_gpsValues[1], &fix_age);
+  
+    _gpsValues[2] = _tinyGPS.f_altitude();
+    _gpsValues[3] = _tinyGPS.f_course();
+    _gpsValues[4] = _tinyGPS.f_speed_kmph();
     
-    switch (command)
-    {
-        //Latitude
-        case 'a':
-            _gpsValues[0] = value;
-            _gpsConnected = true;
-            break;
-        //Longitude
-        case 'b':
-            _gpsValues[1] = value;
-            _gpsConnected = true;
-            break;
-        //Altitude
-        case 'c':
-            _gpsValues[2] = value;
-            _gpsConnected = true;
-            break;
-        //Course
-        case 'd':
-            _gpsValues[3] = value;
-            _gpsConnected = true;
-            break;
-        //Speed
-        case 'e':
-            _gpsValues[4] = value;
-            _gpsConnected = true;
-            break;
-        //Not Connected
-        case 'f':
-            _gpsConnected = false;
-            break;
-        
-        default:
-            break;
-    }
-    
-    return;
-}*/
+    if (fix_age == TinyGPS::GPS_INVALID_AGE)
+      _gpsConnected = false;
+    else if (fix_age > 5000)
+      _gpsConnected = false;
+    else
+      _gpsConnected = true;
+}
 
 //Updates PID tunings
 void UpdatePID()