Diff: serialPortMonitor.h

Revision:

0:0010a5abcc31

Child:

1:045edcf091f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serialPortMonitor.h	Fri May 09 10:04:36 2014 +0000
@@ -0,0 +1,431 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "hardware.h"
+
+//Declarations
+void CheckCommand();
+void ZeroPitchRoll();
+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;
+
+// A thread to monitor the serial ports
+void SerialPortMonitorThread(void const *args) 
+{   
+    while(true)
+    {
+         //Check for serial command
+        if (_wirelessSerial.readable() > 0)
+        {
+            int c = _wirelessSerial.getc();
+            
+            switch (c)
+            {
+                case 60: // 
+                    _serialRxPos = 0;
+                    break;
+                
+                case 10: // LF
+                case 13: // CR
+                case 62: // >
+                    CheckCommand();
+                    break;
+                    
+                default:
+                    _serialBuffer[_serialRxPos++] = c;
+                    if (_serialRxPos > 200)
+                    {
+                        _serialRxPos = 0;
+                    }
+                    break;
+            }
+        }  
+        Thread::wait(100);
+    }
+}
+
+//Checks for a valid command from the serial port and executes it
+void CheckCommand()
+{
+    int length = _serialRxPos;
+    _serialBuffer[_serialRxPos] = 0;
+    _serialRxPos = 0;
+
+    if (length < 1)
+    {
+        return;
+    }
+    
+    char command = _serialBuffer[0];
+    double value = 0;
+    if(length > 1)
+    {
+        value = atof((char*)&_serialBuffer[2]);
+    }
+    
+    switch (command)
+    {
+        //Spare
+        case 'a':
+            break;
+        //Arm disarm
+        case 'b':
+            if(_initialised == true && _armed == false)
+            {
+                Arm();
+            }
+            else
+            {
+                _armed = false;
+            }
+            break;
+        //Set mode
+        case 'c':
+            if(_rate == true)
+            {
+                _rate = false;
+                _stab = true;
+            }
+            else
+            {
+                _rate = true;
+                _stab = false;
+            }
+            break;
+        //Set yaw
+        case 'd':
+            _rcCommands[0] = value; //Yaw
+            break;
+        //Set pitch
+        case 'e':
+            _rcCommands[1] = value; //Pitch
+            break;
+        //Set roll
+        case 'f':
+            _rcCommands[2] = value; //Roll
+            break;
+        //Set thrust
+        case 'g':
+            _rcCommands[3] = value; //Thrust
+            break;
+        //Set PID values
+        case 'h':
+            _yawRatePIDControllerP = value;
+            UpdatePID();
+            WriteSettingsToConfig();
+            break;
+        case 'i':
+            _yawRatePIDControllerI = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'j':
+            _yawRatePIDControllerD = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'k':
+            _pitchRatePIDControllerP = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'l':
+            _pitchRatePIDControllerI = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'm':
+            _pitchRatePIDControllerD = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'n':
+            _rollRatePIDControllerP = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'o':
+            _rollRatePIDControllerI = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'p':
+            _rollRatePIDControllerD = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'q':
+            _yawStabPIDControllerP = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'r':
+            _yawStabPIDControllerI = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 's':
+            _yawStabPIDControllerD = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 't':
+            _pitchStabPIDControllerP = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'u':
+            _pitchStabPIDControllerI = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'v':
+            _pitchStabPIDControllerD = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'w':
+            _rollStabPIDControllerP = value;
+            UpdatePID();
+            //WriteSettingsToConfig();
+            break;
+        case 'x':
+            _rollStabPIDControllerI = value;
+            UpdatePID();
+            //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);
+            break;
+        //Zero pitch and roll
+        case '1':
+            ZeroPitchRoll();
+            //WriteSettingsToConfig();
+            break;
+        default:
+            break;
+    }
+    
+    return;
+}
+
+//Saves settings to config file
+void WriteSettingsToConfig()
+{
+    _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))
+    {
+        _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(_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");
+    
+    //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 );  
+}
+
+//Converts integer to char array
+void ConvertToCharArray(int number)
+{
+    sprintf(_str, "%d", number );  
+}
+
+//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);
+}
+
+//Zero gyro, zero yaw and arm
+void Arm()
+{
+    //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;   
+}
+
+//Zero pitch and roll
+void ZeroPitchRoll()
+{  
+    //Zero pitch and roll
+    int totalPitch = 0;
+    int 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;
+    
+    //WriteSettingsToConfig();
+}