Diff: serialPortMonitor.h

Revision:

9:7b194f83e567

Parent:

7:bc5822aa8878

--- 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()