// Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // bool AP_GPS_UBLOX::read(void) { uint8_t data; int16_t numc; bool parsed = false; if (need_rate_update) { send_next_rate_update(); } numc = port->available(); for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = port->read(); reset: switch(_step) { // Message preamble detection // // If we fail to match any of the expected bytes, we reset // the state machine and re-consider the failed byte as // the first byte of the preamble. This improves our // chances of recovering from a mismatch and makes it less // likely that we will be fooled by the preamble appearing // as data in some other message. // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; Debug("reset %u", __LINE__); // FALLTHROUGH case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // // We sniff the class and message ID to decide whether we // are going to gather the message bytes or just discard // them. // // We always collect the length so that we can avoid being // fooled by preamble bytes in messages. // case 2: _step++; _class = data; _ck_b = _ck_a = data; // reset the checksum accumulators break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)(data<<8); if (_payload_length > 512) { Debug("large payload %u", (unsigned)_payload_length); // assume very large payloads are line noise _payload_length = 0; _step = 0; goto reset; } _payload_counter = 0; // prepare to receive payload break; // Receive message data // case 6: _ck_b += (_ck_a += data); // checksum byte if (_payload_counter < sizeof(_buffer)) { _buffer.bytes[_payload_counter] = data; } if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 7: _step++; if (_ck_a != data) { Debug("bad cka %x should be %x", data, _ck_a); _step = 0; goto reset; } break; case 8: _step = 0; if (_ck_b != data) { Debug("bad ckb %x should be %x", data, _ck_b); break; // bad checksum } if (_parse_gps()) { parsed = true; } } } return parsed; }
// Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // bool AP_GPS_ERB::read(void) { uint8_t data; int16_t numc; bool parsed = false; numc = port->available(); for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = port->read(); reset: switch(_step) { // Message preamble detection // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; Debug("reset %u", __LINE__); FALLTHROUGH; case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // case 2: _step++; _msg_id = data; _ck_b = _ck_a = data; // reset the checksum accumulators break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)(data<<8); _payload_counter = 0; // prepare to receive payload break; // Receive message data // case 5: _ck_b += (_ck_a += data); // checksum byte if (_payload_counter < sizeof(_buffer)) { _buffer[_payload_counter] = data; } if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 6: _step++; if (_ck_a != data) { Debug("bad cka %x should be %x", data, _ck_a); _step = 0; goto reset; } break; case 7: _step = 0; if (_ck_b != data) { Debug("bad ckb %x should be %x", data, _ck_b); break; // bad checksum } if (_parse_gps()) { parsed = true; } break; } } return parsed; }
// Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // bool AP_GPS_UBLOX::read(void) { uint8_t data; int numc; bool parsed = false; numc = _port->available(); for (int i = 0; i < numc; i++){ // Process bytes received // read the next byte data = _port->read(); switch(_step){ // Message preamble detection // // If we fail to match any of the expected bytes, we reset // the state machine and re-consider the failed byte as // the first byte of the preamble. This improves our // chances of recovering from a mismatch and makes it less // likely that we will be fooled by the preamble appearing // as data in some other message. // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; // FALLTHROUGH case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // // We sniff the class and message ID to decide whether we // are going to gather the message bytes or just discard // them. // // We always collect the length so that we can avoid being // fooled by preamble bytes in messages. // case 2: _step++; if (CLASS_NAV == data) { _gather = true; // class is interesting, maybe gather _ck_b = _ck_a = data; // reset the checksum accumulators } else { _gather = false; // class is not interesting, discard } break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; if (_gather) { // if class was interesting switch(data) { case MSG_POSLLH: // message is interesting _expect = sizeof(ubx_nav_posllh); break; case MSG_STATUS: _expect = sizeof(ubx_nav_status); break; case MSG_SOL: _expect = sizeof(ubx_nav_solution); break; case MSG_VELNED: _expect = sizeof(ubx_nav_velned); break; default: _gather = false; // message is not interesting } } break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)data; // payload length high byte _payload_counter = 0; // prepare to receive payload if (_payload_length != _expect) _gather = false; break; // Receive message data // case 6: _ck_b += (_ck_a += data); // checksum byte if (_gather) // gather data if requested _buffer.bytes[_payload_counter] = data; if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 7: _step++; if (_ck_a != data) _step = 0; // bad checksum break; case 8: _step = 0; if (_ck_b != data) break; // bad checksum if (_gather) { parsed = _parse_gps(); // Parse the new GPS packet } } } return parsed; }
// Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // void AP_GPS_I2C::update(void) { if(_dataReceived) _parse_gps(); // Parse the new GPS packet _dataReceived = 0; }
// Ensure there is enough space for the largest possible outgoing message // Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // bool AP_GPS_UBLOX::read(void) { uint8_t data; int16_t numc; bool parsed = false; uint32_t millis_now = AP_HAL::millis(); // walk through the gps configuration at 1 message per second if (millis_now - _last_config_time >= _delay_time) { _request_next_config(); _last_config_time = millis_now; if (_unconfigured_messages) { // send the updates faster until fully configured if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) { _delay_time = 300; } else { _delay_time = 750; } } else { _delay_time = 2000; } } if(!_unconfigured_messages && gps._save_config && !_cfg_saved && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 && !hal.util->get_soft_armed()) { //save the configuration sent until now if (gps._save_config == 1 || (gps._save_config == 2 && _cfg_needs_save)) { _save_cfg(); } } numc = port->available(); for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = port->read(); reset: switch(_step) { // Message preamble detection // // If we fail to match any of the expected bytes, we reset // the state machine and re-consider the failed byte as // the first byte of the preamble. This improves our // chances of recovering from a mismatch and makes it less // likely that we will be fooled by the preamble appearing // as data in some other message. // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; Debug("reset %u", __LINE__); FALLTHROUGH; case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // // We sniff the class and message ID to decide whether we // are going to gather the message bytes or just discard // them. // // We always collect the length so that we can avoid being // fooled by preamble bytes in messages. // case 2: _step++; _class = data; _ck_b = _ck_a = data; // reset the checksum accumulators break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)(data<<8); if (_payload_length > sizeof(_buffer)) { Debug("large payload %u", (unsigned)_payload_length); // assume any payload bigger then what we know about is noise _payload_length = 0; _step = 0; goto reset; } _payload_counter = 0; // prepare to receive payload break; // Receive message data // case 6: _ck_b += (_ck_a += data); // checksum byte if (_payload_counter < sizeof(_buffer)) { _buffer[_payload_counter] = data; } if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 7: _step++; if (_ck_a != data) { Debug("bad cka %x should be %x", data, _ck_a); _step = 0; goto reset; } break; case 8: _step = 0; if (_ck_b != data) { Debug("bad ckb %x should be %x", data, _ck_b); break; // bad checksum } if (_parse_gps()) { parsed = true; } break; } } return parsed; }
// Process bytes available from the stream // // The stream is assumed to contain only our custom message. If it // contains other messages, and those messages contain the preamble bytes, // it is possible for this code to become de-synchronised. Without // buffering the entire message and re-processing it from the top, // this is unavoidable. // // The lack of a standard header length field makes it impossible to skip // unrecognised messages. // void AP_GPS_MTK::update(void) { byte data; int numc; numc = _port->available(); for (int i = 0; i < numc; i++){ // Process bytes received // read the next byte data = _port->read(); restart: switch(_step){ // Message preamble, class, ID detection // // If we fail to match any of the expected bytes, we // reset the state machine and re-consider the failed // byte as the first byte of the preamble. This // improves our chances of recovering from a mismatch // and makes it less likely that we will be fooled by // the preamble appearing as data in some other message. // case 0: if(PREAMBLE1 == data) _step++; break; case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; goto restart; case 2: if (MESSAGE_CLASS == data) { _step++; _ck_b = _ck_a = data; // reset the checksum accumulators } else { _step = 0; // reset and wait for a message of the right class goto restart; } break; case 3: if (MESSAGE_ID == data) { _step++; _ck_b += (_ck_a += data); _payload_length = sizeof(diyd_mtk_msg); // prepare to receive our message _payload_counter = 0; } else { _step = 0; goto restart; } break; // Receive message data // case 4: _buffer.bytes[_payload_counter++] = data; _ck_b += (_ck_a += data); if (_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 5: _step++; if (_ck_a != data) { _error("GPS_MTK: checksum error\n"); _step = 0; } break; case 6: _step = 0; if (_ck_b != data) { _error("GPS_MTK: checksum error\n"); break; } _parse_gps(); // Parse the new GPS packet } } }