diff --git a/laika_repo b/laika_repo index 65901a1e6c..6c79c96474 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 65901a1e6c7c7940d8c97027d3edaca7d0132253 +Subproject commit 6c79c964748ab930bb3699c06025bb2b0565cbea diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2073813363..6c9c199db1 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -18,7 +18,8 @@ from laika.downloader import DownloadFailed from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId, get_sv_id -from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox +from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman @@ -102,9 +103,10 @@ class Laikad: self.use_qcom = use_qcom self.first_log_time = None self.ttff = -1 + self.measurement_lag = 0.630 if self.use_qcom else 0.095 # qcom specific stuff - self.qcom_reports_received = 1 + self.qcom_reports_received = 4 self.qcom_reports = [] def load_cache(self): @@ -161,47 +163,31 @@ class Laikad: def get_lsq_fix(self, t, measurements): if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4 + position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) if len(position_solution) < 3: return None position_estimate = position_solution[:3] - - position_std_residual = np.median(np.abs(pr_residuals)) - position_std = np.median(np.abs(pos_std))/10 - position_std = max(position_std_residual, position_std) * np.ones(3) + position_std = pos_std[:3] velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) if len(velocity_solution) < 3: return None velocity_estimate = velocity_solution[:3] - - velocity_std_residual = np.median(np.abs(prr_residuals)) - velocity_std = np.median(np.abs(vel_std))/10 - velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3) + velocity_std = vel_std[:3] return position_estimate, position_std, velocity_estimate, velocity_std - def gps_time_from_qcom_report(self, gnss_msg): - report = gnss_msg.drMeasurementReport - if report.source == log.QcomGnss.MeasurementSource.gps: - report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) - elif report.source == log.QcomGnss.MeasurementSource.sbas: - report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) - elif report.source == log.QcomGnss.MeasurementSource.glonass: - report_time = GPSTime.from_glonass(report.glonassYear, - report.glonassDay, - report.glonassMilliseconds / 1000.0) - else: - raise NotImplementedError(f'Unknownconstellation {report.source}') - return report_time - def is_good_report(self, gnss_msg): - if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: + if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom: # TODO: Understand and use remaining unknown constellations try: - constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) + if gnss_msg.which() == 'drMeasurementReport': + constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) + else: + constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source) good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] - report_time = self.gps_time_from_qcom_report(gnss_msg) + report_time = gps_time_from_qcom_report(gnss_msg) except NotImplementedError: return False # Garbage timestamps with week > 32767 are sometimes sent by module. @@ -216,21 +202,20 @@ class Laikad: def read_report(self, gnss_msg): if self.use_qcom: # QCOM reports are per constellation, so we need to aggregate them - report = gnss_msg.drMeasurementReport - report_time = self.gps_time_from_qcom_report(gnss_msg) - + # Additionally, the pseudoranges are broken in the measurementReports + # and the doppler filteredSpeed is broken in the drMeasurementReports + report_time = gps_time_from_qcom_report(gnss_msg) if report_time - self.last_report_time > 0: self.qcom_reports_received = max(1, len(self.qcom_reports)) - self.qcom_reports = [report] + self.qcom_reports = [gnss_msg] else: - self.qcom_reports.append(report) + self.qcom_reports.append(gnss_msg) self.last_report_time = report_time - new_meas = [] if len(self.qcom_reports) == self.qcom_reports_received: - for report in self.qcom_reports: - new_meas.extend(read_raw_qcom(report)) - + new_meas = get_measurements_from_qcom_reports(self.qcom_reports) + else: + new_meas = [] else: report = gnss_msg.measurementReport self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) @@ -300,7 +285,7 @@ class Laikad: def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): out_msg = messaging.new_message("gnssMeasurements") t = gnss_mono_time * 1e-9 - msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time} + msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)} if self.first_log_time is None: self.first_log_time = 1e-9 * gnss_mono_time if self.is_ephemeris(gnss_msg): diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 6f298deab6..73ed1b0a56 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 ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM); + return new Localizer(); } 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 9b3e3b3b85..aedc4ac371 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -13,7 +13,6 @@ const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 const double ROTATION_SANITY_CHECK = 10.0; // rad/s const double TRANS_SANITY_CHECK = 200.0; // m/s const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) -const double ALTITUDE_SANITY_CHECK = 10000; // m const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m @@ -23,15 +22,11 @@ const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker const double DECAY = 0.99995; // same as reset tracker const double MAX_FILTER_REWIND_TIME = 0.8; // s -// TODO: GPS sensor time offsets are empirically calculated -// They should be replaced with synced time from a real clock -const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s -const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s const float GPS_POS_STD_THRESHOLD = 50.0; const float GPS_VEL_STD_THRESHOLD = 5.0; const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; -const float GPS_POS_STD_RESET_THRESHOLD = 2.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_POS_STD_RESET_THRESHOLD = 10.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 1.0; const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; @@ -70,7 +65,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(LocalizerGnssSource gnss_source) { +Localizer::Localizer() { this->kf = std::make_unique(); this->reset_kalman(); @@ -84,7 +79,6 @@ Localizer::Localizer(LocalizerGnssSource gnss_source) { 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); } void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { @@ -300,64 +294,6 @@ void Localizer::input_fake_gps_observations(double current_time) { this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } -void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { - // ignore the message if the fix is invalid - bool gps_invalid_flag = (log.getFlags() % 2 == 0); - bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); - bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); - bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); - bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - - if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { - //this->gps_valid = false; - this->determine_gps_mode(current_time); - return; - } - - double sensor_time = current_time - sensor_time_offset; - - // Process message - //this->gps_valid = true; - this->gps_mode = true; - Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; - this->converter = std::make_unique(geodetic); - - 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 = 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(); - - this->unix_timestamp_millis = log.getUnixTimestampMillis(); - double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); - - VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); - VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); - VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); - VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; - for (int i = 0; i < orientation_error.size(); i++) { - orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); - if (orientation_error(i) < 0.0) { - orientation_error(i) += 2.0 * M_PI; - } - orientation_error(i) -= M_PI; - } - VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); - - if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { - LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); - } else if (gps_est_error > 100.0) { - LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } - - this->last_gps_msg = sensor_time; - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { @@ -366,21 +302,22 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: } double sensor_time = log.getMeasTime() * 1e-9; - 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]); - // indexed at 0 cause all std values are the same MAE - auto ecef_pos_std = log.getPositionECEF().getStd()[0]; - MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); + auto ecef_pos_std = log.getPositionECEF().getStd(); + VectorXd ecef_pos_var = Vector3d(ecef_pos_std[0], ecef_pos_std[1], ecef_pos_std[2]).array().square().matrix(); + double ecef_pos_std_norm = std::sqrt(ecef_pos_var.sum()); + MatrixXdr ecef_pos_R = ecef_pos_var.asDiagonal(); auto ecef_vel_v = log.getVelocityECEF().getValue(); VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - // indexed at 0 cause all std values are the same MAE - auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; - MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); + auto ecef_vel_std = log.getVelocityECEF().getStd(); + VectorXd ecef_vel_var = Vector3d(ecef_vel_std[0], ecef_vel_std[1], ecef_vel_std[2]).array().square().matrix(); + double ecef_vel_std_norm = std::sqrt(ecef_vel_var.sum()); + MatrixXdr ecef_vel_R = ecef_vel_var.asDiagonal(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -403,13 +340,13 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: } VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); - if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + if (ecef_pos_std_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) { this->determine_gps_mode(current_time); return; } // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD; orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; orientation_reset &= !this->standstill; if (orientation_reset) { @@ -419,7 +356,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: this->orientation_reset_count = 0; } - if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std_norm < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { // always reset on first gps message and if the location is off but the accuracy is high LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); @@ -588,12 +525,8 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->handle_sensor(t, log.getAccelerometer()); } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); - } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - //} else if (log.isGnssMeasurements()) { - // this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isGnssMeasurements()) { + this->handle_gnss(t, log.getGnssMeasurements()); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -655,38 +588,12 @@ 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() { - LocalizerGnssSource source; - const char* gps_location_socket; - if (Params().getBool("UbloxAvailable")) { - source = LocalizerGnssSource::UBLOX; - gps_location_socket = "gpsLocationExternal"; - } else { - source = LocalizerGnssSource::QCOM; - gps_location_socket = "gpsLocation"; - } - - this->configure_gnss_source(source); - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index e8f2f04a2c..cfa2d0a667 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -21,13 +21,10 @@ #define POSENET_STD_HIST_HALF 20 -enum LocalizerGnssSource { - UBLOX, QCOM -}; class Localizer { public: - Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); + Localizer(); int locationd_thread(); @@ -55,7 +52,6 @@ public: void handle_msg_bytes(const char *data, const size_t size); void handle_msg(const cereal::Event::Reader& log); void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); - void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); void handle_car_state(double current_time, const cereal::CarState::Reader& log); void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); @@ -84,15 +80,8 @@ private: double first_valid_log_time = NAN; double ttff = NAN; double last_gps_msg = 0; - 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); }; diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 58c650e26c..a8d69502cf 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -177,8 +177,8 @@ class TestLaikad(unittest.TestCase): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom) # Disable fetch_orbits to test NAV only correct_msgs = verify_messages(logs, laikad) - correct_msgs_expected = 56 if use_qcom else 560 - valid_fix_expected = 56 if use_qcom else 560 + correct_msgs_expected = 55 if use_qcom else 560 + valid_fix_expected = 55 if use_qcom else 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 6c6ac33431..1fb36bf2fa 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -14,7 +14,7 @@ from openpilot.selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): - LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -35,25 +35,14 @@ class TestLocationdProc(unittest.TestCase): msg = messaging.new_message(name) except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - - if name == "gpsLocationExternal": - msg.gpsLocationExternal.flags = 1 - msg.gpsLocationExternal.verticalAccuracy = 1.0 - msg.gpsLocationExternal.speedAccuracy = 1.0 - msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 - msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] - msg.gpsLocationExternal.latitude = float(self.lat) - msg.gpsLocationExternal.longitude = float(self.lon) - msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 - msg.gpsLocationExternal.altitude = float(self.alt) - #if name == "gnssMeasurements": - # msg.gnssMeasurements.measTime = t - # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] - # msg.gnssMeasurements.positionECEF.std = [0,0,0] - # msg.gnssMeasurements.positionECEF.valid = True - # msg.gnssMeasurements.velocityECEF.value = [] - # msg.gnssMeasurements.velocityECEF.std = [0,0,0] - # msg.gnssMeasurements.velocityECEF.valid = True + if name == "gnssMeasurements": + msg.gnssMeasurements.measTime = t + msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + msg.gnssMeasurements.positionECEF.std = [0,0,0] + msg.gnssMeasurements.positionECEF.valid = True + msg.gnssMeasurements.velocityECEF.value = [] + msg.gnssMeasurements.velocityECEF.std = [0,0,0] + msg.gnssMeasurements.velocityECEF.valid = True elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9256b6015c..c0a49e3324 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -433,13 +433,6 @@ def laikad_config_pubsub_callback(params, cfg, lr): cfg.main_pub_drained = True -def locationd_config_pubsub_callback(params, cfg, lr): - ublox = params.get_bool("UbloxAvailable") - sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) - - cfg.pubs = set(cfg.pubs) - sub_keys - - CONFIGS = [ ProcessConfig( proc_name="controlsd", @@ -494,12 +487,11 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", - "liveCalibration", "carState", "carParams", "gpsLocation" + "cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements", + "liveCalibration", "carState", "carParams" ], subs=["liveLocationKalman"], ignore=["logMonoTime", "valid"], - config_callback=locationd_config_pubsub_callback, tolerance=NUMPY_TOLERANCE, ), ProcessConfig( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8aec881aa5..564eafa597 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -72e3d7b660ee92f5adcc249112cf04c703f4bf9e \ No newline at end of file +89db4d10a4eb0bfd0ea0ff83525e5cf358f5f690 \ No newline at end of file