Ejemplo n.º 1
0
void AP_GPS_UBLOX::unexpected_message(void)
{
    Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
    if (++_disable_counter == 0) {
        // disable future sends of this message id, but
        // only do this every 256 messages, as some
        // message types can't be disabled and we don't
        // want to get into an ack war
        Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
        _configure_message_rate(_class, _msg_id, 0);
    }
}
Ejemplo n.º 2
0
/*
  send the next step of rate updates to the GPS. This reconfigures the
  GPS on the fly to have the right message rates. It needs to be
  careful to only send a message if there is sufficient buffer space
  available on the serial port to avoid it blocking the CPU
 */
void
AP_GPS_UBLOX::send_next_rate_update(void)
{
    if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
        // not enough space - do it next time
        return;
    }

    //hal.console->printf_P(PSTR("next_rate: %u\n"), (unsigned)rate_update_step);

    switch (rate_update_step++) {
    case 0:
        _configure_navigation_rate(200);
        break;
    case 1:
        _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); // 28+8 bytes
        break;
    case 2:
        _configure_message_rate(CLASS_NAV, MSG_STATUS, 1); // 16+8 bytes
        break;
    case 3:
        _configure_message_rate(CLASS_NAV, MSG_DOP, 1);
        break;
    case 4:
        _configure_message_rate(CLASS_NAV, MSG_SOL, 1);    // 52+8 bytes
        break;
    case 5:
        _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // 36+8 bytes
        break;
#if UBLOX_HW_LOGGING
    case 6:
        // gather MON_HW at 0.5Hz
        _configure_message_rate(CLASS_MON, MSG_MON_HW, 2); // 64+8 bytes
        break;
    case 7:
        // gather MON_HW2 at 0.5Hz
        _configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
        break;
#endif
#if UBLOX_VERSION_AUTODETECTION 
    case 8:
        _request_version();
        break;
#endif
    default:
        need_rate_update = false;
        rate_update_step = 0;
        break;
    }
}
Ejemplo n.º 3
0
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
		Debug("Got settings %u min_elev %d drLimit %u\n", 
              (unsigned)_buffer.nav_settings.dynModel,
              (int)_buffer.nav_settings.minElev,
              (unsigned)_buffer.nav_settings.drLimit);
        _buffer.nav_settings.mask = 0;
        if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
            _buffer.nav_settings.dynModel != gps._navfilter) {
            // we've received the current nav settings, change the engine
            // settings and send them back
            Debug("Changing engine setting from %u to %u\n",
                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
            _buffer.nav_settings.dynModel = gps._navfilter;
            _buffer.nav_settings.mask |= 1;
        }
        if (gps._min_elevation != -100 &&
            _buffer.nav_settings.minElev != gps._min_elevation) {
            Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
            _buffer.nav_settings.minElev = gps._min_elevation;
            _buffer.nav_settings.mask |= 2;
        }
        if (_buffer.nav_settings.mask != 0) {
            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                          &_buffer.nav_settings,
                          sizeof(_buffer.nav_settings));
        }
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
		Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
              (unsigned)_buffer.sbas.mode,
              (unsigned)_buffer.sbas.usage,
              (unsigned)_buffer.sbas.maxSBAS,
              (unsigned)_buffer.sbas.scanmode2,
              (unsigned)_buffer.sbas.scanmode1);
        if (_buffer.sbas.mode != gps._sbas_mode) {
            _buffer.sbas.mode = gps._sbas_mode;
            _send_message(CLASS_CFG, MSG_CFG_SBAS,
                          &_buffer.sbas,
                          sizeof(_buffer.sbas));
        }
    }

#if UBLOX_HW_LOGGING
    if (_class == CLASS_MON) {
        if (_msg_id == MSG_MON_HW) {
            if (_payload_length == 60 || _payload_length == 68) {
                log_mon_hw();
            }
        } else if (_msg_id == MSG_MON_HW2) {
            if (_payload_length == 28) {
                log_mon_hw2();  
            }
        } else {
            unexpected_message();
        }
        return false;
    }
#endif // UBLOX_HW_LOGGING

    if (_class != CLASS_NAV) {
        unexpected_message();
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        _last_pos_time        = _buffer.posllh.time;
        state.location.lng    = _buffer.posllh.longitude;
        state.location.lat    = _buffer.posllh.latitude;
        state.location.alt    = _buffer.posllh.altitude_msl / 10;
        state.status          = next_fix;
        _new_position = true;
#if UBLOX_FAKE_3DLOCK
        state.location.lng = 1491652300L;
        state.location.lat = -353632610L;
        state.location.alt = 58400;
#endif
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
#if UBLOX_FAKE_3DLOCK
        state.status = AP_GPS::GPS_OK_FIX_3D;
        next_fix = state.status;
#endif
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
        state.num_sats    = _buffer.solution.satellites;
        state.hdop        = _buffer.solution.position_DOP;
        if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
            state.last_gps_time_ms = hal.scheduler->millis();
            if (state.time_week == _buffer.solution.week &&
                state.time_week_ms + 200 == _buffer.solution.time) {
                // we got a 5Hz update. This relies on the way
                // that uBlox gives timestamps that are always
                // multiples of 200 for 5Hz
                _last_5hz_time = state.last_gps_time_ms;
            }
            state.time_week_ms    = _buffer.solution.time;
            state.time_week       = _buffer.solution.week;
        }
