AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port), _step(0), _msg_id(0), _payload_length(0), _payload_counter(0), _class(0), _cfg_saved(false), _last_cfg_sent_time(0), _num_cfg_save_tries(0), _last_config_time(0), _delay_time(0), _next_message(STEP_RATE_NAV), _ublox_port(255), _have_version(false), _unconfigured_messages(CONFIG_ALL), _hardware_generation(0), _new_position(0), _new_speed(0), _disable_counter(0), next_fix(AP_GPS::NO_FIX), _cfg_needs_save(false), noReceivedHdop(true) { // stop any config strings that are pending gps.send_blob_start(state.instance, NULL, 0); // start the process of updating the GPS rates _request_next_config(); }
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port), _next_message(STEP_PVT), _ublox_port(255), _unconfigured_messages(CONFIG_ALL), _hardware_generation(UBLOX_UNKNOWN_HARDWARE_GENERATION), next_fix(AP_GPS::NO_FIX), noReceivedHdop(true) { // stop any config strings that are pending gps.send_blob_start(state.instance, nullptr, 0); // start the process of updating the GPS rates _request_next_config(); #if CONFIGURE_PPS_PIN _unconfigured_messages |= CONFIG_TP5; #endif }
// 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; }