File content as of revision 5:7b7db24ef6eb:

#include "mbed.h"
#include "rtos.h"
#include "hardware.h"

//Declarations
void SerialPortMonitorTask(void const *n);
void CheckWirelessSerialCommand();
//void CheckGPSSerialCommand();
void WriteSettingsToConfig();
void ConvertToCharArray(float number);
void ConvertToCharArray(int number);
void UpdatePID();

//Variables
char* _str = new char[1024];
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) 
{   
    //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)
{
    //Print to windows application
    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)
    {
        int c = _wirelessSerial.getc();
        
        switch (c)
        {
            case 60: // 
                _wirelessSerialRxPos = 0;
                break;
            
            case 10: // LF
            case 13: // CR
            case 62: // >
                CheckWirelessSerialCommand();
                break;
                
            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
//<Command=Value>
void CheckWirelessSerialCommand()
{
    int length = _wirelessSerialRxPos;
    _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
    _wirelessSerialRxPos = 0;

    if (length < 1)
    {
        return;
    }
    
    char command = _wirelessSerialBuffer[0];
    double value = 0;
    if(length > 1)
    {
        value = atof((char*)&_wirelessSerialBuffer[2]);
    }
    
    switch (command)
    {
        //Spare
        case 'a':
            break;
        //Arm disarm
        case 'b':
            if(_initialised == true && _armed == false)
            {
                Arm();
            }
            else
            {
                Disarm();
            }
            break;
        //Set mode
        case 'c':
            if(_rate == true)
            {
                _rate = false;
                _stab = true;
            }
            else
            {
                _rate = true;
                _stab = false;
            }
            break;
        //Set yaw
        case 'd':
            if(_armed == true) _rcMappedCommands[0] = value; //Yaw
            break;
        //Set pitch
        case 'e':
            if(_armed == true) _rcMappedCommands[1] = value; //Pitch
            break;
        //Set roll
        case 'f':
            if(_armed == true) _rcMappedCommands[2] = value; //Roll
            break;
        //Set thrust
        case 'g':
            if(_armed == true) _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;
}

//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]);
    }
    
    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(_armed == false) //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);
}