#if UBLOX_FAKE_3DLOCK
        next_fix = state.status;
        state.num_sats = 10;
        state.hdop = 200;
        state.time_week = 1721;
        state.time_week_ms = hal.scheduler->millis() + 3*60*60*1000 + 37000;
        state.last_gps_time_ms = hal.scheduler->millis();
#endif
        break;
    case MSG_VELNED:
        Debug("MSG_VELNED");
        _last_vel_time         = _buffer.velned.time;
        state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
        state.ground_course_cd = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
        state.have_vertical_velocity = true;
        state.velocity.x = _buffer.velned.ned_north * 0.01f;
        state.velocity.y = _buffer.velned.ned_east * 0.01f;
        state.velocity.z = _buffer.velned.ned_down * 0.01f;
        _new_speed = true;
        break;
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
        _new_speed = _new_position = false;
		_fix_count++;
        if ((hal.scheduler->millis() - _last_5hz_time) > 15000U && !need_rate_update) {
            // the GPS is running slow. It possibly browned out and
            // restarted with incorrect parameters. We will slowly
            // send out new parameters to fix it
            need_rate_update = true;
            rate_update_step = 0;
            _last_5hz_time = hal.scheduler->millis();
        }

		if (_fix_count == 50 && gps._sbas_mode != 2) {
			// ask for SBAS settings every 20 seconds
			Debug("Asking for SBAS setting\n");
			_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
		}
		if (_fix_count == 100) {
			// ask for nav settings every 20 seconds
			Debug("Asking for engine setting\n");
			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
            _fix_count = 0;
		}
        return true;
    }
    return false;
}
Ejemplo n.º 4
0
void
AP_GPS_UBLOX::_request_version(void)
{
    _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2);
}
Ejemplo n.º 5
0
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
		Debug("Got settings %u min_elev %d drLimit %u\n", 
              (unsigned)_buffer.nav_settings.dynModel,
              (int)_buffer.nav_settings.minElev,
              (unsigned)_buffer.nav_settings.drLimit);
        _buffer.nav_settings.mask = 0;
        if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
            _buffer.nav_settings.dynModel != gps._navfilter) {
            // we've received the current nav settings, change the engine
            // settings and send them back
            Debug("Changing engine setting from %u to %u\n",
                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
            _buffer.nav_settings.dynModel = gps._navfilter;
            _buffer.nav_settings.mask |= 1;
        }
        if (gps._min_elevation != -100 &&
            _buffer.nav_settings.minElev != gps._min_elevation) {
            Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
            _buffer.nav_settings.minElev = gps._min_elevation;
            _buffer.nav_settings.mask |= 2;
        }
        if (_buffer.nav_settings.mask != 0) {
            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                          &_buffer.nav_settings,
                          sizeof(_buffer.nav_settings));
        }
        return false;
    }

#if UBLOX_GNSS_SETTINGS
    if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
        uint8_t gnssCount = 0;
        Debug("Got GNSS Settings %u %u %u %u:\n",
            (unsigned)_buffer.gnss.msgVer,
            (unsigned)_buffer.gnss.numTrkChHw,
            (unsigned)_buffer.gnss.numTrkChUse,
            (unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUG
        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
            Debug("  %u %u %u 0x%08x\n",
            (unsigned)_buffer.gnss.configBlock[i].gnssId,
            (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
            (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
            (unsigned)_buffer.gnss.configBlock[i].flags);
        }
#endif

        for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
            if((gps._gnss_mode & (1 << i)) && i != GNSS_SBAS) {
                gnssCount++;
            }
        }

        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
            // Reserve an equal portion of channels for all enabled systems
            if(gps._gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
                if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
                    _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                    _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
                } else {
                    _buffer.gnss.configBlock[i].resTrkCh = 1;
                    _buffer.gnss.configBlock[i].maxTrkCh = 3;
                }
                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
            } else {
                _buffer.gnss.configBlock[i].resTrkCh = 0;
                _buffer.gnss.configBlock[i].maxTrkCh = 0;
                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
            }
        }
        _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
        return false;
    }
