File content as of revision 1:045edcf091f3:

#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':
             //_rcMappedCommands[0] = value; //Yaw
            break;
        //Set pitch
        case 'e':
            //_rcMappedCommands[1] = value; //Pitch
            break;
        //Set roll
        case 'f':
            //_rcMappedCommands[2] = value; //Roll
            break;
        //Set thrust
        case 'g':
            //_rcMappedCommands[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;
        //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");
    
    if(_rcMappedCommands[3] < 0) //Not flying
    {
        _freeIMU.sample(false);
        
        //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");
        
        _freeIMU.sample(true);
    }
    else
    {
        _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
    }
}

//Converts float to char array
void ConvertToCharArray(float number)
{
    sprintf(_str, "%1.8f", number );  
}

//Converts integer to char array
void ConvertToCharArray(int number)
{
    sprintf(_str, "%d", number );  
}

//Updates PID tunings
void UpdatePID()
{
    _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD);
    _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD);
    _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD);
    _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD);
    _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]);
}