diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 32fec7724a..6f298deab6 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -4,7 +4,7 @@ extern "C" { typedef Localizer* Localizer_t; Localizer *localizer_init(bool has_ublox) { - return new Localizer(has_ublox); + return new Localizer(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM); } void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index af31218499..3616f0af85 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -70,7 +70,7 @@ static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); } -Localizer::Localizer() { +Localizer::Localizer(LocalizerGnssSource gnss_source) { this->kf = std::make_unique(); this->reset_kalman(); @@ -84,10 +84,9 @@ Localizer::Localizer() { VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + this->configure_gnss_source(gnss_source); } -Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; } - void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd predicted_state = this->kf->get_x(); MatrixXdr predicted_cov = this->kf->get_P(); @@ -325,12 +324,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; - float ecef_pos_std; - if (ublox_available) { - ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2)); - } else { - ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2)); - } + float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); @@ -372,10 +366,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: } double sensor_time = log.getMeasTime() * 1e-9; - if (ublox_available) - sensor_time -= GPS_UBLOX_SENSOR_TIME_OFFSET; - else - sensor_time -= GPS_QUECTEL_SENSOR_TIME_OFFSET; + sensor_time -= this->gps_time_offset; auto ecef_pos_v = log.getPositionECEF().getValue(); VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); @@ -664,16 +655,33 @@ void Localizer::determine_gps_mode(double current_time) { } } +void Localizer::configure_gnss_source(LocalizerGnssSource source) { + this->gnss_source = source; + if (source == LocalizerGnssSource::UBLOX) { + this->gps_std_factor = 10.0; + this->gps_variance_factor = 1.0; + this->gps_vertical_variance_factor = 1.0; + this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; + } else { + this->gps_std_factor = 2.0; + this->gps_variance_factor = 0.0; + this->gps_vertical_variance_factor = 3.0; + this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; + } +} + int Localizer::locationd_thread() { - ublox_available = Params().getBool("UbloxAvailable", true); + LocalizerGnssSource source; const char* gps_location_socket; - if (ublox_available) { + if (Params().getBool("UbloxAvailable", true)) { + source = LocalizerGnssSource::UBLOX; gps_location_socket = "gpsLocationExternal"; - this->gps_std_factor = 10.0; } else { + source = LocalizerGnssSource::QCOM; gps_location_socket = "gpsLocation"; - this->gps_std_factor = 2.0; } + + this->configure_gnss_source(source); const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index e2b2096afc..e8f2f04a2c 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -21,10 +21,13 @@ #define POSENET_STD_HIST_HALF 20 +enum LocalizerGnssSource { + UBLOX, QCOM +}; + class Localizer { public: - Localizer(); - Localizer(bool has_ublox); + Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); int locationd_thread(); @@ -81,10 +84,15 @@ private: double first_valid_log_time = NAN; double ttff = NAN; double last_gps_msg = 0; - bool ublox_available = true; + LocalizerGnssSource gnss_source; bool observation_timings_invalid = false; std::map observation_values_invalid; bool standstill = true; int32_t orientation_reset_count = 0; float gps_std_factor; + float gps_variance_factor; + float gps_vertical_variance_factor; + double gps_time_offset; + + void configure_gnss_source(LocalizerGnssSource source); };