#endif

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
		Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
              (unsigned)_buffer.sbas.mode,
              (unsigned)_buffer.sbas.usage,
              (unsigned)_buffer.sbas.maxSBAS,
              (unsigned)_buffer.sbas.scanmode2,
              (unsigned)_buffer.sbas.scanmode1);
        if (_buffer.sbas.mode != gps._sbas_mode) {
            _buffer.sbas.mode = gps._sbas_mode;
            _send_message(CLASS_CFG, MSG_CFG_SBAS,
                          &_buffer.sbas,
                          sizeof(_buffer.sbas));
        }
    }

#if UBLOX_HW_LOGGING
    if (_class == CLASS_MON) {
        if (_msg_id == MSG_MON_HW) {
            if (_payload_length == 60 || _payload_length == 68) {
                log_mon_hw();
            }
        } else if (_msg_id == MSG_MON_HW2) {
            if (_payload_length == 28) {
                log_mon_hw2();  
            }
        } else {
            unexpected_message();
        }
        return false;
    }
#endif // UBLOX_HW_LOGGING

#if UBLOX_RXM_RAW_LOGGING
    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
        log_rxm_raw(_buffer.rxm_raw);
        return false;
    } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
        log_rxm_rawx(_buffer.rxm_rawx);
        return false;
    }
#endif // UBLOX_RXM_RAW_LOGGING

    if (_class != CLASS_NAV) {
        unexpected_message();
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        _last_pos_time        = _buffer.posllh.time;
        state.location.lng    = _buffer.posllh.longitude;
        state.location.lat    = _buffer.posllh.latitude;
        state.location.alt    = _buffer.posllh.altitude_msl / 10;
        state.status          = next_fix;
        _new_position = true;
#if UBLOX_FAKE_3DLOCK
        state.location.lng = 1491652300L;
        state.location.lat = -353632610L;
        state.location.alt = 58400;
#endif
        state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
        state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
        state.have_horizontal_accuracy = true;
        state.have_vertical_accuracy = true;
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
#if UBLOX_FAKE_3DLOCK
        state.status = AP_GPS::GPS_OK_FIX_3D;
        next_fix = state.status;
#endif
        break;
    case MSG_DOP:
        Debug("MSG_DOP");
        noReceivedHdop = false;
        state.hdop        = _buffer.dop.hDOP;
#if UBLOX_FAKE_3DLOCK
        state.hdop = 130;
#endif
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
        if(noReceivedHdop) {
            state.hdop = _buffer.solution.position_DOP;
        }
        state.num_sats    = _buffer.solution.satellites;
        if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
            state.last_gps_time_ms = hal.scheduler->millis();
            if (state.time_week == _buffer.solution.week &&
                state.time_week_ms + 200 == _buffer.solution.time) {
                // we got a 5Hz update. This relies on the way
                // that uBlox gives timestamps that are always
                // multiples of 200 for 5Hz
                _last_5hz_time = state.last_gps_time_ms;
            }
            state.time_week_ms    = _buffer.solution.time;
            state.time_week       = _buffer.solution.week;
        }
#if UBLOX_FAKE_3DLOCK
        next_fix = state.status;
        state.num_sats = 10;
        state.time_week = 1721;
        state.time_week_ms = hal.scheduler->millis() + 3*60*60*1000 + 37000;
        state.last_gps_time_ms = hal.scheduler->millis();
#endif
        break;
    case MSG_VELNED:
        Debug("MSG_VELNED");
        _last_vel_time         = _buffer.velned.time;
        state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
        state.ground_course_cd = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
        state.have_vertical_velocity = true;
        state.velocity.x = _buffer.velned.ned_north * 0.01f;
        state.velocity.y = _buffer.velned.ned_east * 0.01f;
        state.velocity.z = _buffer.velned.ned_down * 0.01f;
        state.have_speed_accuracy = true;
        state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
        _new_speed = true;
        break;
#if UBLOX_VERSION_AUTODETECTION
    case MSG_NAV_SVINFO:
        {
        Debug("MSG_NAV_SVINFO\n");
        static const uint8_t HardwareGenerationMask = 0x07;
        uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
        switch (hardware_generation) {
            case UBLOX_5:
            case UBLOX_6:
                /*speed already configured */;
                break;
            case UBLOX_7:
            case UBLOX_M8:
                port->begin(4000000U);
                Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n");
                break;
            default:
                hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation);
                break;
        };
        /* We don't need that anymore */
        _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
        break;
        }
#endif
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
        _new_speed = _new_position = false;
		_fix_count++;
        if ((hal.scheduler->millis() - _last_5hz_time) > 15000U && !need_rate_update) {
            // the GPS is running slow. It possibly browned out and
            // restarted with incorrect parameters. We will slowly
            // send out new parameters to fix it
            need_rate_update = true;
            rate_update_step = 0;
            _last_5hz_time = hal.scheduler->millis();
        }

		if (_fix_count == 50 && gps._sbas_mode != 2) {
			// ask for SBAS settings every 20 seconds
			Debug("Asking for SBAS setting\n");
			_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
		}
		if (_fix_count == 100) {
			// ask for nav settings every 20 seconds
			Debug("Asking for engine setting\n");
			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
            _fix_count = 0;
		}

