// 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
}
}
}
