IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ 7550fa9

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-02 13:32:42 -05:00
parent 4efaff4cb2
commit 43ee82d228
24 changed files with 882 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
iqpilot/selfdrive/iqlocd/atlas_loc_core.cc
View File

@@ -48,6 +48,18 @@ static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE
return res;
}
static bool finite_vector3(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
if (floatlist.size() != 3) {
return false;
}
for (int i = 0; i < 3; i++) {
if (!std::isfinite(floatlist[i])) {
return false;
}
}
return true;
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
@@ -257,6 +269,10 @@ void AtlasLocator::consume_sensor_frame(double current_time, const cereal::Senso
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
if (!finite_vector3(v)) {
this->observation_values_invalid["gyroscope"] += 1.0;
return;
}
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
@@ -275,6 +291,10 @@ void AtlasLocator::consume_sensor_frame(double current_time, const cereal::Senso
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
if (!finite_vector3(v)) {
this->observation_values_invalid["accelerometer"] += 1.0;
return;
}
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
@@ -295,6 +315,10 @@ void AtlasLocator::seed_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && current_time < filter_time) {
return;
}
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
@@ -308,12 +332,16 @@ void AtlasLocator::seed_fake_gps_observations(double current_time) {
}
void AtlasLocator::consume_gps_frame(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_malformed = !finite_vector3(log.getVNED()) ||
!std::isfinite(log.getLatitude()) || !std::isfinite(log.getLongitude()) || !std::isfinite(log.getAltitude()) ||
!std::isfinite(log.getHorizontalAccuracy()) || !std::isfinite(log.getVerticalAccuracy()) ||
!std::isfinite(log.getSpeedAccuracy()) || !std::isfinite(log.getBearingAccuracyDeg()) || !std::isfinite(log.getBearingDeg());
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), 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);
bool gps_vel_insane = gps_malformed || (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
if (!log.getHasFix() || gps_malformed || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->refresh_gps_mode(current_time);
return;
@@ -450,6 +478,12 @@ void AtlasLocator::consume_car_state_frame(double current_time, const cereal::Ca
}
void AtlasLocator::consume_camera_odometry(double current_time, const cereal::CameraOdometry::Reader& log) {
if (!finite_vector3(log.getRot()) || !finite_vector3(log.getTrans()) ||
!finite_vector3(log.getRotStd()) || !finite_vector3(log.getTransStd())) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
@@ -499,6 +533,10 @@ void AtlasLocator::consume_live_calibration(double current_time, const cereal::E
}
if (log.getRpyCalib().size() > 0) {
if (!finite_vector3(log.getRpyCalib())) {
this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
return;
}
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
@@ -520,7 +558,7 @@ void AtlasLocator::reset_kalman(double current_time) {
}
void AtlasLocator::run_finite_guard(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
bool all_finite = this->kf->get_x().array().isFinite().all() && this->kf->get_P().array().isFinite().all();
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
@@ -590,24 +628,29 @@ void AtlasLocator::consume_bytes(const char *data, const size_t size) {
void AtlasLocator::consume_event(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->run_time_guard(t);
if (log.isAccelerometer()) {
this->consume_sensor_frame(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->consume_sensor_frame(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->consume_gnss_frame(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->consume_car_state_frame(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->consume_camera_odometry(t, log.getCameraOdometry());
} else if (log.isExtrinsicsCalibration()) {
this->consume_live_calibration(t, log.getExtrinsicsCalibration());
try {
if (log.isAccelerometer()) {
this->consume_sensor_frame(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->consume_sensor_frame(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->consume_gnss_frame(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->consume_car_state_frame(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->consume_camera_odometry(t, log.getCameraOdometry());
} else if (log.isExtrinsicsCalibration()) {
this->consume_live_calibration(t, log.getExtrinsicsCalibration());
}
} catch (const std::exception &e) {
LOGE("Estimator rejected an observation (%s), kalman reset", e.what());
this->reset_kalman(t);
}
this->run_finite_guard();
this->run_finite_guard(t);
this->cool_reset_tracker();
}

58
iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py
View File

@@ -112,3 +112,61 @@ class TestIQLocdProc:
assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.2)
def _well_formed_burst(self, t0, frames=60):
published = 0
for i in range(frames):
t = t0 + i * 50_000_000
for name in self.LLD_MSGS:
self.pm.send(name, self.get_msg(name, t))
self.pm.wait_for_readers_to_update("cameraOdometry", 0.1, dt=0.005)
self.sm.update(0)
published += int(self.sm.updated["iqLiveLocation"])
return t0 + frames * 50_000_000, published
def _malformed(self, case, t):
if case in ("odometry_short", "odometry_empty", "odometry_nan", "odometry_nan_std"):
msg = messaging.new_message("cameraOdometry")
odo = msg.cameraOdometry
if case == "odometry_short":
odo.rot = [0.0] * 6; odo.trans = [0.0] * 6; odo.rotStd = [0.01] * 6; odo.transStd = [0.01] * 6
elif case == "odometry_nan":
odo.rot = [float("nan"), 0.0, 0.0]; odo.trans = [0.0] * 3; odo.rotStd = [0.01] * 3; odo.transStd = [0.01] * 3
elif case == "odometry_nan_std":
odo.rot = [0.0] * 3; odo.trans = [0.0] * 3; odo.rotStd = [float("nan"), 0.01, 0.01]; odo.transStd = [0.01] * 3
elif case in ("calibration_short", "calibration_nan"):
msg = messaging.new_message("extrinsicsCalibration")
msg.extrinsicsCalibration.calStatus = "calibrated"
msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0] if case == "calibration_short" else [float("nan"), 0.0, 0.0]
elif case in ("gps_short", "gps_nan"):
msg = self.get_msg("gpsLocationExternal", t)
msg.gpsLocationExternal.vNED = [0.0, 0.0] if case == "gps_short" else [float("nan"), 0.0, 0.0]
elif case == "gyro_nan":
msg = self.get_msg("gyroscope", t)
msg.gyroscope.gyroUncalibrated.v = [float("nan"), 0.0, 0.0]
elif case == "accel_short":
msg = self.get_msg("accelerometer", t)
msg.accelerometer.acceleration.v = [0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
@pytest.mark.parametrize("case", (
"odometry_short", "odometry_empty", "odometry_nan", "odometry_nan_std",
"calibration_short", "calibration_nan", "gps_short", "gps_nan", "gyro_nan", "accel_short",
))
def test_malformed_input_does_not_kill_the_process(self, case):
random.seed(1)
self.x, self.y, self.z = -2710700.0, -4280600.0, 3850300.0
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
t, _ = self._well_formed_burst(int(1e9))
assert self.proc.poll() is None
msg = self._malformed(case, t)
self.pm.send(msg.which(), msg)
time.sleep(0.05)
_, published = self._well_formed_burst(t + 50_000_000)
assert self.proc.poll() is None, f"iqlocd died on {case} with {self.proc.returncode}"
assert published >= 30