#if UBLOX_HW_LOGGING
        log_accuracy();
#endif //UBLOX_HW_LOGGING

        return true;
    }
    return false;
}
Ejemplo n.º 6
0
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
		Debug("Got engine settings %u\n", (unsigned)_buffer.nav_settings.dynModel);
        if (_nav_setting != GPS_ENGINE_NONE &&
            _buffer.nav_settings.dynModel != _nav_setting) {
            // we've received the current nav settings, change the engine
            // settings and send them back
            Debug("Changing engine setting from %u to %u\n",
                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
            _buffer.nav_settings.dynModel = _nav_setting;
            _buffer.nav_settings.mask = 1; // only change dynamic model
            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                          &_buffer.nav_settings,
                          sizeof(_buffer.nav_settings));
        }
        return false;
    }

    if (_class != CLASS_NAV) {
        Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            // disable future sends of this message id, but
            // only do this every 256 messages, as some
            // message types can't be disabled and we don't
            // want to get into an ack war
            Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
            _configure_message_rate(_class, _msg_id, 0);
        }
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        time            = _buffer.posllh.time;
        longitude       = _buffer.posllh.longitude;
        latitude        = _buffer.posllh.latitude;
        altitude_cm     = _buffer.posllh.altitude_msl / 10;
        fix             = next_fix;
        _new_position = true;
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = GPS::FIX_3D;
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = GPS::FIX_2D;
            }else{
                next_fix = GPS::FIX_NONE;
                fix = GPS::FIX_NONE;
            }
        }else{
            next_fix = GPS::FIX_NONE;
            fix = GPS::FIX_NONE;
        }
#if UBLOX_FAKE_3DLOCK
        fix = GPS::FIX_3D;
        next_fix = fix;
#endif
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = GPS::FIX_3D;
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = GPS::FIX_2D;
            }else{
                next_fix = GPS::FIX_NONE;
                fix = GPS::FIX_NONE;
            }
        }else{
            next_fix = GPS::FIX_NONE;
            fix = GPS::FIX_NONE;
        }
        num_sats        = _buffer.solution.satellites;
        hdop            = _buffer.solution.position_DOP;
#if UBLOX_FAKE_3DLOCK
        next_fix = fix;
        num_sats = 10;
