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

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
iqpilot/selfdrive/iqlocd/__init__.py Normal file
View File

BIN
iqpilot/selfdrive/iqlocd/iqlocd Executable file
View File

Binary file not shown.

0
iqpilot/selfdrive/iqlocd/models/__init__.py Normal file
View File

94
iqpilot/selfdrive/iqlocd/models/car_kf.py Normal file
View File

@@ -0,0 +1,94 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
Q = np.diag([(.05 / 100)**2, .01**2, math.radians(0.02)**2, math.radians(0.25)**2,
.1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name != "self":
self.filter.set_global(name, value)

88
iqpilot/selfdrive/iqlocd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]

0
iqpilot/selfdrive/iqlocd/tests/__init__.py Normal file
View File

173
iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py Normal file
View File

@@ -0,0 +1,173 @@
import pytest
import json
import os
import random
import subprocess
import time
import capnp
from pathlib import Path
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.params import Params
from iqpilot.common.transformations.coordinates import ecef2geodetic
from iqpilot.common.basedir import BASEDIR
@pytest.mark.linux
class TestIQLocdProc:
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
'accelerometer', 'gyroscope']
@pytest.fixture(autouse=True)
def setup_iqlocd(self, openpilot_function_fixture):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.sm = messaging.SubMaster(['iqLiveLocation'])
self.params = Params()
assert self.params.get_param_path().endswith(os.environ['OPENPILOT_PREFIX'])
self.params.put_bool("UbloxAvailable", True)
iqlocd_dir = Path(BASEDIR) / 'iqpilot/selfdrive/iqlocd'
self.proc = subprocess.Popen(['./iqlocd'], cwd=iqlocd_dir, env=os.environ.copy())
yield
self.proc.terminate()
self.proc.wait(timeout=5)
def get_msg(self, name, t):
try:
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal":
gps = getattr(msg, name)
gps.flags = 1
gps.hasFix = True
gps.source = 'ublox'
gps.horizontalAccuracy = 1.0
gps.verticalAccuracy = 1.0
gps.speedAccuracy = 1.0
gps.bearingAccuracyDeg = 1.0
gps.vNED = [0.0, 0.0, 0.0]
gps.latitude = float(self.lat)
gps.longitude = float(self.lon)
gps.unixTimestampMillis = t // 1_000_000
gps.altitude = float(self.alt)
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.01, 0.01, 0.01]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.01, 0.01, 0.01]
elif name == 'extrinsicsCalibration':
msg.extrinsicsCalibration.calStatus = 'calibrated'
msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0, 0.0]
elif name == 'accelerometer':
msg.accelerometer.sensor = 1
msg.accelerometer.type = 1
msg.accelerometer.timestamp = t
msg.accelerometer.init('acceleration').v = [0.0, 0.0, 9.81]
elif name == 'gyroscope':
msg.gyroscope.sensor = 5
msg.gyroscope.type = 16
msg.gyroscope.timestamp = t
msg.gyroscope.init('gyroUncalibrated').v = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.params.remove('LastGPSPositionIQLoc')
self.x = -2710700 + (random.random() * 1e5)
self.y = -4280600 + (random.random() * 1e5)
self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
msgs = []
for sec in range(1, 4):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
self.sm.update(0)
time.sleep(0.001)
deadline = time.monotonic() + 5.0
last_gps_raw = None
while time.monotonic() < deadline and last_gps_raw is None:
last_gps_raw = self.params.get('LastGPSPositionIQLoc')
self.sm.update(0)
time.sleep(0.05)
assert self.proc.poll() is None
location = self.sm['iqLiveLocation']
assert last_gps_raw is not None, {
'gpsHealthy': location.gpsHealthy,
'inputsHealthy': location.inputsHealthy,
'sensorsHealthy': location.sensorsHealthy,
'isolatedPath': self.params.get_param_path(),
}
lastGPS = json.loads(last_gps_raw)
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
shapes = {
"odometry_short": ([0.0] * 6, [0.0] * 6, [0.01] * 6, [0.01] * 6),
"odometry_nan": ([float("nan"), 0.0, 0.0], [0.0] * 3, [0.01] * 3, [0.01] * 3),
"odometry_nan_std": ([0.0] * 3, [0.0] * 3, [float("nan"), 0.01, 0.01], [0.01] * 3),
}
if case in shapes:
odo.rot, odo.trans, odo.rotStd, odo.transStd = shapes[case]
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