Chris Pepper
An implementation of the Sirf Binary and NMEA Protocol for gps devices using the SiRFstarIII chipset
Diff: sIRFstarIII.cpp
- Revision:
- 0:43da35949666
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sIRFstarIII.cpp Thu Jun 28 21:17:29 2012 +0000
@@ -0,0 +1,373 @@
+#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);
+}
+
+}
\ No newline at end of file