#endif
        break;
    case MSG_VELNED:
        Debug("MSG_VELNED");
        speed_3d_cm     = _buffer.velned.speed_3d;                              // cm/s
        ground_speed_cm = _buffer.velned.speed_2d;                         // cm/s
        ground_course_cd = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
        _have_raw_velocity = true;
        _vel_north  = _buffer.velned.ned_north;
        _vel_east   = _buffer.velned.ned_east;
        _vel_down   = _buffer.velned.ned_down;
        _new_speed = true;
        break;
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed) {
        _new_speed = _new_position = false;
		_fix_count++;
        uint32_t new_fix_time = hal.scheduler->millis();

        if (!need_rate_update && new_fix_time - _last_fix_time > 300 && _fix_count % 20 == 0) {
            // the GPS is running slow. It possibly browned out and
            // restarted with incorrect parameters. We will slowly
            // send out new parameters to fix it
            need_rate_update = true;
            rate_update_step = 0;
        }
        _last_fix_time = new_fix_time;

		if (_fix_count == 100) {
			// ask for nav settings every 20 seconds
			Debug("Asking for engine setting\n");
			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
            _fix_count = 0;
		}
        return true;
    }
    return false;
}
Ejemplo n.º 7
0
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);
        return false;
    }

    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
		Debug("Got engine settings %u\n", (unsigned)_buffer.nav_settings.dynModel);
        if (_nav_setting != GPS_ENGINE_NONE &&
            _buffer.nav_settings.dynModel != _nav_setting) {
            // we've received the current nav settings, change the engine
            // settings and send them back
            Debug("Changing engine setting from %u to %u\n",
                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
            _buffer.nav_settings.dynModel = _nav_setting;
            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                          &_buffer.nav_settings,
                          sizeof(_buffer.nav_settings));
        }
        return false;
    }

    if (_class != CLASS_NAV) {
        Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            // disable future sends of this message id, but
            // only do this every 256 messages, as some
            // message types can't be disabled and we don't
            // want to get into an ack war
            Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
            _configure_message_rate(_class, _msg_id, 0);
        }
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        time            = _buffer.posllh.time;
        longitude       = _buffer.posllh.longitude;
        latitude        = _buffer.posllh.latitude;
        altitude        = _buffer.posllh.altitude_msl / 10;
        fix                     = next_fix;
        _new_position = true;
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        next_fix        = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
        if (!next_fix) {
            fix = false;
        }
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        next_fix        = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
        if (!next_fix) {
            fix = false;
        }
        num_sats        = _buffer.solution.satellites;
        hdop            = _buffer.solution.position_DOP;
        break;
    case MSG_VELNED:
        Debug("MSG_VELNED");
        speed_3d        = _buffer.velned.speed_3d;                              // cm/s
        ground_speed = _buffer.velned.speed_2d;                         // cm/s
        ground_course = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
        _have_raw_velocity = true;
        _vel_north  = _buffer.velned.ned_north;
        _vel_east   = _buffer.velned.ned_east;
        _vel_down   = _buffer.velned.ned_down;
        _new_speed = true;
        break;
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed) {
        _new_speed = _new_position = false;
		_fix_count++;
		if (_fix_count == 100) {
			// ask for nav settings every 20 seconds
			Debug("Asking for engine setting\n");
			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
		}
        return true;
    }
    return false;
}
Ejemplo n.º 8
0
void
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
    uint8_t desired_rate;

    switch(msg_class) {
    case CLASS_NAV:
        switch(msg_id) {
        case MSG_POSLLH:
            desired_rate = havePvtMsg ? 0 : RATE_POSLLH;
            if(rate == desired_rate) {
                _unconfigured_messages &= ~CONFIG_RATE_POSLLH;
            } else {
                _configure_message_rate(msg_class, msg_id, desired_rate);
                _unconfigured_messages |= CONFIG_RATE_POSLLH;
                _cfg_needs_save = true;
            }
            break;
        case MSG_STATUS:
            desired_rate = havePvtMsg ? 0 : RATE_STATUS;
            if(rate == desired_rate) {
                _unconfigured_messages &= ~CONFIG_RATE_STATUS;
            } else {
                _configure_message_rate(msg_class, msg_id, desired_rate);
                _unconfigured_messages |= CONFIG_RATE_STATUS;
                _cfg_needs_save = true;
            }
            break;
        case MSG_SOL:
            if(rate == RATE_SOL) {
                _unconfigured_messages &= ~CONFIG_RATE_SOL;
            } else {
                _configure_message_rate(msg_class, msg_id, RATE_SOL);
                _unconfigured_messages |= CONFIG_RATE_SOL;
                _cfg_needs_save = true;
            }
            break;
        case MSG_PVT:
            if(rate == RATE_PVT) {
                _unconfigured_messages &= ~CONFIG_RATE_PVT;
            } else {
                _configure_message_rate(msg_class, msg_id, RATE_PVT);
                _unconfigured_messages |= CONFIG_RATE_PVT;
                _cfg_needs_save = true;
            }
            break;
        case MSG_VELNED:
            desired_rate = havePvtMsg ? 0 : RATE_VELNED;
            if(rate == desired_rate) {
                _unconfigured_messages &= ~CONFIG_RATE_VELNED;
            } else {
                _configure_message_rate(msg_class, msg_id, desired_rate);
                _unconfigured_messages |= CONFIG_RATE_VELNED;
                _cfg_needs_save = true;
            }
            break;
        case MSG_DOP:
            if(rate == RATE_DOP) {
                _unconfigured_messages &= ~CONFIG_RATE_DOP;
            } else {
                _configure_message_rate(msg_class, msg_id, RATE_DOP);
                _unconfigured_messages |= CONFIG_RATE_DOP;
                _cfg_needs_save = true;
            }
            break;
        }
        break;
    case CLASS_MON:
        switch(msg_id) {
        case MSG_MON_HW:
            if(rate == RATE_HW) {
                _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
            } else {
                _configure_message_rate(msg_class, msg_id, RATE_HW);
                _unconfigured_messages |= CONFIG_RATE_MON_HW;
                _cfg_needs_save = true;
            }
            break;
        case MSG_MON_HW2:
            if(rate == RATE_HW2) {
                _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
            } else {
                _configure_message_rate(msg_class, msg_id, RATE_HW2);
                _unconfigured_messages |= CONFIG_RATE_MON_HW2;
                _cfg_needs_save = true;
            }
            break;
        }
        break;
#if UBLOX_RXM_RAW_LOGGING
    case CLASS_RXM:
        switch(msg_id) {
        case MSG_RXM_RAW:
            if(rate == gps._raw_data) {
                _unconfigured_messages &= ~CONFIG_RATE_RAW;
            } else {
                _configure_message_rate(msg_class, msg_id, gps._raw_data);
                _unconfigured_messages |= CONFIG_RATE_RAW;
                _cfg_needs_save = true;
            }
            break;
        case MSG_RXM_RAWX:
            if(rate == gps._raw_data) {
                _unconfigured_messages &= ~CONFIG_RATE_RAW;
            } else {
                _configure_message_rate(msg_class, msg_id, gps._raw_data);
                _unconfigured_messages |= CONFIG_RATE_RAW;
                _cfg_needs_save = true;
            }
            break;
        }
        break;
#endif // UBLOX_RXM_RAW_LOGGING
    }
}
Ejemplo n.º 9
0
void
AP_GPS_UBLOX::_request_next_config(void)
{
    // don't request config if we shouldn't configure the GPS
    if (gps._auto_config == 0) {
        return;
    }

    // Ensure there is enough space for the largest possible outgoing message
    if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
        // not enough space - do it next time
        return;
    }

   Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);

    switch (_next_message++) {
    case STEP_RATE_NAV:
        _configure_rate();
        break;
    case STEP_RATE_POSLLH:
        if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
            _next_message--;
        }
        break;
    case STEP_RATE_VELNED:
        if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
            _next_message--;
        }
        break;
    case STEP_PORT:
        _request_port();
        break;
    case STEP_POLL_SVINFO:
        // not required once we know what generation we are on
        if(_hardware_generation == 0) {
            _send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0);
        }
        break;
    case STEP_POLL_SBAS:
        _send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
	break;
    case STEP_POLL_NAV:
        _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
        break;
    case STEP_POLL_GNSS:
        _send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
        break;
    case STEP_NAV_RATE:
        _send_message(CLASS_CFG, MSG_CFG_RATE, NULL, 0);
        break;
    case STEP_POSLLH:
        if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
            _next_message--;
        }
        break;
    case STEP_STATUS:
        if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
            _next_message--;
        }
        break;
    case STEP_SOL:
        if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
            _next_message--;
        }
        break;
    case STEP_VELNED:
        if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
            _next_message--;
        }
        break;
    case STEP_DOP:
       if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
            _next_message--;
        }
        break;
    case STEP_MON_HW:
        if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
            _next_message--;
        }
        break;
    case STEP_MON_HW2:
        if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
            _next_message--;
        }
        break;
    case STEP_RAW:
