An implementation of the Sirf Binary and NMEA Protocol for gps devices using the SiRFstarIII chipset
sIRFstarIII.cpp
- Committer:
- p3p
- Date:
- 2012-06-28
- Revision:
- 0:43da35949666
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); } }