Chris Pepper / sIRFstarIII

An implementation of the Sirf Binary and NMEA Protocol for gps devices using the SiRFstarIII chipset

sIRFstarIII.cpp@0:43da35949666, 2012-06-28 (annotated)

Committer:
p3p
Date:
Thu Jun 28 21:17:29 2012 +0000
Revision:
0:43da35949666
update to baud selection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
p3p 0:43da35949666 1 #include "sIRFstarIII.h"
p3p 0:43da35949666 2
p3p 0:43da35949666 3 extern Serial debug;
p3p 0:43da35949666 4
p3p 0:43da35949666 5 namespace SirfStarIII {
p3p 0:43da35949666 6
p3p 0:43da35949666 7 SirfStarIII::SirfStarIII(PinName tx, PinName rx) : SimpleSerialProtocol::Protocol(tx, rx, LED1) {
p3p 0:43da35949666 8 _mode = BINARY;
p3p 0:43da35949666 9 }
p3p 0:43da35949666 10
p3p 0:43da35949666 11 SirfStarIII::~SirfStarIII() {
p3p 0:43da35949666 12
p3p 0:43da35949666 13 }
p3p 0:43da35949666 14
p3p 0:43da35949666 15 void SirfStarIII::initialise() {
p3p 0:43da35949666 16 //find the device
p3p 0:43da35949666 17 selectMode(_mode, 57600, true);
p3p 0:43da35949666 18 _receive_timeout.start();
p3p 0:43da35949666 19 }
p3p 0:43da35949666 20
p3p 0:43da35949666 21 void SirfStarIII::initialise(ProtocolMode mode, uint32_t baud_rate) {
p3p 0:43da35949666 22 selectMode(mode, baud_rate, true);
p3p 0:43da35949666 23 _receive_timeout.start();
p3p 0:43da35949666 24 }
p3p 0:43da35949666 25
p3p 0:43da35949666 26 void SirfStarIII::receive() {
p3p 0:43da35949666 27 if (_mode == BINARY) {
p3p 0:43da35949666 28 receiveBinary();
p3p 0:43da35949666 29 } else if (_mode == NMEA) {
p3p 0:43da35949666 30 receiveNMEA();
p3p 0:43da35949666 31 }
p3p 0:43da35949666 32 }
p3p 0:43da35949666 33
p3p 0:43da35949666 34 void SirfStarIII::sendPacket(SimpleSerialProtocol::Packet* packet) {
p3p 0:43da35949666 35 if (_mode == BINARY) {
p3p 0:43da35949666 36 sendBinaryPacket(packet);
p3p 0:43da35949666 37 } else if (_mode == NMEA) {
p3p 0:43da35949666 38 sendNMEAPacket(packet);
p3p 0:43da35949666 39 }
p3p 0:43da35949666 40 }
p3p 0:43da35949666 41
p3p 0:43da35949666 42 void SirfStarIII::receiveBinary() {
p3p 0:43da35949666 43 uint8_t new_byte = 0;
p3p 0:43da35949666 44 uint16_t cs = 0;
p3p 0:43da35949666 45 _receive_timeout.reset();
p3p 0:43da35949666 46 while ( MODSERIAL::rxBufferGetCount() > 0 && _receive_timeout.read_us() < 50 ) {
p3p 0:43da35949666 47 switch (_state) {
p3p 0:43da35949666 48 case PACKET_START:
p3p 0:43da35949666 49 new_byte = MODSERIAL::getc();
p3p 0:43da35949666 50 if (_last_byte == 0xA0 && new_byte == 0xA2) {
p3p 0:43da35949666 51 _state = HEADER_RECEIVE;
p3p 0:43da35949666 52 }
p3p 0:43da35949666 53 _last_byte = new_byte;
p3p 0:43da35949666 54 break;
p3p 0:43da35949666 55
p3p 0:43da35949666 56 case HEADER_RECEIVE:
p3p 0:43da35949666 57 _header[_header_read++] = MODSERIAL::getc();
p3p 0:43da35949666 58 if (_header_read == 2) {
p3p 0:43da35949666 59 _state = HEADER_DECODE;
p3p 0:43da35949666 60 }
p3p 0:43da35949666 61 break;
p3p 0:43da35949666 62
p3p 0:43da35949666 63 case HEADER_DECODE:
p3p 0:43da35949666 64 _packet._size = *(uint16_t*) _header;
p3p 0:43da35949666 65 _packet._size = _packet.swapEndian(_packet._size);
p3p 0:43da35949666 66 if (_packet._size < 512) {
p3p 0:43da35949666 67 _state = DATA_RECEIVE;
p3p 0:43da35949666 68 } else {
p3p 0:43da35949666 69 _state = PACKET_RESET;
p3p 0:43da35949666 70 }
p3p 0:43da35949666 71 break;
p3p 0:43da35949666 72
p3p 0:43da35949666 73 case DATA_RECEIVE:
p3p 0:43da35949666 74 if (_data_read < _packet._size) {
p3p 0:43da35949666 75 _packet._data[_data_read++] = MODSERIAL::getc();
p3p 0:43da35949666 76 if (_data_read == _packet._size) {
p3p 0:43da35949666 77 _state = FOOTER_RECEIVE;
p3p 0:43da35949666 78 }
p3p 0:43da35949666 79 }
p3p 0:43da35949666 80 break;
p3p 0:43da35949666 81
p3p 0:43da35949666 82 case FOOTER_RECEIVE:
p3p 0:43da35949666 83 if (_footer_read < 4) {
p3p 0:43da35949666 84 _footer[_footer_read++] = MODSERIAL::getc();
p3p 0:43da35949666 85 } else {
p3p 0:43da35949666 86 _state = DATA_VALIDATE;
p3p 0:43da35949666 87 }
p3p 0:43da35949666 88 break;
p3p 0:43da35949666 89
p3p 0:43da35949666 90 case DATA_VALIDATE:
p3p 0:43da35949666 91 if (_footer[2] == 0xB0 && _footer[3] == 0xB3) {
p3p 0:43da35949666 92
p3p 0:43da35949666 93 _packet._checksum = *(uint16_t*) _footer;
p3p 0:43da35949666 94 _packet._checksum = _packet.swapEndian(_packet._checksum);
p3p 0:43da35949666 95
p3p 0:43da35949666 96 cs = checksumBinary(_packet._data, _packet._size);
p3p 0:43da35949666 97
p3p 0:43da35949666 98 if (cs == _packet._checksum) {
p3p 0:43da35949666 99 _packet._type = _packet._data[0];
p3p 0:43da35949666 100 _state = PACKET_VALID;
p3p 0:43da35949666 101 _packet._valid = true;
p3p 0:43da35949666 102 } else {
p3p 0:43da35949666 103 _state = PACKET_RESET;
p3p 0:43da35949666 104 }
p3p 0:43da35949666 105 } else {
p3p 0:43da35949666 106 _state = PACKET_RESET;
p3p 0:43da35949666 107 }
p3p 0:43da35949666 108 break;
p3p 0:43da35949666 109
p3p 0:43da35949666 110 case PACKET_VALID:
p3p 0:43da35949666 111 if (!_packet._valid) {
p3p 0:43da35949666 112 _state = PACKET_RESET;
p3p 0:43da35949666 113 } else {
p3p 0:43da35949666 114 return;
p3p 0:43da35949666 115 }
p3p 0:43da35949666 116 return;
p3p 0:43da35949666 117
p3p 0:43da35949666 118 default:
p3p 0:43da35949666 119 _state = PACKET_START;
p3p 0:43da35949666 120 _header_read = 0;
p3p 0:43da35949666 121 _data_read = 0;
p3p 0:43da35949666 122 _footer_read = 0;
p3p 0:43da35949666 123 break;
p3p 0:43da35949666 124 }
p3p 0:43da35949666 125 }
p3p 0:43da35949666 126 }
p3p 0:43da35949666 127
p3p 0:43da35949666 128 void SirfStarIII::sendBinaryPacket(SimpleSerialProtocol::Packet* packet) {
p3p 0:43da35949666 129 if (packet!=0) {
p3p 0:43da35949666 130 send(0xA0);
p3p 0:43da35949666 131 send(0xA2);
p3p 0:43da35949666 132
p3p 0:43da35949666 133 //size (reversed 2 byte value)
p3p 0:43da35949666 134 uint8_t* size_array = reinterpret_cast<uint8_t *>(&packet->_size);
p3p 0:43da35949666 135 send( size_array[1] );
p3p 0:43da35949666 136 send( size_array[0] );
p3p 0:43da35949666 137
p3p 0:43da35949666 138 //packet data
p3p 0:43da35949666 139 for (int i = 0; i < packet->_size; i++) {
p3p 0:43da35949666 140 send(packet->_data[i]);
p3p 0:43da35949666 141 }
p3p 0:43da35949666 142
p3p 0:43da35949666 143 //checksum (reversed 2 byte value)
p3p 0:43da35949666 144 uint16_t check_value = checksumBinary(packet->_data, packet->_size);
p3p 0:43da35949666 145 uint8_t* check_array = reinterpret_cast<uint8_t *>( &check_value );
p3p 0:43da35949666 146 send( check_array[1] );
p3p 0:43da35949666 147 send( check_array[0] );
p3p 0:43da35949666 148
p3p 0:43da35949666 149 //end bytes
p3p 0:43da35949666 150 send(0xB0);
p3p 0:43da35949666 151 send(0xB3);
p3p 0:43da35949666 152
p3p 0:43da35949666 153 }
p3p 0:43da35949666 154 }
p3p 0:43da35949666 155
p3p 0:43da35949666 156 uint16_t SirfStarIII::checksumBinary(uint8_t* packet, uint16_t packet_size) {
p3p 0:43da35949666 157 uint16_t cs = 0;
p3p 0:43da35949666 158 for (int i = 0; i < packet_size; i++) {
p3p 0:43da35949666 159 cs += packet[i];
p3p 0:43da35949666 160 cs &= 32767;
p3p 0:43da35949666 161 }
p3p 0:43da35949666 162 return cs;
p3p 0:43da35949666 163 }
p3p 0:43da35949666 164
p3p 0:43da35949666 165 void SirfStarIII::receiveNMEA() {
p3p 0:43da35949666 166 uint8_t new_byte = 0;
p3p 0:43da35949666 167 uint16_t cs = 0;
p3p 0:43da35949666 168 char checksum_array[2];
p3p 0:43da35949666 169
p3p 0:43da35949666 170 _receive_timeout.reset();
p3p 0:43da35949666 171 while (MODSERIAL::rxBufferGetCount() > 0 && _receive_timeout.read_us() < 50) {
p3p 0:43da35949666 172
p3p 0:43da35949666 173 switch (_state) {
p3p 0:43da35949666 174 case PACKET_START:
p3p 0:43da35949666 175 new_byte = MODSERIAL::getc();
p3p 0:43da35949666 176 if (new_byte == '$') {
p3p 0:43da35949666 177 _state = DATA_RECEIVE;
p3p 0:43da35949666 178 }
p3p 0:43da35949666 179 break;
p3p 0:43da35949666 180
p3p 0:43da35949666 181 case DATA_RECEIVE:
p3p 0:43da35949666 182 new_byte = MODSERIAL::getc();
p3p 0:43da35949666 183 if (new_byte != '\r') {
p3p 0:43da35949666 184 _packet._data[_data_read++] = new_byte;
p3p 0:43da35949666 185 } else {
p3p 0:43da35949666 186 _packet._size = _data_read - 3; //checksum is last 3 bytes
p3p 0:43da35949666 187 _state = FOOTER_RECEIVE;
p3p 0:43da35949666 188 }
p3p 0:43da35949666 189 break;
p3p 0:43da35949666 190
p3p 0:43da35949666 191 case FOOTER_RECEIVE:
p3p 0:43da35949666 192 //discard '\n"
p3p 0:43da35949666 193 new_byte = MODSERIAL::getc();
p3p 0:43da35949666 194 _state = DATA_VALIDATE;
p3p 0:43da35949666 195 break;
p3p 0:43da35949666 196
p3p 0:43da35949666 197 case DATA_VALIDATE:
p3p 0:43da35949666 198 _packet._data[_packet._size] = 0;
p3p 0:43da35949666 199 checksum_array[0] = _packet._data[_packet._size + 1];
p3p 0:43da35949666 200 checksum_array[1] = _packet._data[_packet._size + 2];
p3p 0:43da35949666 201 cs = (uint16_t)strtoul(checksum_array, 0, 16);
p3p 0:43da35949666 202
p3p 0:43da35949666 203 if (cs == checksumNMEA((const char *)_packet._data)) {
p3p 0:43da35949666 204 _state = PACKET_VALID;
p3p 0:43da35949666 205 _packet._valid = true;
p3p 0:43da35949666 206 std::string raw_data( (char *) _packet._data);
p3p 0:43da35949666 207 size_t pos = raw_data.find(",", 0);
p3p 0:43da35949666 208 std::string temp = raw_data.substr(0, pos);
p3p 0:43da35949666 209
p3p 0:43da35949666 210 if (!temp.compare("GPGGA")) {
p3p 0:43da35949666 211 _packet._type = NMEAPacket::ID_GGA;
p3p 0:43da35949666 212 } else if (!temp.compare("GPGLL")) {
p3p 0:43da35949666 213 _packet._type = NMEAPacket::ID_GLL;
p3p 0:43da35949666 214 } else if (!temp.compare("GPGSA")) {
p3p 0:43da35949666 215 _packet._type = NMEAPacket::ID_GSA;
p3p 0:43da35949666 216 } else if (!temp.compare("GPGSV")) {
p3p 0:43da35949666 217 _packet._type = NMEAPacket::ID_GSV;
p3p 0:43da35949666 218 } else if (!temp.compare("GPMSS")) {
p3p 0:43da35949666 219 _packet._type = NMEAPacket::ID_MSS;
p3p 0:43da35949666 220 } else if (!temp.compare("GPRMC")) {
p3p 0:43da35949666 221 _packet._type = NMEAPacket::ID_RMC;
p3p 0:43da35949666 222 } else if (!temp.compare("GPVTG")) {
p3p 0:43da35949666 223 _packet._type = NMEAPacket::ID_VTG;
p3p 0:43da35949666 224 } else if (!temp.compare("GPZDA")) {
p3p 0:43da35949666 225 _packet._type = NMEAPacket::ID_ZDA;
p3p 0:43da35949666 226 } else if (!temp.compare("PSRF150")) {
p3p 0:43da35949666 227 _packet._type = NMEAPacket::ID_150;
p3p 0:43da35949666 228 } else {
p3p 0:43da35949666 229 _packet._type = 255;
p3p 0:43da35949666 230 }
p3p 0:43da35949666 231 } else {
p3p 0:43da35949666 232 _state = PACKET_RESET;
p3p 0:43da35949666 233 }
p3p 0:43da35949666 234 break;
p3p 0:43da35949666 235
p3p 0:43da35949666 236 case PACKET_VALID:
p3p 0:43da35949666 237 if (!_packet._valid) {
p3p 0:43da35949666 238 _state = PACKET_RESET;
p3p 0:43da35949666 239 } else {
p3p 0:43da35949666 240 return;
p3p 0:43da35949666 241 }
p3p 0:43da35949666 242
p3p 0:43da35949666 243 default:
p3p 0:43da35949666 244 _state = PACKET_START;
p3p 0:43da35949666 245 _data_read = 0;
p3p 0:43da35949666 246 break;
p3p 0:43da35949666 247 }
p3p 0:43da35949666 248 }
p3p 0:43da35949666 249 }
p3p 0:43da35949666 250
p3p 0:43da35949666 251 void SirfStarIII::sendNMEAPacket(SimpleSerialProtocol::Packet* packet) {
p3p 0:43da35949666 252
p3p 0:43da35949666 253 }
p3p 0:43da35949666 254
p3p 0:43da35949666 255 uint8_t SirfStarIII::checksumNMEA(const char * command) {
p3p 0:43da35949666 256 uint8_t i = 1;
p3p 0:43da35949666 257 uint8_t command_checksum = command[0];
p3p 0:43da35949666 258 while (command[i] != 0) {
p3p 0:43da35949666 259 command_checksum = command_checksum ^ command[i];
p3p 0:43da35949666 260 i++;
p3p 0:43da35949666 261 }
p3p 0:43da35949666 262 return command_checksum;
p3p 0:43da35949666 263 }
p3p 0:43da35949666 264
p3p 0:43da35949666 265 void SirfStarIII::selectMode(ProtocolMode mode, uint32_t baud_rate, bool find_baud) {
p3p 0:43da35949666 266 int default_baud_rate[6];
p3p 0:43da35949666 267 default_baud_rate[0] = 4800;
p3p 0:43da35949666 268 default_baud_rate[1] = 9600;
p3p 0:43da35949666 269 default_baud_rate[2] = 14400;
p3p 0:43da35949666 270 default_baud_rate[3] = 19200;
p3p 0:43da35949666 271 default_baud_rate[4] = 38400;
p3p 0:43da35949666 272 default_baud_rate[5] = 57600;
p3p 0:43da35949666 273
p3p 0:43da35949666 274 _mode = mode;
p3p 0:43da35949666 275 /*
p3p 0:43da35949666 276 if (find_baud) {
p3p 0:43da35949666 277 char command[] = "PSRF100,0,%d,8,1,0\0";
p3p 0:43da35949666 278 char buffer[sizeof(command) + 10];
p3p 0:43da35949666 279 snprintf(buffer, sizeof(buffer), command, baud_rate );
p3p 0:43da35949666 280
p3p 0:43da35949666 281 uint8_t c_checksum = checksumNMEA(buffer);
p3p 0:43da35949666 282 //send NMEA command at all baud rates to switch to BINARY @ 57600
p3p 0:43da35949666 283 for (int i = 0; i < 6; ++i) {
p3p 0:43da35949666 284 Protocol::baud(default_baud_rate[i]);
p3p 0:43da35949666 285 wait_ms(100);
p3p 0:43da35949666 286 Protocol::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 287 blockUntilTxEmpty();
p3p 0:43da35949666 288 }
p3p 0:43da35949666 289
p3p 0:43da35949666 290 //send Binary command at all baud rates to switch to 57600
p3p 0:43da35949666 291 for (int i = 0; i < 6; ++i) {
p3p 0:43da35949666 292 Protocol::baud(default_baud_rate[i]);
p3p 0:43da35949666 293 wait_ms(100);
p3p 0:43da35949666 294 BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
p3p 0:43da35949666 295 binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
p3p 0:43da35949666 296 sendBinaryPacket(&binary_config);
p3p 0:43da35949666 297 blockUntilTxEmpty();
p3p 0:43da35949666 298
p3p 0:43da35949666 299 BinaryPacket::ConfigureNMEA nmea_config(baud_rate);
p3p 0:43da35949666 300 nmea_config.buildData<BinaryPacket::ConfigureNMEA::Interface>(&nmea_config.interface);
p3p 0:43da35949666 301 sendBinaryPacket(&nmea_config);
p3p 0:43da35949666 302 blockUntilTxEmpty();
p3p 0:43da35949666 303 }
p3p 0:43da35949666 304
p3p 0:43da35949666 305 //should be binary @ baud now
p3p 0:43da35949666 306 }
p3p 0:43da35949666 307 wait_ms(50);
p3p 0:43da35949666 308 */
p3p 0:43da35949666 309 if (mode == BINARY) {
p3p 0:43da35949666 310 baud(4800);
p3p 0:43da35949666 311 wait_ms(100);
p3p 0:43da35949666 312 //Build the NMEA packet
p3p 0:43da35949666 313 char command[] = "PSRF100,0,%d,8,1,0\0";
p3p 0:43da35949666 314 char buffer[sizeof(command) + 10];
p3p 0:43da35949666 315 snprintf(buffer, sizeof(buffer), command, baud_rate );
p3p 0:43da35949666 316 uint8_t c_checksum = checksumNMEA(buffer);
p3p 0:43da35949666 317 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 318 wait_ms(10);
p3p 0:43da35949666 319 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 320 wait_ms(10);
p3p 0:43da35949666 321 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 322 wait_ms(10);
p3p 0:43da35949666 323 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 324 wait_ms(10);
p3p 0:43da35949666 325 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 326 wait_ms(10);
p3p 0:43da35949666 327 Serial::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 328 blockUntilTxEmpty();
p3p 0:43da35949666 329 }
p3p 0:43da35949666 330 /*
p3p 0:43da35949666 331 if (mode == NMEA) {
p3p 0:43da35949666 332 Protocol::baud(4800);
p3p 0:43da35949666 333
p3p 0:43da35949666 334 NMEAChangeBaud(baud_rate);
p3p 0:43da35949666 335 wait_ms(10);
p3p 0:43da35949666 336
p3p 0:43da35949666 337 BinaryPacket::ConfigureNMEA nmea_config(baud_rate);
p3p 0:43da35949666 338 nmea_config.buildData<BinaryPacket::ConfigureNMEA::Interface>(&nmea_config.interface);
p3p 0:43da35949666 339 sendBinaryPacket(&nmea_config);
p3p 0:43da35949666 340
p3p 0:43da35949666 341 BinaryPacket::SetProtocol protocol_select(2);
p3p 0:43da35949666 342 protocol_select.buildData<BinaryPacket::SetProtocol::Interface>(&protocol_select.interface);
p3p 0:43da35949666 343 sendBinaryPacket(&protocol_select);
p3p 0:43da35949666 344
p3p 0:43da35949666 345 BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
p3p 0:43da35949666 346 binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
p3p 0:43da35949666 347 sendBinaryPacket(&binary_config);
p3p 0:43da35949666 348
p3p 0:43da35949666 349 blockUntilTxEmpty();*/
p3p 0:43da35949666 350 baud(baud_rate);
p3p 0:43da35949666 351 // }
p3p 0:43da35949666 352 }
p3p 0:43da35949666 353
p3p 0:43da35949666 354 void SirfStarIII::NMEAChangeBaud(uint32_t baud_rate){
p3p 0:43da35949666 355 //Build the NMEA packet
p3p 0:43da35949666 356 char command[] = "PSRF100,1,%d,8,1,0";
p3p 0:43da35949666 357 char buffer[sizeof(command) + 10];
p3p 0:43da35949666 358 snprintf(buffer, sizeof(buffer), command, baud_rate );
p3p 0:43da35949666 359 uint8_t c_checksum = checksumNMEA(buffer);
p3p 0:43da35949666 360
p3p 0:43da35949666 361 Protocol::printf("$%s*%02X\r\n", buffer, c_checksum);
p3p 0:43da35949666 362 baud(9600);
p3p 0:43da35949666 363 }
p3p 0:43da35949666 364
p3p 0:43da35949666 365 void SirfStarIII::BinaryChangeBaud(uint32_t baud_rate){
p3p 0:43da35949666 366 BinaryPacket::SetBinarySerialPort binary_config(baud_rate);
p3p 0:43da35949666 367 binary_config.buildData<BinaryPacket::SetBinarySerialPort::Interface>(&binary_config.interface);
p3p 0:43da35949666 368 sendBinaryPacket(&binary_config);
p3p 0:43da35949666 369 blockUntilTxEmpty();
p3p 0:43da35949666 370 baud(baud_rate);
p3p 0:43da35949666 371 }
p3p 0:43da35949666 372
p3p 0:43da35949666 373 }