diff --git a/cereal b/cereal index 22b1431132..f200875ca3 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 22b1431132b038253a24ab3fbbe3af36ef93b95b +Subproject commit f200875ca300d3a7b9293c4effcc9456e359e505 diff --git a/laika_repo b/laika_repo index e1049cde0a..5eb0c3c259 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55 +Subproject commit 5eb0c3c2596dd12a232b83bdb057a716810e89cf diff --git a/release/files_common b/release/files_common index f438904118..a06543abbf 100644 --- a/release/files_common +++ b/release/files_common @@ -233,7 +233,6 @@ selfdrive/locationd/generated/gps.cpp selfdrive/locationd/generated/gps.h selfdrive/locationd/laikad.py -selfdrive/locationd/laikad_helpers.py selfdrive/locationd/locationd.h selfdrive/locationd/locationd.cc selfdrive/locationd/paramsd.py diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 6936d88acc..1f13c2b69a 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -20,7 +20,7 @@ from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom -from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun +from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from selfdrive.locationd.models.gnss_kf import GNSSKalman from selfdrive.locationd.models.gnss_kf import States as GStates @@ -58,9 +58,9 @@ class Laikad: self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} - self.last_pos_fix = [] - self.last_pos_residual = [] - self.last_pos_fix_t = None + self.velfix_function = get_velfix_sympy_func() + self.last_fix_pos = None + self.last_fix_t = None self.gps_week = None self.use_qcom = use_qcom @@ -92,20 +92,23 @@ class Laikad: cloudlog.debug("Cache saved") self.last_cached_t = t - def get_est_pos(self, t, processed_measurements): - if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2: - min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 5 - pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) - if len(pos_fix) > 0: - self.last_pos_fix_t = t - residual_median = np.median(np.abs(pos_fix_residual)) - if np.median(np.abs(pos_fix_residual)) < POS_FIX_RESIDUAL_THRESHOLD: - cloudlog.debug(f"Pos fix is within threshold with median: {residual_median.round()}") - self.last_pos_fix = pos_fix[:3] - self.last_pos_residual = pos_fix_residual - else: - cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}") - return self.last_pos_fix + def get_lsq_fix(self, t, measurements): + if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: + min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 5 + position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) + + if len(position_solution) < 3: + return None + position_estimate = position_solution[:3] + #TODO median abs residual is decent estimate of std, can be improved with measurements stds and/or DOP + position_std = np.median(np.abs(pr_residuals)) * np.ones(3) + velocity_solution, prr_residuals = 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 = np.median(np.abs(prr_residuals)) * np.ones(3) + return position_estimate, position_std, velocity_estimate, velocity_std def is_good_report(self, gnss_msg): if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: @@ -148,11 +151,36 @@ class Laikad: self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) + def process_report(self, new_meas, t): + # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites + new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] + processed_measurements = process_measurements(new_meas, self.astro_dog) + + instant_fix = self.get_lsq_fix(t, processed_measurements) + if instant_fix is None: + return None + else: + position_estimate, position_std, velocity_estimate, velocity_std = instant_fix + self.last_fix_t = t + self.last_fix_pos = position_estimate + self.lat_fix_pos_std = position_std + + corrected_measurements = correct_measurements(processed_measurements, position_estimate, self.astro_dog) + if (t*1e9) % 10 == 0: + cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") + return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): - if self.is_good_report(gnss_msg): + if self.is_ephemeris(gnss_msg): + self.read_ephemeris(gnss_msg) + return None + elif self.is_good_report(gnss_msg): + week, tow, new_meas = self.read_report(gnss_msg) - self.gps_week = week + if len(new_meas) == 0: + return None + self.gps_week = week t = gnss_mono_time * 1e-9 if week > 0: self.got_first_gnss_msg = True @@ -160,44 +188,38 @@ class Laikad: if self.auto_fetch_orbits: self.fetch_orbits(latest_msg_t, block) - # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites - new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] - - processed_measurements = process_measurements(new_meas, self.astro_dog) - est_pos = self.get_est_pos(t, processed_measurements) - - corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else [] - if gnss_mono_time % 10 == 0: - cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") - - self.update_localizer(est_pos, t, corrected_measurements) - kf_valid = all(self.kf_valid(t)) - ecef_pos = self.gnss_kf.x[GStates.ECEF_POS] - ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY] - - p = self.gnss_kf.P.diagonal() - pos_std = np.sqrt(p[GStates.ECEF_POS]) - vel_std = np.sqrt(p[GStates.ECEF_VELOCITY]) + output = self.process_report(new_meas, t) + if output is None: + return None + position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output + self.update_localizer(position_estimate, t, corrected_measurements) meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] - dat = messaging.new_message("gnssMeasurements") + msg = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message - dat.gnssMeasurements = { + + P_diag = self.gnss_kf.P.diagonal() + kf_valid = all(self.kf_valid(t)) + msg.gnssMeasurements = { "gpsWeek": week, "gpsTimeOfWeek": tow, - "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), - "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), - # TODO std is incorrectly the dimension of the measurements and not 3D - "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), - "ubloxMonoTime": gnss_mono_time, + "kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(), + std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(), + valid=kf_valid), + "kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(), + std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(), + valid=kf_valid), + "positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)), + "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)), + + "measTime": gnss_mono_time, "correctedMeasurements": meas_msgs } - return dat - elif self.is_ephemeris(gnss_msg): - self.read_ephemeris(gnss_msg) + return msg #elif gnss_msg.which() == 'ionoData': - # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. + # TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them. + def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid @@ -349,9 +371,23 @@ class EphemerisSourceType(IntEnum): qcom = 3 -def main(sm=None, pm=None): +def process_msg(laikad, gnss_msg, mono_time, block=False): + # TODO: Understand and use remaining unknown constellations + if gnss_msg.which() == "drMeasurementReport": + if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: + return None + + if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: + # gpsWeek 65535 is received rarely from quectel, this cannot be + # passed to GnssMeasurements's gpsWeek (Int16) + return None + + return laikad.process_gnss_msg(gnss_msg, mono_time, block=block) + + +def main(sm=None, pm=None, qc=None): use_qcom = not Params().get_bool("UbloxAvailable", block=True) - if use_qcom: + if use_qcom or (qc is not None and qc): raw_gnss_socket = "qcomGnss" else: raw_gnss_socket = "ubloxGnss" @@ -371,19 +407,13 @@ def main(sm=None, pm=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] - # TODO: Understand and use remaining unknown constellations - if gnss_msg.which() == "drMeasurementReport": - if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: - continue + msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay) + if msg is None: + msg = messaging.new_message("gnssMeasurements") + msg.valid = False - if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: - # gpsWeek 65535 is received rarely from quectel, this cannot be - # passed to GnssMeasurements's gpsWeek (Int16) - continue + pm.send('gnssMeasurements', msg) - msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) - if msg is not None: - pm.send('gnssMeasurements', msg) if not laikad.got_first_gnss_msg and sm.updated['clocks']: clocks_msg = sm['clocks'] t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9)) diff --git a/selfdrive/locationd/laikad_helpers.py b/selfdrive/locationd/laikad_helpers.py deleted file mode 100644 index f13e8e73bb..0000000000 --- a/selfdrive/locationd/laikad_helpers.py +++ /dev/null @@ -1,89 +0,0 @@ -import numpy as np -import sympy - -from laika.constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT -from laika.helpers import ConstellationId - - -def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6): - ''' - Calculates gps fix using gauss newton method - To solve the problem a minimal of 4 measurements are required. - If Glonass is included 5 are required to solve for the additional free variable. - returns: - 0 -> list with positions - ''' - if x0 is None: - x0 = [0, 0, 0, 0, 0] - n = len(measurements) - if n < min_measurements: - return [], [] - - Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) - x = gauss_newton(Fx_pos, x0) - residual, _ = Fx_pos(x, weight=1.0) - return x.tolist(), residual.tolist() - - -def pr_residual(measurements, posfix_functions, signal='C1C'): - def Fx_pos(inp, weight=None): - vals, gradients = [], [] - - for meas in measurements: - pr = meas.observables[signal] - pr += meas.sat_clock_err * SPEED_OF_LIGHT - - w = (1 / meas.observables_std[signal]) if weight is None else weight - - val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w) - vals.append(val) - gradients.append(gradient) - return np.asarray(vals), np.asarray(gradients) - - return Fx_pos - - -def gauss_newton(fun, b, xtol=1e-8, max_n=25): - for _ in range(max_n): - # Compute function and jacobian on current estimate - r, J = fun(b) - - # Update estimate - delta = np.linalg.pinv(J) @ r - b -= delta - - # Check step size for stopping condition - if np.linalg.norm(delta) < xtol: - break - return b - - -def get_posfix_sympy_fun(constellation): - # Unknowns - x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z') - bc = sympy.Symbol('bc') - bg = sympy.Symbol('bg') - var = [x, y, z, bc, bg] - - # Knowns - pr = sympy.Symbol('pr') - sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z') - weight = sympy.Symbol('weight') - - theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT - val = sympy.sqrt( - (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 + - (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 + - (sat_z - z) ** 2 - ) - - if constellation == ConstellationId.GLONASS: - res = weight * (val - (pr - bc - bg)) - elif constellation == ConstellationId.GPS: - res = weight * (val - (pr - bc)) - else: - raise NotImplementedError(f"Constellation {constellation} not supported") - - res = [res] + [sympy.diff(res, v) for v in var] - - return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res, modules=["numpy"]) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 4325900c0e..442adcd347 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -25,8 +25,15 @@ 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_LOCATION_SENSOR_TIME_OFFSET = 0.630; // s -const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095; // s +const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s +const float GPS_MUL_FACTOR = 10.0; +const float GPS_POS_STD_THRESHOLD = 50.0; +const float GPS_VEL_STD_THRESHOLD = 5.0; +const float GPS_POS_ERROR_RESET_THRESHOLD = 200.0; +const float GPS_POS_STD_RESET_THRESHOLD = 5.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 5.0; static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -316,8 +323,8 @@ 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; - MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal(); - MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal(); + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_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(); @@ -348,6 +355,77 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::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) { + + this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid(); + if (!this->gps_valid) { + this->determine_gps_mode(current_time); + return; + } + this->gps_mode = true; + + 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; + + 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(GPS_MUL_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]); + + // 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(GPS_MUL_FACTOR*ecef_vel_std, 2)).asDiagonal(); + + 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); + + LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; + VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); + double bearing_rad = atan2(ned_vel[1], ned_vel[0]); + + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); + 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_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + this->gps_valid = false; + this->determine_gps_mode(current_time); + return; + } + + 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); + } else if (ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD && orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD) { + LOGE("Locationd vs gnssMeasurement 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 }); + } + + this->last_gps_msg = current_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_car_state(double current_time, const cereal::CarState::Reader& log) { this->car_speed = std::abs(log.getVEgo()); if (log.getStandstill()) { @@ -497,9 +575,11 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation(), GPS_LOCATION_SENSOR_TIME_OFFSET); + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal(), GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET); + 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()) { diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index f0872d9f56..7a282be9d7 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -52,6 +52,7 @@ public: 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); void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); @@ -77,6 +78,7 @@ private: bool device_fell = false; bool gps_mode = false; bool gps_valid = false; + double last_gps_msg = 0; bool ublox_available = true; bool observation_timings_invalid = false; std::map observation_values_invalid; diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py index 427003b24c..7aa588d43e 100755 --- a/selfdrive/locationd/test/test_ublox_processing.py +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -4,7 +4,8 @@ import numpy as np from laika import AstroDog from laika.helpers import ConstellationId -from laika.raw_gnss import calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox +from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ublox +from laika.opt import calc_pos_fix from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -54,14 +55,14 @@ class TestUbloxProcessing(unittest.TestCase): processed_meas = process_measurements(measurements, dog) count_processed_measurements += len(processed_meas) pos_fix = calc_pos_fix(processed_meas) - if len(pos_fix) > 0 and all(pos_fix[0] != 0): + if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]): position_fix_found += 1 corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog) count_corrected_measurements += len(corrected_meas) pos_fix = calc_pos_fix(corrected_meas) - if len(pos_fix) > 0 and all(pos_fix[0] != 0): + if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]): pos_ests.append(pos_fix[0]) position_fix_found_after_correcting += 1 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index bc55e33cbf..c5465fc025 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -251,8 +251,10 @@ def ublox_rcv_callback(msg): def laika_rcv_callback(msg, CP, cfg, fsm): if msg.which() == 'ubloxGnss' and msg.ubloxGnss.which() == "measurementReport": return ["gnssMeasurements"], True + elif msg.which() == 'qcomGnss' and msg.qcomGnss.which() == "drMeasurementReport": + return ["gnssMeasurements"], True else: - return [], True + return [], False CONFIGS = [ @@ -360,24 +362,11 @@ CONFIGS = [ tolerance=None, fake_pubsubmaster=False, ), - ProcessConfig( - proc_name="laikad", - subtest_name="Offline", - pub_sub={ - "ubloxGnss": ["gnssMeasurements"], - "clocks": [] - }, - ignore=["logMonoTime"], - init_callback=get_car_params, - should_recv_callback=laika_rcv_callback, - tolerance=NUMPY_TOLERANCE, - fake_pubsubmaster=True, - environ={"LAIKAD_NO_INTERNET": "1"}, - ), ProcessConfig( proc_name="laikad", pub_sub={ "ubloxGnss": ["gnssMeasurements"], + "qcomGnss": ["gnssMeasurements"], "clocks": [] }, ignore=["logMonoTime"], @@ -474,6 +463,12 @@ def python_replay_process(cfg, lr, fingerprint=None): all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] + # laikad needs decision between submaster ubloxGnss and qcomGnss, prio given to ubloxGnss + if cfg.proc_name == "laikad": + args = (*args, not any(m.which() == "ubloxGnss" for m in pub_msgs)) + service = "qcomGnss" if args[2] else "ubloxGnss" + pub_msgs = [m for m in pub_msgs if m.which() == service or m.which() == 'clocks'] + controlsState = None initialized = False for msg in lr: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1d586b0334..603d2b2d96 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -05ba201bcf97b6c1067dbcde2a60f71f43469c56 +557ffbf5a1c9cafba1ff8d6f3e2642df98b2d6e6 \ No newline at end of file