#if UBLOX_RXM_RAW_LOGGING
        if(gps._raw_data == 0) {
            _unconfigured_messages &= ~CONFIG_RATE_RAW;
        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
            _next_message--;
        }
#else
        _unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
        break;
    case STEP_RAWX:
#if UBLOX_RXM_RAW_LOGGING
        if(gps._raw_data == 0) {
            _unconfigured_messages &= ~CONFIG_RATE_RAW;
        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
            _next_message--;
        }
#else
        _unconfigured_messages & = ~CONFIG_RATE_RAW;
#endif
        break;
    case STEP_VERSION:
        if(!_have_version && !hal.util->get_soft_armed()) {
            _request_version();
        } else {
            _unconfigured_messages &= ~CONFIG_VERSION;
        }
        // no need to send the initial rates, move to checking only
        _next_message = STEP_PORT;
        break;
    default:
        // this case should never be reached, do a full reset if it is hit
        _next_message = STEP_RATE_NAV;
        break;
    }
}
Ejemplo n.º 10
0
bool
AP_GPS_UBLOX::_parse_gps(void)
{
    if (_class == CLASS_ACK) {
        Debug("ACK %u", (unsigned)_msg_id);

        if(_msg_id == MSG_ACK_ACK) {
            switch(_buffer.ack.clsID) {
            case CLASS_CFG:
                switch(_buffer.ack.msgID) {
                case MSG_CFG_CFG:
                    _cfg_saved = true;
                    _cfg_needs_save = false;
                    break;
                case MSG_CFG_GNSS:
                    _unconfigured_messages &= ~CONFIG_GNSS;
                    break;
                case MSG_CFG_MSG:
                    // There is no way to know what MSG config was ack'ed, assume it was the last
                    // one requested. To verify it rerequest the last config we sent. If we miss
                    // the actual ack we will catch it next time through the poll loop, but that
                    // will be a good chunk of time later.
                    break;
                case MSG_CFG_NAV_SETTINGS:
                    _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
                    break;
                case MSG_CFG_RATE:
                    // The GPS will ACK a update rate that is invalid. in order to detect this
                    // only accept the rate as configured by reading the settings back and
                   //  validating that they all match the target values
                    break;
                case MSG_CFG_SBAS:
                    _unconfigured_messages &= ~CONFIG_SBAS;
                    break;
                }
                break;
            case CLASS_MON:
                switch(_buffer.ack.msgID) {
                case MSG_MON_HW:
                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
                    break;
                case MSG_MON_HW2:
                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
                    break;
                }
            }
        }
        return false;
    }

    if (_class == CLASS_CFG) {
        switch(_msg_id) {
        case  MSG_CFG_NAV_SETTINGS:
	    Debug("Got settings %u min_elev %d drLimit %u\n", 
                  (unsigned)_buffer.nav_settings.dynModel,
                  (int)_buffer.nav_settings.minElev,
                  (unsigned)_buffer.nav_settings.drLimit);
            _buffer.nav_settings.mask = 0;
            if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
                _buffer.nav_settings.dynModel != gps._navfilter) {
                // we've received the current nav settings, change the engine
                // settings and send them back
                Debug("Changing engine setting from %u to %u\n",
                      (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
                _buffer.nav_settings.dynModel = gps._navfilter;
                _buffer.nav_settings.mask |= 1;
            }
            if (gps._min_elevation != -100 &&
                _buffer.nav_settings.minElev != gps._min_elevation) {
                Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
                _buffer.nav_settings.minElev = gps._min_elevation;
                _buffer.nav_settings.mask |= 2;
            }
            if (_buffer.nav_settings.mask != 0) {
                _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                              &_buffer.nav_settings,
                              sizeof(_buffer.nav_settings));
                _unconfigured_messages |= CONFIG_NAV_SETTINGS;
                _cfg_needs_save = true;
            } else {
                _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
            }
            return false;

#if UBLOX_GNSS_SETTINGS
        case MSG_CFG_GNSS:
            if (gps._gnss_mode[state.instance] != 0) {
                struct ubx_cfg_gnss start_gnss = _buffer.gnss;
                uint8_t gnssCount = 0;
                Debug("Got GNSS Settings %u %u %u %u:\n",
                    (unsigned)_buffer.gnss.msgVer,
                    (unsigned)_buffer.gnss.numTrkChHw,
                    (unsigned)_buffer.gnss.numTrkChUse,
                    (unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUGGING
                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
                    Debug("  %u %u %u 0x%08x\n",
                    (unsigned)_buffer.gnss.configBlock[i].gnssId,
                    (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
                    (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
                    (unsigned)_buffer.gnss.configBlock[i].flags);
                }
#endif

                for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
                    if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
                        gnssCount++;
                    }
                }

                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
                    // Reserve an equal portion of channels for all enabled systems
                    if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
                        if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
                            _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                            _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
                        } else {
                            _buffer.gnss.configBlock[i].resTrkCh = 1;
                            _buffer.gnss.configBlock[i].maxTrkCh = 3;
                        }
                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
                    } else {
                        _buffer.gnss.configBlock[i].resTrkCh = 0;
                        _buffer.gnss.configBlock[i].maxTrkCh = 0;
                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
                    }
                }
                if (!memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
                    _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
                    _unconfigured_messages |= CONFIG_GNSS;
                    _cfg_needs_save = true;
                } else {
                    _unconfigured_messages &= ~CONFIG_GNSS;
                }
            } else {
                _unconfigured_messages &= ~CONFIG_GNSS;
            }
            return false;
#endif

        case MSG_CFG_SBAS:
            if (gps._sbas_mode != 2) {
	        Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
                      (unsigned)_buffer.sbas.mode,
                      (unsigned)_buffer.sbas.usage,
                      (unsigned)_buffer.sbas.maxSBAS,
                      (unsigned)_buffer.sbas.scanmode2,
                      (unsigned)_buffer.sbas.scanmode1);
                if (_buffer.sbas.mode != gps._sbas_mode) {
                    _buffer.sbas.mode = gps._sbas_mode;
                    _send_message(CLASS_CFG, MSG_CFG_SBAS,
                                  &_buffer.sbas,
                                  sizeof(_buffer.sbas));
                    _unconfigured_messages |= CONFIG_SBAS;
                    _cfg_needs_save = true;
                } else {
                    _unconfigured_messages &= ~CONFIG_SBAS;
                }
            } else {
                    _unconfigured_messages &= ~CONFIG_SBAS;
            }
            return false;
        case MSG_CFG_MSG:
            if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
                // can't verify the setting without knowing the port
                // request the port again
                if(_ublox_port >= UBLOX_MAX_PORTS) {
                    _request_port();
                    return false;
                }
                _verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
                             _buffer.msg_rate_6.rates[_ublox_port]);
            } else {
                _verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
                             _buffer.msg_rate.rate);
            }
            return false;
        case MSG_CFG_PRT:
           _ublox_port = _buffer.prt.portID;
           return false;
        case MSG_CFG_RATE:
            if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] ||
               _buffer.nav_rate.nav_rate != 1 ||
               _buffer.nav_rate.timeref != 0) {
               _configure_rate();
                _unconfigured_messages |= CONFIG_RATE_NAV;
                _cfg_needs_save = true;
            } else {
                _unconfigured_messages &= ~CONFIG_RATE_NAV;
            }
            return false;
        }
           
    }

    if (_class == CLASS_MON) {
        switch(_msg_id) {
        case MSG_MON_HW:
            if (_payload_length == 60 || _payload_length == 68) {
                log_mon_hw();
            }
            break;
        case MSG_MON_HW2:
            if (_payload_length == 28) {
                log_mon_hw2();  
            }
            break;
        case MSG_MON_VER:
            _have_version = true;
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, 
                                             "u-blox %d HW: %s SW: %s",
                                             state.instance,
                                             _buffer.mon_ver.hwVersion,
                                             _buffer.mon_ver.swVersion);
            break;
        default:
            unexpected_message();
        }
        return false;
    }

