sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Laikad preperation (#26800)

Browse Source 
* laikad update, renaming

* update locationd

* address PR comments

* draft to fix replay

* fix process relay to allow no response for messages

* bump cereal

* update process replay ref commit

* move laikad helpers to laika

* fix ublox test

* update refs

* add proper qcom replay support

* fix gnss support if both is available

* update refs

* move laika back to master

* move cereal back to master

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: 981532f0c3
beeps
Kurt Nistelberger 2 years ago committed by GitHub
parent d37a669449
commit c469ee2f6c
10 changed files with 197 additions and 179 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 2
      laika_repo
  3. 1
      release/files_common
  4. 154
      selfdrive/locationd/laikad.py
  5. 89
      selfdrive/locationd/laikad_helpers.py
  6. 92
      selfdrive/locationd/locationd.cc
  7. 2
      selfdrive/locationd/locationd.h
  8. 7
      selfdrive/locationd/test/test_ublox_processing.py
  9. 25
      selfdrive/test/process_replay/process_replay.py
  10. 2
      selfdrive/test/process_replay/ref_commit

2
cereal

@ -1 +1 @@
Subproject commit 22b1431132b038253a24ab3fbbe3af36ef93b95b
Subproject commit f200875ca300d3a7b9293c4effcc9456e359e505

2
laika_repo

@ -1 +1 @@
Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55
Subproject commit 5eb0c3c2596dd12a232b83bdb057a716810e89cf

1
release/files_common
Unescape View File

@ -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

154
selfdrive/locationd/laikad.py
Unescape View File

@ -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))

89
selfdrive/locationd/laikad_helpers.py
Unescape View File

@ -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"])

92
selfdrive/locationd/locationd.cc
Unescape View File

@ -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<float, capnp::Kind::PRIMITIVE>::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_LEN>(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_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(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()) {

2
selfdrive/locationd/locationd.h
Unescape View File

@ -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<std::string, double> observation_values_invalid;

7
selfdrive/locationd/test/test_ublox_processing.py
Unescape View File

@ -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

25
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -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:

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
05ba201bcf97b6c1067dbcde2a60f71f43469c56
557ffbf5a1c9cafba1ff8d6f3e2642df98b2d6e6
Write Preview
Loading…
Cancel
Save