File content as of revision 7:bc5822aa8878:

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

//Declarations
void CheckWirelessSerialCommand();
//void CheckGPSSerialCommand();
void UpdatePID();

//Variables
char _wirelessSerialBuffer[255];
int _wirelessSerialRxPos = 0;
//char _gpsSerialBuffer[255];
//int _gpsSerialRxPos = 0;

// A thread to monitor the serial ports
void SerialPortMonitorThread(void const *args) 
{     
    printf("Serial port monitor thread started\r\n");
    
    while(true)
    {
        //Print to windows application
        //int batt = 0;
        //_wirelessSerial.printf("<%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%d:%d:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f::1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f>",
        //_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*/, _zeroValues[1], _zeroValues[2], _oldZeroValues[1], _oldZeroValues[2]); 
        //      0               1               2               3            4        5       6       7                   8                       9                                   10                              11  
        
        //Check comms mode and print correct data back to PC application
        switch(_commsMode)
        {
            //Motor power
            case 0:
                _wirelessSerial.printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>",
                _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3]);
                break;
            
            //PID outputs
            case 1:
                _wirelessSerial.printf("<SYPID=%1.2f:SPPID=%1.2f:SRPID=%1.2f:RYPID=%1.2f:RPPID=%1.2f:RRPID=%1.2f>",
                _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2]);
                break;
            
            //IMU outputs 
            case 2:
                _wirelessSerial.printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>",
                _ypr[0], _ypr[1], _ypr[2], _gyroRate[0], _gyroRate[1], _gyroRate[2]);
                break;
            
            //Status  
            case 3:
                _wirelessSerial.printf("<Batt=%d:Armed=%d:Init=%d:Rate=%d:Stab=%d:Lev=%d>",
                _batt, _armed, _initialised, _rate, _stab, _levelOffset);
                break;
            
            //Mapped RC commands   
            case 4:
                _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f>",
                _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3]);
                break;
            
            //PID Tuning
            case 5:
                _wirelessSerial.printf("<RYPIDP=%1.2f:RYPIDI=%1.2f:RYPIDD=%1.2f:RPPIDP=%1.2f:RPPIDI=%1.2f:RPPIDD=%1.2f:RRPIDP=%1.2f:RRPIDI=%1.2f:RRPIDD=%1.2f:SYPIDP=%1.2f:SYPIDI=%1.2f:SYPIDD=%1.2f:SPPIDP=%1.2f:SPPIDI=%1.2f:SPPIDD=%1.2f:SRPIDP=%1.2f:SRPIDI=%1.2f:SRPIDD=%1.2f>",
                _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
                break;
              
            //GPS  
            case 6:
                _wirelessSerial.printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GAng=%1.2f:GSpd=%1.2f:GInit=%d>",
                _gpsValues[0], _gpsValues[1], _gpsValues[2], _gpsValues[3], _gpsValues[4], _gpsConnected);
                break;
            
            //Zero mode  
            case 7:
                _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
                _zeroValues[0], _zeroValues[1], _zeroValues[2]);
                break;  
            
            //Raw RC Commands
            case 8:
                _wirelessSerial.printf("<RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
                _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]);
                break;
                
            default:
                break;                  
        }
        
        //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;
            }
        } */
        Thread::wait(100);
    }
}

//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)
    {
        //Start level offset mode to teach quad level
        case 'a':
            _levelOffset = true;
            break;
            
        //Arm disarm
        case 'b':
            if(_initialised == true && _armed == false)
            {
                Arm();
            }
            else if(_armed == true)
            {
                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
            else _rcMappedCommands[0] = 0;
            break;
            
        //Set pitch
        case 'e':
            if(_armed == true) _rcMappedCommands[1] = value; //Pitch
            else _rcMappedCommands[1] = 0;
            break;
            
        //Set roll
        case 'f':
            if(_armed == true) _rcMappedCommands[2] = value; //Roll
            else _rcMappedCommands[2] = 0;
            break;
            
        //Set thrust
        case 'g':
            if(_armed == true) _rcMappedCommands[3] = value; //Thrust
            else _rcMappedCommands[3] = 0;
            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;
            
        case 'z':
            _commsMode = value;
            break;
            
        case '1':
            _levelOffset = false;
            _zeroValues[0] = 0;
            _zeroValues[1] = 0;
            _zeroValues[2] = 0;
            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;
}*/

//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);
}