#if UBLOX_RXM_RAW_LOGGING
    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
        log_rxm_raw(_buffer.rxm_raw);
        return false;
    } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
        log_rxm_rawx(_buffer.rxm_rawx);
        return false;
    }
#endif // UBLOX_RXM_RAW_LOGGING

    if (_class != CLASS_NAV) {
        unexpected_message();
        return false;
    }

    switch (_msg_id) {
    case MSG_POSLLH:
        Debug("MSG_POSLLH next_fix=%u", next_fix);
        _last_pos_time        = _buffer.posllh.time;
        state.location.lng    = _buffer.posllh.longitude;
        state.location.lat    = _buffer.posllh.latitude;
        state.location.alt    = _buffer.posllh.altitude_msl / 10;
        state.status          = next_fix;
        _new_position = true;
        state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
        state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
        state.have_horizontal_accuracy = true;
        state.have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
        state.location.lng = 1491652300L;
        state.location.lat = -353632610L;
        state.location.alt = 58400;
        state.vertical_accuracy = 0;
        state.horizontal_accuracy = 0;
#endif
        break;
    case MSG_STATUS:
        Debug("MSG_STATUS fix_status=%u fix_type=%u",
              _buffer.status.fix_status,
              _buffer.status.fix_type);
        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
#if UBLOX_FAKE_3DLOCK
        state.status = AP_GPS::GPS_OK_FIX_3D;
        next_fix = state.status;
#endif
        break;
    case MSG_DOP:
        Debug("MSG_DOP");
        noReceivedHdop = false;
        state.hdop        = _buffer.dop.hDOP;
        state.vdop        = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
        state.hdop = 130;
        state.hdop = 170;
#endif
        break;
    case MSG_SOL:
        Debug("MSG_SOL fix_status=%u fix_type=%u",
              _buffer.solution.fix_status,
              _buffer.solution.fix_type);
        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
            if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
                (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
            }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                next_fix = AP_GPS::GPS_OK_FIX_3D;
            }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                next_fix = AP_GPS::GPS_OK_FIX_2D;
            }else{
                next_fix = AP_GPS::NO_FIX;
                state.status = AP_GPS::NO_FIX;
            }
        }else{
            next_fix = AP_GPS::NO_FIX;
            state.status = AP_GPS::NO_FIX;
        }
        if(noReceivedHdop) {
            state.hdop = _buffer.solution.position_DOP;
        }
        state.num_sats    = _buffer.solution.satellites;
        if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
            state.last_gps_time_ms = AP_HAL::millis();
            state.time_week_ms    = _buffer.solution.time;
            state.time_week       = _buffer.solution.week;
        }
