File content as of revision 0:43da35949666:

#include "sIRFstarIII.h"

extern Serial debug;

namespace SirfStarIII {

SirfStarIII::SirfStarIII(PinName tx, PinName rx) : SimpleSerialProtocol::Protocol(tx, rx, LED1) {
    _mode = BINARY;
}

SirfStarIII::~SirfStarIII() {

}

void SirfStarIII::initialise() {
    //find the device
    selectMode(_mode, 57600, true);
    _receive_timeout.start();
}

void SirfStarIII::initialise(ProtocolMode mode, uint32_t baud_rate) {
    selectMode(mode, baud_rate, true);
    _receive_timeout.start();
}

void SirfStarIII::receive() {
    if (_mode == BINARY) {
        receiveBinary();
    } else if (_mode == NMEA) {
        receiveNMEA();
    }
}

void SirfStarIII::sendPacket(SimpleSerialProtocol::Packet* packet) {
    if (_mode == BINARY) {
        sendBinaryPacket(packet);
    } else if (_mode == NMEA) {
        sendNMEAPacket(packet);
    }
}

void SirfStarIII::receiveBinary() {
    uint8_t new_byte = 0;
    uint16_t cs = 0;
    _receive_timeout.reset();
    while ( MODSERIAL::rxBufferGetCount() > 0 &&  _receive_timeout.read_us() < 50 ) {
        switch (_state) {
            case PACKET_START:
                new_byte = MODSERIAL::getc();
                if (_last_byte == 0xA0 && new_byte == 0xA2) {
                    _state = HEADER_RECEIVE;
                }
                _last_byte = new_byte;
                break;

            case HEADER_RECEIVE:
                _header[_header_read++] = MODSERIAL::getc();
                if (_header_read == 2) {
                    _state = HEADER_DECODE;
                }
                break;

            case HEADER_DECODE:
                _packet._size = *(uint16_t*) _header;
                _packet._size = _packet.swapEndian(_packet._size);
                if (_packet._size < 512) {
                    _state = DATA_RECEIVE;
                } else {
                    _state = PACKET_RESET;
                }
                break;

            case DATA_RECEIVE:
                if (_data_read < _packet._size) {
                    _packet._data[_data_read++] = MODSERIAL::getc();
                    if (_data_read == _packet._size) {
                        _state = FOOTER_RECEIVE;
                    }
                }
                break;

            case FOOTER_RECEIVE:
                if (_footer_read < 4) {
                    _footer[_footer_read++] =  MODSERIAL::getc();
                } else {
                    _state = DATA_VALIDATE;
                }
                break;

            case DATA_VALIDATE:
                if (_footer[2] == 0xB0 && _footer[3] == 0xB3) {

                    _packet._checksum =  *(uint16_t*) _footer;
                    _packet._checksum = _packet.swapEndian(_packet._checksum);

                    cs = checksumBinary(_packet._data, _packet._size);

                    if (cs == _packet._checksum) {
                        _packet._type = _packet._data[0];
                        _state = PACKET_VALID;
                        _packet._valid = true;
                    } else {
                        _state = PACKET_RESET;
                    }
                } else {
                    _state = PACKET_RESET;
                }
                break;

            case PACKET_VALID:
                if (!_packet._valid) {
                    _state = PACKET_RESET;
                } else {
                    return;
                }
                return;

            default:
                _state = PACKET_START;
                _header_read = 0;
                _data_read = 0;
                _footer_read = 0;
                break;
        }
    }
}

void SirfStarIII::sendBinaryPacket(SimpleSerialProtocol::Packet* packet) {
    if (packet!=0) {
        send(0xA0);
        send(0xA2);

        //size (reversed 2 byte value)
        uint8_t* size_array = reinterpret_cast<uint8_t *>(&packet->_size);
        send( size_array[1] );
        send( size_array[0] );

        //packet data
        for (int i = 0; i < packet->_size; i++) {
            send(packet->_data[i]);
        }

        //checksum (reversed 2 byte value)
        uint16_t check_value = checksumBinary(packet->_data,  packet->_size);
        uint8_t* check_array = reinterpret_cast<uint8_t *>( &check_value );
        send( check_array[1] );
        send( check_array[0] );

        //end bytes
        send(0xB0);
        send(0xB3);
               
    }
}

uint16_t SirfStarIII::checksumBinary(uint8_t* packet, uint16_t packet_size) {
    uint16_t cs = 0;
    for (int i = 0; i < packet_size; i++) {
        cs += packet[i];
        cs &= 32767;
    }
    return cs;
}

void SirfStarIII::receiveNMEA() {
    uint8_t new_byte = 0;
    uint16_t cs = 0;
    char checksum_array[2];

    _receive_timeout.reset();
    while (MODSERIAL::rxBufferGetCount() > 0 &&  _receive_timeout.read_us() < 50) {

        switch (_state) {
            case PACKET_START:
                new_byte = MODSERIAL::getc();
                if (new_byte == '$') {
                    _state = DATA_RECEIVE;
                }
                break;

            case DATA_RECEIVE:
                new_byte = MODSERIAL::getc();
                if (new_byte != '\r') {
                    _packet._data[_data_read++] = new_byte;
                } else {
                    _packet._size = _data_read - 3; //checksum is last 3 bytes
                    _state = FOOTER_RECEIVE;
                }
                break;

            case FOOTER_RECEIVE:
                //discard '\n"
                new_byte = MODSERIAL::getc();
                _state = DATA_VALIDATE;
                break;

            case DATA_VALIDATE:
                _packet._data[_packet._size] = 0;
                checksum_array[0] = _packet._data[_packet._size + 1];
                checksum_array[1] = _packet._data[_packet._size + 2];
                cs = (uint16_t)strtoul(checksum_array, 0, 16);

                if (cs == checksumNMEA((const char *)_packet._data)) {
                    _state = PACKET_VALID;
                    _packet._valid = true;
                    std::string raw_data( (char *) _packet._data);
                    size_t pos = raw_data.find(",", 0);
                    std::string temp = raw_data.substr(0, pos);

                    if (!temp.compare("GPGGA")) {
                        _packet._type = NMEAPacket::ID_GGA;
                    } else if (!temp.compare("GPGLL")) {
                        _packet._type = NMEAPacket::ID_GLL;
                    } else if (!temp.compare("GPGSA")) {
                        _packet._type = NMEAPacket::ID_GSA;
                    } else if (!temp.compare("GPGSV")) {
                        _packet._type = NMEAPacket::ID_GSV;
                    } else if (!temp.compare("GPMSS")) { 
                        _packet._type = NMEAPacket::ID_MSS;
                    } else if (!temp.compare("GPRMC")) {
                        _packet._type = NMEAPacket::ID_RMC;
                    } else if (!temp.compare("GPVTG")) {
                        _packet._type = NMEAPacket::ID_VTG;
                    } else if (!temp.compare("GPZDA")) {
                        _packet._type = NMEAPacket::ID_ZDA;
                    } else if (!temp.compare("PSRF150")) {
                        _packet._type = NMEAPacket::ID_150;
                    } else {
                        _packet._type = 255;
                    }
                } else {
                    _state = PACKET_RESET;
                }
                break;

            case PACKET_VALID:
                if (!_packet._valid) {
                    _state = PACKET_RESET;
                } else {
                    return;
                }

            default:
                _state = PACKET_START;
                _data_read = 0;
                break;
        }
    }
}

void SirfStarIII::sendNMEAPacket(SimpleSerialProtocol::Packet* packet) {

}

uint8_t SirfStarIII::checksumNMEA(const char * command) {
    uint8_t i = 1;
    uint8_t command_checksum = command[0];
    while (command[i] != 0) {
        command_checksum = command_checksum ^ command[i];
        i++;
    }
    return command_checksum;
}

void  SirfStarIII::selectMode(ProtocolMode mode, uint32_t baud_rate, bool find_baud) {
    int default_baud_rate[6];
    default_baud_rate[0] = 4800;
    default_baud_rate[1] = 9600;
    default_baud_rate[2] = 14400;
    default_baud_rate[3] = 19200;
    default_baud_rate[4] = 38400;
    default_baud_rate[5] = 57600;

    _mode = mode;
/*
    if (find_baud) {
        char command[] = "PSRF100,0,%d,8,1,0\0";
        char buffer[sizeof(command) + 10];
        snprintf(buffer, sizeof(buffer), command, baud_rate );
        
        uint8_t c_checksum = checksumNMEA(buffer);
        //send NMEA command at all baud rates to switch to BINARY @ 57600
        for (int i = 0; i < 6; ++i) {
            Protocol::baud(default_baud_rate[i]);
            wait_ms(100);
            Protocol::printf("$%s*%02X\r\n", buffer, c_checksum);
            blockUntilTxEmpty();
        }

        //send Binary command at all baud rates to switch to 57600
        for (int i = 0; i < 6; ++i) {
            Protocol::baud(default_baud_rate[i]);
            wait_ms(100);
            BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
            binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
            sendBinaryPacket(&binary_config);
            blockUntilTxEmpty();
            
            BinaryPacket::ConfigureNMEA nmea_config(baud_rate);
            nmea_config.buildData<BinaryPacket::ConfigureNMEA::Interface>(&nmea_config.interface);
            sendBinaryPacket(&nmea_config);
            blockUntilTxEmpty();
        }

        //should be binary @ baud now
    }
    wait_ms(50);
    */
    if (mode == BINARY) {
        baud(4800);
        wait_ms(100);
        //Build the NMEA packet
        char command[] = "PSRF100,0,%d,8,1,0\0";
        char buffer[sizeof(command) + 10];
        snprintf(buffer, sizeof(buffer), command, baud_rate );
        uint8_t c_checksum = checksumNMEA(buffer);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        wait_ms(10);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        wait_ms(10);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        wait_ms(10);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        wait_ms(10);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        wait_ms(10);
        Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
        blockUntilTxEmpty();
    }
/*
    if (mode == NMEA) {
        Protocol::baud(4800);

        NMEAChangeBaud(baud_rate);
        wait_ms(10);

        BinaryPacket::ConfigureNMEA nmea_config(baud_rate);
        nmea_config.buildData<BinaryPacket::ConfigureNMEA::Interface>(&nmea_config.interface);
        sendBinaryPacket(&nmea_config);
        
        BinaryPacket::SetProtocol protocol_select(2);
        protocol_select.buildData<BinaryPacket::SetProtocol::Interface>(&protocol_select.interface);
        sendBinaryPacket(&protocol_select);
        
        BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
        binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
        sendBinaryPacket(&binary_config);
        
       blockUntilTxEmpty();*/
       baud(baud_rate);
  //  }
}

void SirfStarIII::NMEAChangeBaud(uint32_t baud_rate){
    //Build the NMEA packet
    char command[] = "PSRF100,1,%d,8,1,0";
    char buffer[sizeof(command) + 10];
    snprintf(buffer, sizeof(buffer), command, baud_rate );
    uint8_t c_checksum = checksumNMEA(buffer);
    
    Protocol::printf("$%s*%02X\r\n", buffer, c_checksum);
    baud(9600);
}

void SirfStarIII::BinaryChangeBaud(uint32_t baud_rate){
    BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
    binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
    sendBinaryPacket(&binary_config);
    blockUntilTxEmpty();
    baud(baud_rate);
}

}