Diff: serialPortMonitor.h

Revision:

3:82665e39f1ea

Parent:

1:045edcf091f3

Child:

5:7b7db24ef6eb

--- a/serialPortMonitor.h	Fri May 16 14:22:18 2014 +0000
+++ b/serialPortMonitor.h	Thu Sep 18 08:45:46 2014 +0000
@@ -3,71 +3,118 @@
 #include "hardware.h"
 
 //Declarations
-void CheckCommand();
-void ZeroPitchRoll();
+void SerialPortMonitorTask(void const *n);
+void CheckWirelessSerialCommand();
+//void CheckGPSSerialCommand();
 void WriteSettingsToConfig();
 void ConvertToCharArray(float number);
 void ConvertToCharArray(int number);
 void UpdatePID();
-void Arm();
 
 //Variables
 char* _str = new char[1024];
-char _serialBuffer[255];
-int _serialRxPos = 0;
+char _wirelessSerialBuffer[255];
+int _wirelessSerialRxPos = 0;
+char _gpsSerialBuffer[255];
+int _gpsSerialRxPos = 0;
+
+//Timers
+RtosTimer       *_serialPortMonitorUpdateTimer;
 
 // A thread to monitor the serial ports
 void SerialPortMonitorThread(void const *args) 
 {   
-    while(true)
+    //Update Timer
+    _serialPortMonitorUpdateTimer = new RtosTimer(SerialPortMonitorTask, osTimerPeriodic, (void *)0);
+    int updateTime = (1.0 / SERIAL_MONITOR_FREQUENCY) * 1000;
+    _serialPortMonitorUpdateTimer->start(updateTime);
+    
+    // Wait here forever
+    Thread::wait(osWaitForever);
+}
+
+void SerialPortMonitorTask(void const *n)
+{
+    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:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%d>",
+    _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*/); 
+        
+        
+    //Check for wireless serial command
+    if (_wirelessSerial.readable() > 0)
     {
-         //Check for serial command
-        if (_wirelessSerial.readable() > 0)
+        int c = _wirelessSerial.getc();
+        
+        switch (c)
         {
-            int c = _wirelessSerial.getc();
+            case 60: // 
+                _wirelessSerialRxPos = 0;
+                break;
             
-            switch (c)
-            {
-                case 60: // 
-                    _serialRxPos = 0;
-                    break;
+            case 10: // LF
+            case 13: // CR
+            case 62: // >
+                CheckWirelessSerialCommand();
+                break;
                 
-                case 10: // LF
-                case 13: // CR
-                case 62: // >
-                    CheckCommand();
-                    break;
-                    
-                default:
-                    _serialBuffer[_serialRxPos++] = c;
-                    if (_serialRxPos > 200)
-                    {
-                        _serialRxPos = 0;
-                    }
-                    break;
-            }
-        }  
-        Thread::wait(100);
-    }
+            default:
+                _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
+                if (_wirelessSerialRxPos > 200)
+                {
+                    _wirelessSerialRxPos = 0;
+                }
+                break;
+        }
+    } 
+
+    //Check for GPS serial command
+    /*if (_gpsSerial.readable() > 0)
+    {
+        int c = _gpsSerial.getc();
+        //printf("%c", c);
+        _wiredSerial.putc(c);
+        
+        switch (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;
+        }
+    } */
 }
 
 //Checks for a valid command from the serial port and executes it
-void CheckCommand()
+void CheckWirelessSerialCommand()
 {
-    int length = _serialRxPos;
-    _serialBuffer[_serialRxPos] = 0;
-    _serialRxPos = 0;
+    int length = _wirelessSerialRxPos;
+    _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
+    _wirelessSerialRxPos = 0;
 
     if (length < 1)
     {
         return;
     }
     
-    char command = _serialBuffer[0];
+    char command = _wirelessSerialBuffer[0];
     double value = 0;
     if(length > 1)
     {
-        value = atof((char*)&_serialBuffer[2]);
+        value = atof((char*)&_wirelessSerialBuffer[2]);
     }
     
     switch (command)
@@ -83,7 +130,7 @@
             }
             else
             {
-                _armed = false;
+                Disarm();
             }
             break;
         //Set mode
@@ -101,19 +148,19 @@
             break;
         //Set yaw
         case 'd':
-             //_rcMappedCommands[0] = value; //Yaw
+            if(_armed == true) _rcMappedCommands[0] = value; //Yaw
             break;
         //Set pitch
         case 'e':
-            //_rcMappedCommands[1] = value; //Pitch
+            if(_armed == true) _rcMappedCommands[1] = value; //Pitch
             break;
         //Set roll
         case 'f':
-            //_rcMappedCommands[2] = value; //Roll
+            if(_armed == true) _rcMappedCommands[2] = value; //Roll
             break;
         //Set thrust
         case 'g':
-            //_rcMappedCommands[3] = value; //Thrust
+            if(_armed == true) _rcMappedCommands[3] = value; //Thrust
             break;
         //Set PID values
         case 'h':
@@ -218,12 +265,72 @@
     return;
 }
 
+//Checks for a valid command from the serial port and executes it
+/*void CheckGPSSerialCommand()
+{
+    int length = _gpsSerialRxPos;
+    _gpsSerialBuffer[_gpsSerialRxPos] = 0;
+    _gpsSerialRxPos = 0;
+
+    if (length < 1)
+    {
+        return;
+    }
+    
+    printf("command\r\n");
+    
+    char command = _gpsSerialBuffer[0];
+    double value = 0;
+    if(length > 1)
+    {
+        value = atof((char*)&_gpsSerialBuffer[2]);
+    }
+    
+    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;
+}*/
+
 //Saves settings to config file
 void WriteSettingsToConfig()
 {
     _wiredSerial.printf("Writing settings to config file\n\r");
     
-    if(_rcMappedCommands[3] < 0) //Not flying
+    if(_armed == false) //Not flying
     {
         _freeIMU.sample(false);
         
@@ -384,33 +491,3 @@
     _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD);
     _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
 }
-
-//Zero gyro, zero yaw and arm
-void Arm()
-{
-    //Zero gyro
-    _freeIMU.zeroGyro();
-    
-    //Set armed to true
-    _armed = true;   
-}
-
-//Zero pitch and roll
-void ZeroPitchRoll()
-{  
-    //Zero pitch and roll
-    float totalPitch = 0;
-    float totalRoll = 0;
-    float ypr[3] = {0,0,0}; // Yaw, pitch, roll
-    for(int i = 0; i < 500; i++)
-    {
-        _freeIMU.getYawPitchRoll(ypr);
-        totalPitch += ypr[1];
-        totalRoll += ypr[2];
-    }
-    
-    _zeroValues[1] = totalPitch/500;
-    _zeroValues[2] = totalRoll/500;
-    printf("Pitch %f\r\n", _zeroValues[1]);
-    printf("Roll %f\r\n", _zeroValues[2]);
-}