#if UBLOX_FAKE_3DLOCK
        next_fix = state.status;
        state.num_sats = 10;
        state.time_week = 1721;
        state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
        state.last_gps_time_ms = AP_HAL::millis();
        state.hdop = 130;
#endif
        break;
    case MSG_VELNED:
        Debug("MSG_VELNED");
        _last_vel_time         = _buffer.velned.time;
        state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
        state.ground_course    = wrap_360(_buffer.velned.heading_2d * 1.0e-5f);       // Heading 2D deg * 100000
        state.have_vertical_velocity = true;
        state.velocity.x = _buffer.velned.ned_north * 0.01f;
        state.velocity.y = _buffer.velned.ned_east * 0.01f;
        state.velocity.z = _buffer.velned.ned_down * 0.01f;
        state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
        state.ground_speed = norm(state.velocity.y, state.velocity.x);
        state.have_speed_accuracy = true;
        state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK
        state.speed_accuracy = 0;
#endif
        _new_speed = true;
        break;
    case MSG_NAV_SVINFO:
        {
        Debug("MSG_NAV_SVINFO\n");
        static const uint8_t HardwareGenerationMask = 0x07;
        _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
        switch (_hardware_generation) {
            case UBLOX_5:
            case UBLOX_6:
                // only 7 and newer support CONFIG_GNSS
                _unconfigured_messages &= ~CONFIG_GNSS;
                break;
            case UBLOX_7:
            case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
                port->begin(4000000U);
                Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
                break;
            default:
                hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
                break;
        };
        _unconfigured_messages &= ~CONFIG_VERSION;
        /* We don't need that anymore */
        _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
        break;
        }
    default:
        Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
        if (++_disable_counter == 0) {
            Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
            _configure_message_rate(CLASS_NAV, _msg_id, 0);
        }
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
        _new_speed = _new_position = false;
        return true;
    }
    return false;
}