From fda3106c28af236e8b3dc78bfdca7a1365904d1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Tue, 22 Aug 2023 14:12:51 -0700 Subject: [PATCH] Revert laikad llk (#29535) * Revert "bump laika (#29522)" This reverts commit 3523bef732316b534de66dcb9e7d29e53e37e99f. * Revert "Locationd: switch to laikad (#29278)" This reverts commit fd3b8a3d7511ad3d7c5aa0b82a1e2c28505e5a7f. --- laika_repo | 2 +- selfdrive/locationd/laikad.py | 59 +++++--- selfdrive/locationd/liblocationd.cc | 2 +- selfdrive/locationd/locationd.cc | 129 +++++++++++++++--- selfdrive/locationd/locationd.h | 13 +- selfdrive/locationd/test/test_laikad.py | 4 +- selfdrive/locationd/test/test_locationd.py | 29 ++-- .../test/process_replay/process_replay.py | 12 +- selfdrive/test/process_replay/ref_commit | 2 +- 9 files changed, 195 insertions(+), 57 deletions(-) diff --git a/laika_repo b/laika_repo index 989b6a8505..65901a1e6c 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 989b6a8505441a550c94f60b4378be296ea3fe92 +Subproject commit 65901a1e6c7c7940d8c97027d3edaca7d0132253 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 6c9c199db1..2073813363 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -18,8 +18,7 @@ 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 -from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom 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 @@ -103,10 +102,9 @@ 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 = 4 + self.qcom_reports_received = 1 self.qcom_reports = [] def load_cache(self): @@ -163,31 +161,47 @@ 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 = pos_std[: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) 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 = vel_std[: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) 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() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom: + if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: # TODO: Understand and use remaining unknown constellations try: - 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) + constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] - report_time = gps_time_from_qcom_report(gnss_msg) + report_time = self.gps_time_from_qcom_report(gnss_msg) except NotImplementedError: return False # Garbage timestamps with week > 32767 are sometimes sent by module. @@ -202,20 +216,21 @@ class Laikad: def read_report(self, gnss_msg): if self.use_qcom: # QCOM reports are per constellation, so we need to aggregate them - # 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) + report = gnss_msg.drMeasurementReport + report_time = self.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 = [gnss_msg] + self.qcom_reports = [report] else: - self.qcom_reports.append(gnss_msg) + self.qcom_reports.append(report) self.last_report_time = report_time + new_meas = [] if len(self.qcom_reports) == self.qcom_reports_received: - new_meas = get_measurements_from_qcom_reports(self.qcom_reports) - else: - new_meas = [] + for report in self.qcom_reports: + new_meas.extend(read_raw_qcom(report)) + else: report = gnss_msg.measurementReport self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) @@ -285,7 +300,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 - int(1e9 * self.measurement_lag)} + msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time} 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 73ed1b0a56..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(); + 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 d17f6593e4..68f16a572e 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -13,6 +13,7 @@ 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 @@ -22,11 +23,15 @@ 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 = 10.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 1.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; @@ -65,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(); @@ -79,6 +84,7 @@ 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); } void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { @@ -294,6 +300,64 @@ 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()) { @@ -302,22 +366,21 @@ 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]); - 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(); + // 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_vel_v = log.getVelocityECEF().getValue(); VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - 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(); + // 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(); double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); @@ -340,13 +403,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_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) { + if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { this->determine_gps_mode(current_time); return; } // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD; + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; orientation_reset &= !this->standstill; if (orientation_reset) { @@ -356,7 +419,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_norm < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < 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); @@ -526,8 +589,12 @@ 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.isGnssMeasurements()) { - this->handle_gnss(t, log.getGnssMeasurements()); + } 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.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -589,12 +656,38 @@ 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() { - const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", + 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", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index cfa2d0a667..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(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); int locationd_thread(); @@ -52,6 +55,7 @@ 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); @@ -80,8 +84,15 @@ 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 a8d69502cf..58c650e26c 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 = 55 if use_qcom else 560 - valid_fix_expected = 55 if use_qcom else 560 + correct_msgs_expected = 56 if use_qcom else 560 + valid_fix_expected = 56 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 1fb36bf2fa..6c6ac33431 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 = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -35,14 +35,25 @@ class TestLocationdProc(unittest.TestCase): msg = messaging.new_message(name) except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - 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 == "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 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 c0a49e3324..9256b6015c 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -433,6 +433,13 @@ 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", @@ -487,11 +494,12 @@ CONFIGS = [ ProcessConfig( proc_name="locationd", pubs=[ - "cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements", - "liveCalibration", "carState", "carParams" + "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", + "liveCalibration", "carState", "carParams", "gpsLocation" ], 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 564eafa597..8aec881aa5 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -89db4d10a4eb0bfd0ea0ff83525e5cf358f5f690 \ No newline at end of file +72e3d7b660ee92f5adcc249112cf04c703f4bf9e \ No newline at end of file