File content as of revision 0:0010a5abcc31:

#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();
}