From 43ee82d228d85ad1863ec3703b31befba8da337e Mon Sep 17 00:00:00 2001 From: "IQ.Lvbs CI [bot]" Date: Wed, 2 Sep 2026 13:32:42 -0500 Subject: [PATCH] IQ.Pilot Release Commit @ 7550fa9 --- .../car/volkswagen/tests/test_volkswagen.py | 77 ++++++++ .../tinygrad/.iqpilot-revision | 2 +- .../tinygrad/tinygrad/nn/onnx.py | 4 + iqpilot/cereal/log.capnp | 1 + .../models/dmonitoring_model_tinygrad.pkl | Bin 24735376 -> 24735376 bytes .../models/prebuilt_check.json | 4 +- iqpilot/selfdrive/iqlocd/atlas_loc_core.cc | 83 +++++++-- iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py | 58 ++++++ iqpilot/selfdrive/iqmodeld/egpu_helpers.py | 19 +- iqpilot/selfdrive/iqmodeld/egpu_pipeline.py | 26 ++- iqpilot/selfdrive/iqmodeld/egpu_policy.py | 172 +++++++++++++++++- iqpilot/selfdrive/iqmodeld/iqegpumodeld.py | 150 ++++++++++++--- iqpilot/selfdrive/iqmodeld/messaging.py | 13 +- iqpilot/selfdrive/iqmodeld/modeld_selector.py | 6 + .../iqmodeld/tests/test_egpu_policy.py | 79 ++++++++ .../iqmodeld/tools/compile_egpu_model.py | 170 +++++++++++++++-- iqpilot/system/hardware/tici/agnos.json | 8 +- iqpilot/system/hardware/tici/agnos.json.sig | 2 +- .../system/hardware/tici/agnos_tici_15_1.json | 8 +- .../hardware/tici/agnos_tici_15_1.json.sig | 2 +- iqpilot/system/manager/process_config.py | 2 +- iqpilot/system/ui/iqwidgets/lib/canvas.py | 9 +- iqpilot/ui/mici/onroad/hud_renderer.py | 5 + iqpilot/ui/mici/onroad/speed_limit_sign.py | 75 ++++++++ 24 files changed, 882 insertions(+), 93 deletions(-) create mode 100644 iqpilot/ui/mici/onroad/speed_limit_sign.py diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py index bff552d01..12af3b91d 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py @@ -434,3 +434,80 @@ def test_meb_does_not_infer_mqb_cluster_from_address(platform): fingerprints[0][0x30B] = 8 params = CarInterface.get_params(platform, fingerprints, [], alpha_long=False, is_release=False, docs=False) assert not params.flags & VolkswagenFlags.KOMBI_PRESENT + + + +def _subscribed_addresses(car, updates=5): + nanos = 0 + for _ in range(updates): + nanos += int(DT_CTRL * 1e9) + car.update([(nanos, [])]) + return {bus: set(parser.addresses) for bus, parser in car.can_parsers.items()} + + +def _frames_for(car, packer, subscribed): + frames = [] + for bus, parser in car.can_parsers.items(): + for addr in sorted(subscribed[bus]): + frames.append(packer.make_can_msg(parser.dbc.addr_to_msg[addr].name, parser.bus, {})) + return frames + + +def _run_past_aliveness_timeout(car, build, seconds=15.0): + nanos = 0 + ret = None + for _ in range(int(seconds / DT_CTRL)): + nanos += int(DT_CTRL * 1e9) + ret, _ = car.update([(nanos, build())]) + return ret + + +def _sparse_q5(car_fw=(), bus1=()): + fingerprints = _mlb_fingerprint(0, MLB_ECAN_GATEWAY_NO_GEARBOX) + fingerprints[1] = {msg: 8 for msg in bus1} + fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA} + CP = CarInterface.get_params(CAR.AUDI_Q5_MK1, fingerprints, list(car_fw), alpha_long=False, is_release=False, docs=False) + CP_IQ = CarInterface.get_params_iq(CP, CAR.AUDI_Q5_MK1, fingerprints, list(car_fw), alpha_long=False, is_release_iq=False, docs=False) + return CarInterface(CP, CP_IQ) + + +@pytest.mark.parametrize("car_fw", ( + (), + (CarParams.CarFw.new_message(ecu=Ecu.transmission, address=0x7e1),), +), ids=("no_fw", "transmission_fw")) +def test_mlb_gateway_car_with_a_sparse_fingerprint_snapshot_reads_only_what_an_automatic_reads(car_fw): + automatic = _subscribed_addresses(_q5_mk1_car()) + assert all(MLB_MSG_GATEWAY_05 not in addrs for addrs in automatic.values()) + + q5 = _sparse_q5(car_fw) + packer = CANPacker("vw_mlb") + ret = _run_past_aliveness_timeout(q5, lambda: _frames_for(q5, packer, automatic)) + + assert ret.canValid + assert all(parser.can_valid for parser in q5.can_parsers.values()) + assert not any(parser.bus_timeout for parser in q5.can_parsers.values()) + assert {bus: set(parser.addresses) for bus, parser in q5.can_parsers.items()} == automatic + assert q5.CP.transmissionType == CarParams.TransmissionType.automatic + + +def test_mlb_manual_gateway_car_adds_only_the_reverse_switch_on_the_powertrain_bus(): + automatic = _subscribed_addresses(_q5_mk1_car()) + manual = _subscribed_addresses(_sparse_q5(bus1=(MLB_MSG_GATEWAY_05,))) + + assert manual[Bus.pt] == automatic[Bus.pt] + assert manual[Bus.cam] == automatic[Bus.cam] + assert manual[Bus.aux] == automatic[Bus.aux] | {MLB_MSG_GATEWAY_05} + + +@pytest.mark.parametrize("build", (_a4_mk4_car, _q5_mk1_car), ids=("a4_mk4", "q5_mk1")) +def test_mlb_carstate_subscriptions_settle_and_stay_alive(build): + car = build() + subscribed = _subscribed_addresses(car) + + packer = CANPacker("vw_mlb") + ret = _run_past_aliveness_timeout(car, lambda: _frames_for(car, packer, subscribed)) + + assert {bus: set(parser.addresses) for bus, parser in car.can_parsers.items()} == subscribed + assert ret.canValid + assert all(parser.can_valid for parser in car.can_parsers.values()) + assert not any(parser.bus_timeout for parser in car.can_parsers.values()) diff --git a/artifacts/package_sources/tinygrad/.iqpilot-revision b/artifacts/package_sources/tinygrad/.iqpilot-revision index 299cd06dc..aacd55ee6 100644 --- a/artifacts/package_sources/tinygrad/.iqpilot-revision +++ b/artifacts/package_sources/tinygrad/.iqpilot-revision @@ -1 +1 @@ -95bfc521351d3acba7338605686155a9ee8b0009 +b80bc5202db4ecae448c210ec60702bc63a134af diff --git a/artifacts/package_sources/tinygrad/tinygrad/nn/onnx.py b/artifacts/package_sources/tinygrad/tinygrad/nn/onnx.py index a8a19f077..693ace840 100644 --- a/artifacts/package_sources/tinygrad/tinygrad/nn/onnx.py +++ b/artifacts/package_sources/tinygrad/tinygrad/nn/onnx.py @@ -46,6 +46,7 @@ class Domain(enum.Enum): MICROSOFT_NCHWC = "com.microsoft.nchwc" MICROSOFT_EXPERIMENTAL = "com.microsoft.experimental" PYTORCH_ATEN = "org.pytorch.aten" + TINYGRAD = "org.tinygrad" @classmethod def from_onnx(cls, domain: str | None) -> "Domain": return cls.ONNX if domain is None or domain == "" else cls(domain) @@ -555,6 +556,9 @@ def get_onnx_ops() -> dict[str, types.FunctionType|dict[OpSetId, types.FunctionT cond = _resolve_const(_to_python_const(condition)) return tuple(t if cond else e for t,e in zip(then_out.values(), else_out.values())) + def contiguous_1(x:Tensor): return x.contiguous() + Contiguous = {OpSetId(Domain.TINYGRAD, 1):contiguous_1} + def Identity(x:Tensor): return x def Constant(sparse_value:Tensor|None=None, value:Tensor|None=None, value_float:float|None=None, value_floats:list[float]|None=None, value_int:int|None=None, value_ints:list[int]|None=None, value_string:str|None=None, value_strings:list[str]|None=None): diff --git a/iqpilot/cereal/log.capnp b/iqpilot/cereal/log.capnp index fb080df1c..5395faed1 100644 --- a/iqpilot/cereal/log.capnp +++ b/iqpilot/cereal/log.capnp @@ -1086,6 +1086,7 @@ struct DrivingModelData { frameIdExtra @1 :UInt32; frameDropPerc @6 :Float32; modelExecutionTime @7 :Float32; + big @8 :Bool; action @2 :ModelDataV2.Action; diff --git a/iqpilot/selfdrive/dmonitoringmodeld/models/dmonitoring_model_tinygrad.pkl b/iqpilot/selfdrive/dmonitoringmodeld/models/dmonitoring_model_tinygrad.pkl index 263c1b091fffd3b17777cc3bd28b7fcc7fa2dd21..e927fd7c162caf5ef0bd89d440b0f2654fb0dab6 100644 GIT binary patch delta 1871 zcmY+?Xc(2Sp)?IcSR6UgcgnRUsG8Og*K_x zShm>J#a5nf*q>hKDZyMVJmEd2fzUj!gkmJJK-UC7#;y9 z?1D$(G1v`{!xQi%JOz8;X?O;nh3DXTc)=BSq^6+MkyTlnZ?kGI!b|Y7%c|8TPV)(} zZC#b2I(5>Fny)h?rBHQu43FL)X{)KMg}v|!yb3OO4PJ*g;7xc7>YyHUcpKhq%P;R5^w7vX2P1i!$q@EiONf54yc z7hHC!ux877izhbEaD5*wnzDri{0&!JApuvTvfYfD(P2s0mg(?Kz2;@BE4m8*xax{p znos(fGy|Uh{|F44$r+h8O2xbIY~e9Whuap-((Z&>XWP;guI#`V7&oOig&WnSqvm$A z8n4gVI>sJhbXQ1crAHV&)cWOSQ`-`PHDl{LlI<-89ttrro{Sgc&G;}~7&GI`SQsng zr}UNre^pX!?(k{t);`kNbS_X??TyF%71EUnU;>ric)Xk1e9DnibT!m*WzWHJ&TCkQ z)!v|YS4a=0C)11R&Gb=vgC3+}4_Q>(cbIS_|7~T0bN6;LW+ znNVgBGng5|gfZCWlIo$%FeZW-&WvCpnUTyWW;8Q~8Ow}g#xqgO1SXn^VJ0$@m{?{q zGliMTOk<`qam)-Ro=IR5nVF2nBr&s?WG01~&7?AOm^3Dx$zU>>xl9(5&CFxwGdWBy zvw&I1bra)vJ|pCx42zW14K2h(d)FF>9D&W-U{q zbctA}nj6g+(Kg1%nd&T5x%RWB^$ICvHYok9sZ7;hR3>N4#xj*%f4NMB=5&0Sp><%A VUXfm|#%Rt^TnhByUFE8H-@hER{L%mb delta 1885 zcmYk+d013c9L8}E3^Ibk3<&NliW?{hxZ}RCT?7$OL=bnB5ky3xo$^;xSqz3Wsnuw< zZ<=Tm72fuRN=hZeB+{}-&81b#toNmQhCe>v=b7g&=iGDdo$A&z-qlbkO5DU<4B{bO z#Z!#ZO}xZgx=Ro75nnNhS^UIbdP;xT zFc~i45+NgGq(n-TjFM=HkysfmV`Qw1lkqY^CQ6)4lF2efrph##E;D4N%#zs>FA0(; zNs=s@q{tjel{A?v=`v3;4rHD`=BG4YZIX3cVV3IBMKfv!&CL;=5vvld@y(f#1@mD6 zWWz$pfkluDd9WDrVF@gSWw0Dpz)DyJ1+W^{Kp|LQEfhg9tb-C*52dgH%3vdGf^w*U z8{kH`32ug4;8wT|D&cmx12%)eop2Xe;cmDG?u9LIAKVXBV1sJd3J*XHY=c^O5bW>} zY=<4N6CQ>~;8AeEF4zr^!5(-Vo`5IeDR>&5foI`4cphGW7oG9VdkZ(3d^Pp|Mi?}u z1$Ashf_-mQotM?D?Sq$KztgPMCC#+&Pu`brt*@$s18@*t1}D4%_3$dZ2CqW{90DEQ zfH&bScpKhNx z)iIgOjxlYK1`maFReGesQ5Y1jntIgK>5*LPnCRHE-3Q~A)&wgggb8H^GJ}{fW-v2^8OjV} zhBM&|*0{801T&I}WTKc+Of(b2#4@9qG0a$I95bGoz)WP~m`TiJW(qTvnZ`_KW-v3E zS{%8W*(ElWHMRId}aZY%`9Ycm_MSA{A${12M-2Y~

::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_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(); } diff --git a/iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py b/iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py index 545b5d5c1..b84b26da8 100644 --- a/iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py +++ b/iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py @@ -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 diff --git a/iqpilot/selfdrive/iqmodeld/egpu_helpers.py b/iqpilot/selfdrive/iqmodeld/egpu_helpers.py index 1317d78be..652fe37de 100644 --- a/iqpilot/selfdrive/iqmodeld/egpu_helpers.py +++ b/iqpilot/selfdrive/iqmodeld/egpu_helpers.py @@ -71,6 +71,11 @@ def egpu_oob_pkl_path(meta: dict) -> str: return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy_oob.pkl") +def egpu_model_oob_pkl_path(meta: dict) -> str: + from iqpilot.system.hardware.hw import Paths + return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_model_oob.pkl") + + def onnx_cache_path(meta: dict) -> str: from iqpilot.system.hardware.hw import Paths return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx") @@ -156,13 +161,21 @@ def download_onnx(meta: dict, progress_cb=None) -> str: return path -def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False) -> str | None: - field = "egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact" +ARTIFACT_PATHS = { + "egpu_model_oob_artifact": egpu_model_oob_pkl_path, + "egpu_oob_artifact": egpu_oob_pkl_path, + "egpu_policy_artifact": egpu_policy_pkl_path, + "egpu_artifact": egpu_pkl_path, +} + + +def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False, field: str | None = None) -> str | None: + field = field or ("egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact") art = meta.get(field) if not art or not (art.get("objects") or art.get("hf_path")): return None from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file, download_lfs_bundle - dest = egpu_oob_pkl_path(meta) if oob else egpu_policy_pkl_path(meta) if policy else egpu_pkl_path(meta) + dest = ARTIFACT_PATHS[field](meta) if art.get("hf_path"): try: return download_hf_file(art["hf_path"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb) diff --git a/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py b/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py index 428b095a2..9da2a36c9 100644 --- a/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py +++ b/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py @@ -5,7 +5,7 @@ from __future__ import annotations import numpy as np -from iqpilot.selfdrive.iqmodeld.egpu_policy import PolicyRunner +from iqpilot.selfdrive.iqmodeld.egpu_policy import ModelRunner, PolicyRunner from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC, TemporalInputState, spec_from_meta @@ -13,6 +13,10 @@ class EgpuPipelineError(RuntimeError): pass +class EgpuOutputInvalid(EgpuPipelineError): + pass + + class EgpuPipeline: def __init__(self, meta: dict, infer_fn): @@ -31,14 +35,26 @@ class EgpuPipeline: else: inputs = self.state.push_and_materialize(warped, desire_vec, traffic_convention, action_t) out = np.asarray(self.infer_fn(inputs), dtype=np.float32).reshape(-1) - if out.shape[0] != self.output_len: - raise EgpuPipelineError(f"eGPU output length {out.shape[0]} != {self.output_len}") - if not np.isfinite(out).all(): - raise EgpuPipelineError("eGPU output contains non-finite values") + self._check(out) if not isinstance(self.infer_fn, PolicyRunner): self.state.note_hidden_state(out, self.hidden_slice) return out + def run_frames(self, main_frame, extra_frame, tfm: np.ndarray, big_tfm: np.ndarray, desire_vec: np.ndarray, + traffic_convention: np.ndarray, action_t: np.ndarray) -> np.ndarray: + if not isinstance(self.infer_fn, ModelRunner): + raise EgpuPipelineError("run_frames needs a format-3 (warp-on-dock) artifact") + out = np.asarray(self.infer_fn.run(main_frame, extra_frame, tfm, big_tfm, desire_vec, traffic_convention, action_t), + dtype=np.float32).reshape(-1) + self._check(out) + return out + + def _check(self, out: np.ndarray) -> None: + if out.shape[0] != self.output_len: + raise EgpuPipelineError(f"eGPU output length {out.shape[0]} != {self.output_len}") + if not np.isfinite(out).all(): + raise EgpuOutputInvalid("eGPU output contains non-finite values") + def make_big_channel_payload(frame_id: int, live_calib_seen: bool, execution_time: float, egpu_exec_ms: float, msgs: dict[str, bytes]) -> dict: diff --git a/iqpilot/selfdrive/iqmodeld/egpu_policy.py b/iqpilot/selfdrive/iqmodeld/egpu_policy.py index 2d0b12d7a..1ec4365a6 100644 --- a/iqpilot/selfdrive/iqmodeld/egpu_policy.py +++ b/iqpilot/selfdrive/iqmodeld/egpu_policy.py @@ -14,9 +14,11 @@ import tempfile import numpy as np POLICY_FORMAT = 2 +MODEL_FORMAT = 3 OOB_MAGIC = b"IQEGPUOOB1" QUEUE_NAMES = ("img_q", "big_img_q", "feat_q", "desire_q") PACKED_ORDER = ("desire", "traffic_convention", "action_t", "prev_feat") +MODELD_INPUTS = (*QUEUE_NAMES, "packed_npy_inputs") def packed_layout(input_spec: dict) -> tuple[dict[str, tuple[int, ...]], list[int]]: @@ -56,7 +58,8 @@ class PackedInputs: from tinygrad.tensor import Tensor self.shapes, self.sizes = packed_layout(input_spec) self.array = np.zeros(sum(self.sizes), dtype=np.float32) - self.views = dict(zip(self.shapes, [v.reshape(s) for s, v in zip(self.shapes.values(), np.split(self.array, np.cumsum(self.sizes[:-1])))], strict=True)) + parts = np.split(self.array, np.cumsum(self.sizes[:-1])) + self.views = {name: part.reshape(shape) for (name, shape), part in zip(self.shapes.items(), parts, strict=True)} self.tensor = Tensor(self.array, device="NPY").realize() @@ -124,6 +127,173 @@ class PolicyRunner: return flat +def nv12_copy_size(stride: int, y_height: int, uv_height: int) -> int: + return stride * (y_height + uv_height) + + +def frame_layout(input_spec: dict) -> tuple[dict[str, tuple[int, ...]], list[int], int]: + policy_shapes, _ = packed_layout(input_spec) + shapes = {"tfm": (3, 3), "big_tfm": (3, 3)} | policy_shapes + sizes = [math.prod(s) for s in shapes.values()] + return shapes, sizes, sum(sizes) * np.dtype(np.float32).itemsize + + +def model_size(input_spec: dict) -> tuple[int, int]: + img = input_spec["img"][0] + return img[3] * 2, img[2] * 2 + + +def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None): + from tinygrad.tensor import Tensor + w_dst, h_dst = dst_shape + h_src, w_src = src_shape + + x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1) + y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1) + + src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2] + src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2] + src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2] + + src_x = src_x / src_w + src_y = src_y / src_w + + x_round = Tensor.round(src_x) + y_round = Tensor.round(src_y) + x_nn_clipped = x_round.clip(0, w_src - 1).cast("int") + y_nn_clipped = y_round.clip(0, h_src - 1).cast("int") + idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped + sampled = src_flat[idx] + + if border_fill_val is None: + return sampled + + in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) & + (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype) + return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds) + + +def frames_to_tensor(frames): + from tinygrad.tensor import Tensor + H = (frames.shape[0] * 2) // 3 + W = frames.shape[1] + in_img1 = Tensor.cat(frames[0:H:2, 0::2], + frames[1:H:2, 0::2], + frames[0:H:2, 1::2], + frames[1:H:2, 1::2], + frames[H:H + H // 4].reshape((H // 2, W // 2)), + frames[H + H // 4:H + H // 2].reshape((H // 2, W // 2)), dim=0).reshape((6, H // 2, W // 2)) + return in_img1 + + +def make_frame_prepare(nv12: tuple[int, int, int, int, int], model_w: int, model_h: int, device: str): + from tinygrad.helpers import Context + from tinygrad.tensor import Tensor + cam_w, cam_h, stride, y_height, uv_height = nv12 + uv_offset = stride * y_height + stride_pad = stride - cam_w + + def frame_prepare_tinygrad(input_frame, M_inv): + M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=device) + uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride) + with Context(SPLIT_REDUCEOP=0): + y = warp_perspective_tinygrad(input_frame[:cam_h * stride], + M_inv, (model_w, model_h), + (cam_h, cam_w), stride_pad).realize() + u = warp_perspective_tinygrad(uv[:cam_h // 2, :cam_w:2].flatten(), + M_inv_uv, (model_w // 2, model_h // 2), + (cam_h // 2, cam_w // 2), 0).realize() + v = warp_perspective_tinygrad(uv[:cam_h // 2, 1:cam_w:2].flatten(), + M_inv_uv, (model_w // 2, model_h // 2), + (cam_h // 2, cam_w // 2), 0).realize() + yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w)) + return frames_to_tensor(yuv) + return frame_prepare_tinygrad + + +def make_warp(nv12: tuple[int, int, int, int, int], model_w: int, model_h: int, device: str): + from tinygrad.tensor import Tensor + frame_prepare = make_frame_prepare(nv12, model_w, model_h, device) + + def warp(tfm, big_tfm, frame, big_frame): + tfm = tfm.to(device) + big_tfm = big_tfm.to(device) + frame = frame.to(device) + big_frame = big_frame.to(device) + Tensor.realize(tfm, big_tfm, frame, big_frame) + + warped_frame = frame_prepare(frame, tfm).unsqueeze(0) + warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0) + return Tensor.cat(warped_frame, warped_big_frame) + + return warp + + +def make_run_model(warp, run_policy, input_spec: dict, frame_copy_size: int, device: str): + from tinygrad.tensor import Tensor + _, policy_sizes = packed_layout(input_spec) + _, _, packed_npy_size = frame_layout(input_spec) + + def run_model(img_q, big_img_q, feat_q, desire_q, packed_npy_inputs): + packed_input = packed_npy_inputs.to(device) + Tensor.realize(packed_input) + packed_npy_inputs = packed_input[:packed_npy_size].bitcast("float32") + frame = packed_input[packed_npy_size:packed_npy_size + frame_copy_size] + big_frame = packed_input[packed_npy_size + frame_copy_size:] + tfm, big_tfm, policy_inputs = packed_npy_inputs.split([9, 9, sum(policy_sizes)]) + warped = warp(tfm.reshape(3, 3), big_tfm.reshape(3, 3), frame, big_frame) + return run_policy(warped, img_q, big_img_q, feat_q, desire_q, policy_inputs) + + return run_model + + +class PackedFrames: + def __init__(self, input_spec: dict, frame_copy_size: int): + from tinygrad.tensor import Tensor + self.shapes, self.sizes, npy_bytes = frame_layout(input_spec) + self.frame_copy_size = frame_copy_size + self.array = np.zeros(npy_bytes + 2 * frame_copy_size, dtype=np.uint8) + npy = self.array[:npy_bytes].view(np.float32) + self.views = dict(zip(self.shapes, [v.reshape(s) for s, v in zip(self.shapes.values(), np.split(npy, np.cumsum(self.sizes[:-1])), strict=True)], + strict=True)) + frames = self.array[npy_bytes:] + self.frames = {"img": frames[:frame_copy_size], "big_img": frames[frame_copy_size:]} + self.tensor = Tensor(self.array, device="NPY").realize() + + +def make_model_queues(input_spec: dict, frame_skip: int, device: str, frame_copy_size: int) -> tuple[dict, PackedFrames]: + packed = PackedFrames(input_spec, frame_copy_size) + return {**make_queues(input_spec, frame_skip, device), "packed_npy_inputs": packed.tensor}, packed + + +class ModelRunner: + def __init__(self, jit, input_spec: dict, frame_skip: int, hidden_slice: slice, device: str, frame_copy_size: int): + self._jit = jit + self._queues, self._packed = make_model_queues(input_spec, frame_skip, device, frame_copy_size) + self._hidden = hidden_slice + self._prev_desire = np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32) + self.frame_copy_size = frame_copy_size + + def run(self, main_frame, extra_frame, tfm: np.ndarray, big_tfm: np.ndarray, desire_pulse: np.ndarray, + traffic_convention: np.ndarray, action_t: np.ndarray) -> np.ndarray: + n = self.frame_copy_size + v = self._packed.views + f = self._packed.frames + np.copyto(f["img"], np.frombuffer(main_frame, dtype=np.uint8, count=n)) + np.copyto(f["big_img"], np.frombuffer(extra_frame, dtype=np.uint8, count=n)) + v["tfm"][:, :] = tfm + v["big_tfm"][:, :] = big_tfm + cur = desire_pulse.astype(np.float32, copy=False) + v["desire"][:] = np.where(cur - self._prev_desire > 0.99, cur, 0) + self._prev_desire[:] = cur + v["traffic_convention"][:] = np.asarray(traffic_convention, dtype=np.float32).reshape(v["traffic_convention"].shape) + v["action_t"][:] = np.asarray(action_t, dtype=np.float32).reshape(v["action_t"].shape) + out, = self._jit(**self._queues) + flat = out.numpy().reshape(-1) + v["prev_feat"][:] = flat[self._hidden].reshape(v["prev_feat"].shape) + return flat + + def dump_oob(obj, f) -> None: # Out-of-band pickle buffers keep the host peak at one tensor while the weights stream to the # dock; a plain pickle keeps every weight referenced in the memo until load() returns (~1.7GB). diff --git a/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py b/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py index bc5757c49..4e21f0d5d 100644 --- a/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py +++ b/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py @@ -39,16 +39,17 @@ from iqpilot.selfdrive.iqmodeld.driving_action import ( DESIRE_LEN, LAT_SMOOTH_SECONDS, LONG_SMOOTH_SECONDS, get_action_from_model, ) from iqpilot.selfdrive.iqmodeld.egpu_helpers import ( - download_onnx, download_precompiled, egpu_oob_pkl_path, egpu_pkl_path, egpu_policy_pkl_path, egpu_present_consented, egpu_selected, local_onnx, + download_onnx, download_precompiled, egpu_model_oob_pkl_path, egpu_oob_pkl_path, egpu_pkl_path, egpu_policy_pkl_path, egpu_present_consented, + egpu_selected, local_onnx, patch_tinygrad_fetch_fw, quarantine_artifact, resolve_backend, usbgpu_present, ) from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model -from iqpilot.selfdrive.iqmodeld.egpu_pipeline import EgpuPipeline, EgpuPipelineError, make_big_channel_payload +from iqpilot.selfdrive.iqmodeld.egpu_pipeline import EgpuOutputInvalid, EgpuPipeline, EgpuPipelineError, make_big_channel_payload from iqpilot.selfdrive.iqmodeld.egpu_telemetry import EgpuDockTelemetry from iqpilot.selfdrive.iqmodeld.messaging import DrivePacketMemory, populate_drive_messages, populate_odometry_message from iqpilot.selfdrive.iqmodeld.metadata import Meta20hz from iqpilot.selfdrive.iqmodeld.model_channel import BIG_CHANNEL, ModelChannel -from iqpilot.selfdrive.iqmodeld.egpu_policy import POLICY_FORMAT, PolicyRunner, load_bundle +from iqpilot.selfdrive.iqmodeld.egpu_policy import MODEL_FORMAT, POLICY_FORMAT, ModelRunner, PolicyRunner, load_bundle from iqpilot.selfdrive.iqmodeld.model_warp import FrameWarp from iqpilot.selfdrive.iqmodeld.parser import PhaseParser @@ -62,6 +63,11 @@ MIN_LOAD_AVAIL_MB = 350 MEMORY_WAIT_S = 90.0 SETUP_RETRY_BASE_S = 3.0 SETUP_RETRY_MAX_S = 30.0 +MAX_INVALID_STREAK = 20 +DOCK_MIN_SUPPLY_MV = 5000 +DOCK_POWER_STABLE_POLLS = 4 +DOCK_POWER_POLL_S = 0.1 +DOCK_POWER_TIMEOUT_S = 30.0 def park(reason: str) -> None: @@ -86,15 +92,53 @@ def _wait_for_egpu(params: Params) -> None: while time.monotonic() < deadline: try: if link_up(): - return + break except Exception: return time.sleep(0.5) + _wait_for_dock_power(params) -def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str) -> None: +def dock_power_ready(reading) -> bool: + voltage, _current, fault = reading + return int(voltage) >= DOCK_MIN_SUPPLY_MV and not fault + + +def _wait_for_dock_power(params: Params) -> None: + telemetry = EgpuDockTelemetry(None, big=False) + deadline = time.monotonic() + DOCK_POWER_TIMEOUT_S + stable = 0 + warned = False + try: + while time.monotonic() < deadline: + try: + reading = telemetry._read_ina() + except Exception: + return + if reading is None: + return + stable = stable + 1 if dock_power_ready(reading) else 0 + if stable >= DOCK_POWER_STABLE_POLLS: + return + if stable == 0 and not warned: + warned = True + cloudlog.warning(f"iqegpumodeld dock supply not ready {reading}; waiting for a stable 5V rail") + params.put("UsbGpuLastError", f"dock supply not ready (voltage={reading[0]}mV fault={reading[2]}); waiting") + time.sleep(DOCK_POWER_POLL_S) + cloudlog.warning("iqegpumodeld dock supply never stabilised; continuing") + finally: + handle = getattr(telemetry, "_asm_usb", None) + if handle is not None: + try: + handle.close() + except Exception: + pass + + +def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str, cam_size: tuple[int, int]) -> None: cmd = [sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model", "--model", meta["key"], "--onnx", onnx_path, "--output", pkl_path, + "--format", str(MODEL_FORMAT), "--camera-resolutions", f"{cam_size[0]}x{cam_size[1]}", "--progress-param", "UsbGpuSetupProgress", "--progress-base", "0.5", "--progress-span", "0.48"] compile_env = {**os.environ, "DEV": "USB+AMD:LLVM", "FLOAT16": "1", "JIT_BATCH_SIZE": "0", "GMMU": "0", "TC_OPT": "2"} @@ -106,10 +150,36 @@ def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str) -> None: _precompiled_tried = False +_model_precompiled_tried = False -def _ensure_artifact(params: Params, meta: dict) -> str: - global _precompiled_tried +def _ensure_artifact(params: Params, meta: dict, cam_size: tuple[int, int]) -> str: + global _precompiled_tried, _model_precompiled_tried + model_path = egpu_model_oob_pkl_path(meta) + if os.path.isfile(model_path): + return model_path + if meta.get("egpu_model_oob_artifact") and not _model_precompiled_tried: + _model_precompiled_tried = True + params.put_bool("UsbGpuCompiled", False) + params.put_bool("UsbGpuReady", False) + params.put("UsbGpuSetupProgress", "0.0") + model_last = [-1.0] + + def _model_prog(p: float) -> None: + if p - model_last[0] >= 0.02 or p >= 1.0: + model_last[0] = p + params.put("UsbGpuSetupProgress", f"{p:.3f}") + + try: + size_mb = int(meta["egpu_model_oob_artifact"].get("size", 0)) / 1e6 + cloudlog.warning(f"iqegpumodeld downloading precompiled {meta['key']} (warp-on-dock, {size_mb:.0f}MB)") + precompiled = download_precompiled(meta, progress_cb=_model_prog, field="egpu_model_oob_artifact") + if precompiled is not None: + cloudlog.warning(f"iqegpumodeld precompiled ready -> {precompiled}") + return precompiled + except Exception as e: + cloudlog.warning(f"iqegpumodeld warp-on-dock artifact unavailable ({e}); falling back") + oob_path = egpu_oob_pkl_path(meta) if os.path.isfile(oob_path): return oob_path @@ -179,10 +249,10 @@ def _ensure_artifact(params: Params, meta: dict) -> str: onnx_path = download_onnx(meta, progress_cb=_prog) - cloudlog.warning(f"iqegpumodeld compiling {meta['key']} for USB-AMD (one-time, can take minutes)") - _compile_in_subprocess(meta, onnx_path, policy_path) - cloudlog.warning(f"iqegpumodeld compiled -> {policy_path}") - return policy_path + cloudlog.warning(f"iqegpumodeld compiling {meta['key']} for USB-AMD ({cam_size[0]}x{cam_size[1]}, one-time, can take minutes)") + _compile_in_subprocess(meta, onnx_path, model_path, cam_size) + cloudlog.warning(f"iqegpumodeld compiled -> {model_path}") + return model_path def _mem_available_mb() -> int: @@ -207,7 +277,7 @@ def _wait_for_memory(need_mb: int) -> None: raise RuntimeError(f"insufficient memory to load the dock model: {avail}MB available, need {need_mb}MB") -def _load_infer_fn(pkl_path: str, meta: dict): +def _load_infer_fn(pkl_path: str, meta: dict, cam_size: tuple[int, int]): patch_tinygrad_fetch_fw() from tinygrad.tensor import Tensor @@ -219,6 +289,14 @@ def _load_infer_fn(pkl_path: str, meta: dict): if int(bundle.get("output_len", -1)) != int(meta["output_len"]): quarantine_artifact(pkl_path, "pkl output_len mismatch") raise RuntimeError(f"artifact output_len {bundle.get('output_len')} != {meta['output_len']}") + if bundle.get("format") == MODEL_FORMAT: + jits = bundle["run_model"] + if cam_size not in jits: + have = ", ".join(f"{w}x{h}" for w, h in sorted(jits)) + raise RuntimeError(f"artifact has no warp for the {cam_size[0]}x{cam_size[1]} camera (bundled: {have})") + runner = ModelRunner(jits[cam_size], bundle["input_spec"], int(bundle["frame_skip"]), meta["output_slices"]["hidden_state"], + bundle.get("input_device", "AMD"), int(bundle["frame_copy_size"][cam_size])) + return runner, bundle["input_spec"] if bundle.get("format") == POLICY_FORMAT: runner = PolicyRunner(bundle["run_policy"], bundle["input_spec"], int(bundle["frame_skip"]), meta["output_slices"]["hidden_state"], bundle.get("input_device", "AMD")) @@ -239,7 +317,12 @@ def _load_infer_fn(pkl_path: str, meta: dict): def _warmup(infer_fn, input_spec: dict, output_len: int) -> float: zeros = {name: np.zeros(shape, dtype=dtype) for name, (shape, dtype) in input_spec.items()} t0 = time.perf_counter() - if isinstance(infer_fn, PolicyRunner): + if isinstance(infer_fn, ModelRunner): + n = infer_fn.frame_copy_size + eye = np.eye(3, dtype=np.float32) + out = infer_fn.run(np.zeros(n, dtype=np.uint8), np.zeros(n, dtype=np.uint8), eye, eye, + np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32), np.zeros(2, dtype=np.float32), np.zeros(2, dtype=np.float32)) + elif isinstance(infer_fn, PolicyRunner): img = input_spec["img"][0] out = infer_fn.run(np.zeros((2, 6, img[2], img[3]), dtype=np.uint8), np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32), np.zeros(2, dtype=np.float32), np.zeros(2, dtype=np.float32)) @@ -284,9 +367,10 @@ def main(demo: bool = False) -> None: if meta.get("split"): params.put_bool("UsbGpuLoading", False) park(f"model {meta['key']} needs the Mac backend; the eGPU runs fused models only") - warp = FrameWarp(cameras._primary.width, cameras._primary.height, meta["frame_skip"]) - pkl_path = _ensure_artifact(params, meta) - infer_fn, input_spec = _load_infer_fn(pkl_path, meta) + cam_size = (int(cameras._primary.width), int(cameras._primary.height)) + pkl_path = _ensure_artifact(params, meta, cam_size) + infer_fn, input_spec = _load_infer_fn(pkl_path, meta, cam_size) + warp = None if isinstance(infer_fn, ModelRunner) else FrameWarp(cam_size[0], cam_size[1], meta["frame_skip"]) warm_s = _warmup(infer_fn, input_spec, meta["output_len"]) break except Exception as e: @@ -312,7 +396,7 @@ def main(demo: bool = False) -> None: params.put_bool("UsbGpuReady", True) params.put("UsbGpuSetupProgress", "1.0") cloudlog.warning(f"iqegpumodeld model: {meta['key']} ({meta['model_name']})") - cloudlog.warning(f"iqegpumodeld model up (warmup {warm_s * 1e3:.0f}ms)") + cloudlog.warning(f"iqegpumodeld model up (warmup {warm_s * 1e3:.0f}ms, {'warp on dock' if warp is None else 'warp on device'})") pipeline = EgpuPipeline(meta, infer_fn) telemetry_pm = messaging.PubMaster(["egpuDockState"]) @@ -339,6 +423,7 @@ def main(demo: bool = False) -> None: stats: dict[str, list[float]] = {k: [] for k in ("pull", "warp", "infer", "publish", "loop")} iter_count = 0 skip_count = 0 + invalid_streak = 0 last_pulled_fid = -1 last_frame_mono = time.monotonic() t_loop = time.perf_counter() @@ -394,20 +479,34 @@ def main(demo: bool = False) -> None: action_t = np.array([lat_action_t, long_action_t], dtype=np.float32) started_at = time.perf_counter() + t_warp = started_at try: - warped = warp.run(main_buf, extra_buf, main_tfm, extra_tfm) - except Exception as e: - park(f"warp run failed: {e}") - t_warp = time.perf_counter() - stats["warp"].append(t_warp - started_at) - - try: - output = pipeline.run(warped, desire_vec, traffic, action_t) + if warp is None: + output = pipeline.run_frames(main_buf.data, extra_buf.data, main_tfm, extra_tfm, desire_vec, traffic, action_t) + else: + try: + warped = warp.run(main_buf, extra_buf, main_tfm, extra_tfm) + except Exception as e: + park(f"warp run failed: {e}") + t_warp = time.perf_counter() + output = pipeline.run(warped, desire_vec, traffic, action_t) + except EgpuOutputInvalid as e: + invalid_streak += 1 + if invalid_streak == 1 or invalid_streak % MAX_INVALID_STREAK == 0: + cloudlog.warning(f"iqegpumodeld dropping frame {main_stamp.frame_id}: {e} (streak {invalid_streak})") + if invalid_streak >= MAX_INVALID_STREAK: + params.put("UsbGpuLastError", f"{e} for {invalid_streak} consecutive frames"[:512]) + cloudlog.error(f"iqegpumodeld output invalid for {invalid_streak} frames; exiting for a clean restart") + sys.exit(1) + frame_meter.commit(main_stamp.frame_id) + continue except EgpuPipelineError as e: park(str(e)) except Exception as e: park(f"eGPU inference failed: {e}") + invalid_streak = 0 t_infer = time.perf_counter() + stats["warp"].append(t_warp - started_at) stats["infer"].append(t_infer - t_warp) execution_time = time.perf_counter() - started_at @@ -431,6 +530,7 @@ def main(demo: bool = False) -> None: ) model_msg.modelV2.big = True + driving_msg.drivingModelData.big = True desire_state = model_msg.modelV2.meta.desireState lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight] diff --git a/iqpilot/selfdrive/iqmodeld/messaging.py b/iqpilot/selfdrive/iqmodeld/messaging.py index ca43b2702..7f3ff5db6 100644 --- a/iqpilot/selfdrive/iqmodeld/messaging.py +++ b/iqpilot/selfdrive/iqmodeld/messaging.py @@ -248,16 +248,19 @@ def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_m def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray], vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: - msg.valid = live_calib_seen & (vipc_dropped_frames < 1) + pose = outputs["pose"][0, :6] + pose_stds = outputs["pose_stds"][0, :6] + pose_finite = bool(np.isfinite(pose).all() and np.isfinite(pose_stds).all()) + msg.valid = live_calib_seen & (vipc_dropped_frames < 1) & pose_finite odo = msg.cameraOdometry odo.frameId = vipc_frame_id odo.timestampEof = timestamp_eof - odo.trans = outputs["pose"][0, :3].tolist() - odo.rot = outputs["pose"][0, 3:].tolist() + odo.trans = pose[:3].tolist() + odo.rot = pose[3:6].tolist() odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist() odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist() - odo.transStd = outputs["pose_stds"][0, :3].tolist() - odo.rotStd = outputs["pose_stds"][0, 3:].tolist() + odo.transStd = pose_stds[:3].tolist() + odo.rotStd = pose_stds[3:6].tolist() odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist() odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist() diff --git a/iqpilot/selfdrive/iqmodeld/modeld_selector.py b/iqpilot/selfdrive/iqmodeld/modeld_selector.py index b05d2658e..a6a9151ae 100644 --- a/iqpilot/selfdrive/iqmodeld/modeld_selector.py +++ b/iqpilot/selfdrive/iqmodeld/modeld_selector.py @@ -159,23 +159,29 @@ class BigLatch: return False, False +BIG_SOURCES = frozenset({"egpu_big", "mac_big"}) + + def _patch_and_send(pm: PubMaster, payload: dict, frame_drop_perc: float, selector_dropped: int, target: int, source_lag: int, mismatch: bool | None = None) -> None: msgs = payload["msgs"] if mismatch is None: mismatch = source_lag > 0 + big = payload.get("source") in BIG_SOURCES model_msg = log_from_bytes(msgs["modelV2"]).as_builder() if mismatch: model_msg.modelV2.frameId = target model_msg.modelV2.frameAge = max(model_msg.modelV2.frameAge, source_lag) model_msg.modelV2.frameDropPerc = frame_drop_perc + model_msg.modelV2.big = big pm.send("modelV2", model_msg) driving_msg = log_from_bytes(msgs["drivingModelData"]).as_builder() if mismatch: driving_msg.drivingModelData.frameId = target driving_msg.drivingModelData.frameDropPerc = frame_drop_perc + driving_msg.drivingModelData.big = big pm.send("drivingModelData", driving_msg) pose_msg = log_from_bytes(msgs["cameraOdometry"]).as_builder() diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_egpu_policy.py b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_policy.py index 670d9e3c6..42756cda0 100644 --- a/iqpilot/selfdrive/iqmodeld/tests/test_egpu_policy.py +++ b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_policy.py @@ -80,3 +80,82 @@ def test_layouts(): assert sum(sizes) == 8 + 2 + 2 + 512 q = queue_shapes(SPEC, FS) assert q["img_q"][0] == (5, 6, 8, 16) and q["feat_q"][0] == (96, 1, 512) and q["desire_q"][0] == (100, 1, 8) + + +CAM = (64, 48) + + +def _nv12(cam_w, cam_h): + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h) + return (cam_w, cam_h, stride, y_height, uv_height) + + +def _numpy_warp_plane(src, m, w_dst, h_dst): + h_src, w_src = src.shape + x = np.tile(np.arange(w_dst, dtype=np.float32), h_dst) + y = np.repeat(np.arange(h_dst, dtype=np.float32), w_dst) + sx = (m[0, 0] * x + m[0, 1] * y + m[0, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2]) + sy = (m[1, 0] * x + m[1, 1] * y + m[1, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2]) + xi = np.clip(np.round(sx), 0, w_src - 1).astype(np.int64) + yi = np.clip(np.round(sy), 0, h_src - 1).astype(np.int64) + return src[yi, xi].reshape(h_dst, w_dst) + + +def _numpy_frame_prepare(frame, m, nv12, model_w, model_h): + cam_w, cam_h, stride, y_height, uv_height = nv12 + m = m.astype(np.float32) + y_src = frame[:cam_h * stride].reshape(cam_h, stride) + uv = frame[stride * y_height:stride * y_height + uv_height * stride].reshape(uv_height, stride) + m_uv = m * np.array([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], dtype=np.float32) + y = _numpy_warp_plane(y_src, m, model_w, model_h) + u = _numpy_warp_plane(uv[:cam_h // 2, :cam_w:2], m_uv, model_w // 2, model_h // 2) + v = _numpy_warp_plane(uv[:cam_h // 2, 1:cam_w:2], m_uv, model_w // 2, model_h // 2) + f = np.concatenate([y.ravel(), u.ravel(), v.ravel()]).reshape(model_h * 3 // 2, model_w) + H, W = model_h, model_w + return np.stack([f[0:H:2, 0::2], f[1:H:2, 0::2], f[0:H:2, 1::2], f[1:H:2, 1::2], + f[H:H + H // 4].reshape(H // 2, W // 2), f[H + H // 4:H + H // 2].reshape(H // 2, W // 2)]) + + +def _jittered_scale(rng, cam, model_w, model_h): + m = np.array([[cam[0] / model_w, 0.0, 0.0], [0.0, cam[1] / model_h, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32) + m += (0.05 * rng.standard_normal((3, 3))).astype(np.float32) * np.array([[1, 1, 1], [1, 1, 1], [0.01, 0.01, 0.1]], dtype=np.float32) + return m + + +def test_frame_layout(): + from iqpilot.selfdrive.iqmodeld.egpu_policy import frame_layout, model_size, nv12_copy_size + shapes, sizes, npy_bytes = frame_layout(SPEC) + assert list(shapes) == ["tfm", "big_tfm", "desire", "traffic_convention", "action_t", "prev_feat"] + assert npy_bytes == (18 + 8 + 2 + 2 + 512) * 4 + assert model_size(SPEC) == (32, 16) + assert nv12_copy_size(128, 64, 32) == 128 * 96 + + +def test_model_runner_matches_device_warp(): + from tinygrad.engine.jit import TinyJit + + from iqpilot.selfdrive.iqmodeld.egpu_policy import ModelRunner, make_run_model, make_warp, model_size, nv12_copy_size + nv12 = _nv12(*CAM) + fcs = nv12_copy_size(nv12[2], nv12[3], nv12[4]) + model_w, model_h = model_size(SPEC) + run_policy = make_run_policy(_fake_model, SPEC, FS, "CPU") + jit = TinyJit(make_run_model(make_warp(nv12, model_w, model_h, "CPU"), run_policy, SPEC, fcs, "CPU"), prune=True) + runner = ModelRunner(jit, SPEC, FS, HIDDEN, "CPU", fcs) + ref = PolicyRunner(TinyJit(make_run_policy(_fake_model, SPEC, FS, "CPU"), prune=True), SPEC, FS, HIDDEN, "CPU") + rng = np.random.default_rng(7) + desire = np.zeros(8, dtype=np.float32) + for i in range(10): + main = rng.integers(0, 256, fcs, dtype=np.int64).astype(np.uint8) + extra = rng.integers(0, 256, fcs, dtype=np.int64).astype(np.uint8) + tfm = _jittered_scale(rng, CAM, model_w, model_h) + big_tfm = _jittered_scale(rng, CAM, model_w, model_h) + if i in (2, 6): + desire[:] = 0 + desire[1 + i % 3] = 1 + traffic = np.array([1.0, 0.0], dtype=np.float32) if i % 2 else np.array([0.0, 1.0], dtype=np.float32) + action_t = np.array([0.1 * i, 0.2], dtype=np.float32) + got = runner.run(main, extra, tfm, big_tfm, desire, traffic, action_t) + warped = np.stack([_numpy_frame_prepare(main, tfm, nv12, model_w, model_h), _numpy_frame_prepare(extra, big_tfm, nv12, model_w, model_h)]) + want = ref.run(warped, desire, traffic, action_t) + np.testing.assert_array_equal(got, want, err_msg=f"frame {i}") diff --git a/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py b/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py index 873a7b498..82b6a49b5 100644 --- a/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py +++ b/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py @@ -157,25 +157,34 @@ def _policy_frame(seed: int, input_spec: dict): return warped -def _tc_off_reference(onnx_path: str, meta: dict): +def _tc_off_reference(onnx_path: str, meta: dict, fmt: int = 2, resolutions: tuple[tuple[int, int], ...] = ()): """Compile+run the model with tensor cores OFF in a child process and return the last of 3 policy frames. This is the trusted reference: TC-off kernels are the conservative path the eMac gate also trusts. Used to catch a TC kernel miscompile that would bias steering.""" import subprocess import tempfile with tempfile.TemporaryDirectory() as td: - ref = os.path.join(td, "ref.npy") + ref = os.path.join(td, "ref.npz" if fmt == 3 else "ref.npy") env = {k: v for k, v in os.environ.items() if k not in ("TC_OPT", "BEAM")} env["TC_OPT"] = "0" env["IQ_EGPU_REFERENCE"] = ref - r = subprocess.run([sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model", - "--model", meta["key"], "--onnx", onnx_path, "--tc-off"], - env=env, capture_output=True, text=True, timeout=14400) + cmd = [sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model", + "--model", meta["key"], "--onnx", onnx_path, "--tc-off", "--format", str(fmt)] + if resolutions: + cmd += ["--camera-resolutions", *(f"{w}x{h}" for w, h in resolutions)] + r = subprocess.run(cmd, env=env, capture_output=True, text=True, timeout=14400) if r.returncode != 0 or not os.path.isfile(ref): raise RuntimeError(f"parity reference compile failed:\n{r.stderr[-2000:]}") return np.load(ref) +def _parity_check(key: str, got: np.ndarray, ref: np.ndarray, label: str = "") -> None: + rel = float(np.abs(got - ref).mean() / max(1e-3, float(np.abs(ref).mean()))) + if rel > 0.01: + raise _ParityFail(f"PARITY FAIL: TC kernels miscompiled {key} {label}(rel={rel:.4f} vs TC-off); recompiling with tensor cores disabled") + print(f" parity vs TC-off reference {label}: rel={rel:.6f} OK") + + def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str: from tinygrad.device import Device from tinygrad.engine.jit import TinyJit @@ -259,16 +268,138 @@ def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str: _validate_pose_outputs(PhaseParser().parse_vision_outputs(_slice_outputs(outs[-1], meta["output_slices"]))) if os.environ.get("TC_OPT") != "0" and not os.environ.get("IQ_EGPU_SKIP_PARITY"): - ref = _tc_off_reference(onnx_path, meta) - rel = float(np.abs(outs[-1] - ref).mean() / max(1e-3, float(np.abs(ref).mean()))) - if rel > 0.01: - raise _ParityFail(f"PARITY FAIL: TC kernels miscompiled {meta['key']} (rel={rel:.4f} vs TC-off); recompiling with tensor cores disabled") - print(f" parity vs TC-off reference: rel={rel:.6f} OK") + _parity_check(meta["key"], outs[-1], _tc_off_reference(onnx_path, meta)) os.replace(tmp, out_path) return out_path +DEFAULT_CAMERA_RESOLUTIONS: tuple[tuple[int, int], ...] = ((1928, 1208), (1344, 760)) + + +def camera_nv12(cam_w: int, cam_h: int) -> tuple[int, int, int, int, int]: + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h) + return (cam_w, cam_h, stride, y_height, uv_height) + + +def _fill_model_frame(packed, seed: int, res: tuple[int, int], model_w: int, model_h: int) -> None: + rng = np.random.default_rng(seed) + cam_w, cam_h = res + scale = np.array([[cam_w / model_w, 0.0, 0.0], [0.0, cam_h / model_h, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32) + for name in ("tfm", "big_tfm"): + packed.views[name][:, :] = scale * (1.0 + 0.02 * rng.standard_normal((3, 3))).astype(np.float32) + for v in packed.frames.values(): + v[:] = rng.integers(0, 256, size=v.shape, dtype=np.uint8) + packed.views["traffic_convention"][:] = [1, 0] + packed.views["action_t"][:] = [0.2, 0.3] + + +def compile_model_v3(meta: dict, onnx_path: str, out_path: str, + resolutions: tuple[tuple[int, int], ...] = DEFAULT_CAMERA_RESOLUTIONS) -> str: + from tinygrad.device import Device + from tinygrad.engine.jit import TinyJit + from tinygrad.nn.onnx import OnnxRunner + + from iqpilot.selfdrive.iqmodeld.egpu_policy import ( + MODEL_FORMAT, dump_oob, load_bundle, make_model_queues, make_run_model, make_run_policy, make_warp, model_size, nv12_copy_size, + ) + + if meta.get("split"): + raise RuntimeError(f"model {meta['key']} is a split model; eGPU compiles fused models only") + input_spec = {name: (tuple(shape), dtype) for name, (shape, dtype) in INPUT_SPEC.items()} + frame_skip = int(meta["frame_skip"]) + hidden = meta["output_slices"]["hidden_state"] + device = Device.DEFAULT + model_w, model_h = model_size(input_spec) + runner = OnnxRunner(onnx_path) + run_policy = make_run_policy(runner, input_spec, frame_skip, device) + + def step(jit, queues, packed, seed: int, res: tuple[int, int]) -> np.ndarray: + _fill_model_frame(packed, seed, res, model_w, model_h) + st = time.perf_counter() + out, = jit(**queues) + flat = out.numpy().reshape(-1) + print(f" model step(seed={seed}, {res[0]}x{res[1]}) {(time.perf_counter() - st) * 1e3:6.1f} ms") + packed.views["prev_feat"][:] = flat[hidden].reshape(packed.views["prev_feat"].shape) + return flat + + def run_three(jit, fcs: int, res: tuple[int, int]) -> list[np.ndarray]: + queues, packed = make_model_queues(input_spec, frame_skip, device, fcs) + return [step(jit, queues, packed, SEED + i, res) for i in range(3)] + + jits: dict[tuple[int, int], object] = {} + sizes: dict[tuple[int, int], int] = {} + nv12s: dict[tuple[int, int], tuple[int, int, int, int, int]] = {} + baselines: dict[tuple[int, int], np.ndarray] = {} + for res in resolutions: + nv12 = camera_nv12(*res) + fcs = nv12_copy_size(nv12[2], nv12[3], nv12[4]) + jit = TinyJit(make_run_model(make_warp(nv12, model_w, model_h, device), run_policy, input_spec, fcs, device), prune=True) + print(f"capture + replay {res[0]}x{res[1]} (frame copy {fcs} B)") + baseline = run_three(jit, fcs, res)[-1] + if baseline.shape[0] != meta["output_len"]: + raise RuntimeError(f"model output length {baseline.shape[0]} != registry {meta['output_len']}") + if not HOST and not np.isfinite(baseline).all(): + raise RuntimeError("compiled model produced non-finite outputs") + jits[res], sizes[res], nv12s[res], baselines[res] = jit, fcs, nv12, baseline + + bundle = { + "format": MODEL_FORMAT, + "run_model": jits, + "frame_copy_size": sizes, + "nv12": nv12s, + "model_key": meta["key"], + "model_sha256": meta["sha256"], + "output_len": int(meta["output_len"]), + "frame_skip": frame_skip, + "input_spec": input_spec, + "input_device": device, + } + os.makedirs(os.path.dirname(out_path), exist_ok=True) + tmp = out_path + ".part" + print("serialize (out-of-band buffers)") + with open(tmp, "wb") as f: + dump_oob(bundle, f) + + del bundle, jits, run_policy, runner + gc.collect() + + print("reload + validate") + loaded = load_bundle(tmp) + outs = {res: run_three(loaded["run_model"][res], loaded["frame_copy_size"][res], res) for res in resolutions} + ref_target = os.environ.get("IQ_EGPU_REFERENCE") + if ref_target: + np.savez(ref_target, **{f"{w}x{h}": outs[(w, h)][-1] for (w, h) in resolutions}) + return out_path + if HOST: + os.replace(tmp, out_path) + return out_path + for res in resolutions: + if not np.array_equal(outs[res][-1], baselines[res]): + raise RuntimeError(f"model outputs differ from baseline after pickle round trip ({res[0]}x{res[1]})") + if np.array_equal(outs[res][0], outs[res][-1]): + raise RuntimeError(f"model outputs insensitive to inputs after pickle round trip ({res[0]}x{res[1]})") + if not all(np.isfinite(o).all() for o in outs[res]): + raise RuntimeError(f"reloaded model produced non-finite outputs ({res[0]}x{res[1]})") + from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import _slice_outputs, _validate_pose_outputs + _validate_pose_outputs(PhaseParser().parse_vision_outputs(_slice_outputs(outs[resolutions[0]][-1], meta["output_slices"]))) + + if os.environ.get("TC_OPT") != "0" and not os.environ.get("IQ_EGPU_SKIP_PARITY"): + ref = _tc_off_reference(onnx_path, meta, fmt=3, resolutions=resolutions) + for (w, h) in resolutions: + _parity_check(meta["key"], outs[(w, h)][-1], ref[f"{w}x{h}"], label=f"{w}x{h} ") + + os.replace(tmp, out_path) + return out_path + + +def _parse_resolution(text: str) -> tuple[int, int]: + w, h = text.lower().split("x") + return int(w), int(h) + + def main() -> None: p = argparse.ArgumentParser() p.add_argument("--model", default=None, help=f"registry key, one of {sorted(EGPU_MODELS)}") @@ -277,13 +408,16 @@ def main() -> None: p.add_argument("--progress-param", default=None) p.add_argument("--progress-base", type=float, default=None) p.add_argument("--progress-span", type=float, default=0.0) - p.add_argument("--format", type=int, default=2, choices=(1, 2)) + p.add_argument("--format", type=int, default=3, choices=(1, 2, 3), + help="3 = warp on the dock from raw NV12 (comma master); 2 = device-warped policy bundle") + p.add_argument("--camera-resolutions", type=_parse_resolution, nargs="+", default=list(DEFAULT_CAMERA_RESOLUTIONS), + help="WxH camera sizes bundled into a format-3 artifact") p.add_argument("--host", action="store_true", help="compile on a mock dock (no AMD hardware); outputs need a dock parity gate") p.add_argument("--arch", default=None, help="target gfx arch for --host") p.add_argument("--tc-off", action="store_true", help="disable tensor-core kernels (conservative; auto-set on parity failure)") args = p.parse_args() - if args.host and args.format != 2: - raise SystemExit("--host supports format 2 only") + if args.host and args.format == 1: + raise SystemExit("--host supports formats 2 and 3 only") if args.model is not None: if args.model in EGPU_MODELS: @@ -311,16 +445,20 @@ def main() -> None: daemon=True) sampler.start() try: - build = compile_policy_model if args.format == 2 else compile_model + if args.format == 3: + from functools import partial + build = partial(compile_model_v3, resolutions=tuple(args.camera_resolutions)) + else: + build = compile_policy_model if args.format == 2 else compile_model try: out = build(meta, onnx_path, args.output or egpu_pkl_path(meta)) except _ParityFail as e: - if os.environ.get("TC_OPT") == "0" or args.format != 2: + if os.environ.get("TC_OPT") == "0" or args.format == 1: raise print(f"{e}\nretrying compile with tensor cores disabled", flush=True) os.environ["TC_OPT"] = "0" os.environ["IQ_EGPU_TC_OFF"] = "1" - out = compile_policy_model(meta, onnx_path, args.output or egpu_pkl_path(meta)) + out = build(meta, onnx_path, args.output or egpu_pkl_path(meta)) finally: if stop is not None: stop.set() diff --git a/iqpilot/system/hardware/tici/agnos.json b/iqpilot/system/hardware/tici/agnos.json index 5172685b5..218676221 100644 --- a/iqpilot/system/hardware/tici/agnos.json +++ b/iqpilot/system/hardware/tici/agnos.json @@ -67,14 +67,14 @@ }, { "name": "boot", - "url": "https://git.konn3kt.com/IQ.Lvbs/iqos/raw/branch/master/boot-4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906.img.xz", - "hash": "4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906", - "hash_raw": "4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906", + "url": "https://git.konn3kt.com/IQ.Lvbs/iqos/raw/branch/master/boot-595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4.img.xz", + "hash": "595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4", + "hash_raw": "595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4", "size": 18216960, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "7be61ba2be5617ac22d6beb8ba8b896109a554347021f5e44b72458748336975" + "ondevice_hash": "b0ee082b2e63a49fcfd1ca2adfb49275e1bb567889cbb9985827fb1de50f943b" }, { "name": "system", diff --git a/iqpilot/system/hardware/tici/agnos.json.sig b/iqpilot/system/hardware/tici/agnos.json.sig index 556ca7b7c..724d7d0ed 100644 --- a/iqpilot/system/hardware/tici/agnos.json.sig +++ b/iqpilot/system/hardware/tici/agnos.json.sig @@ -1 +1 @@ -Myu8k7msX7pt28JKl1Lx5/NnXu9i15nT8bFWnwGsip8wdCYPJqj69AWrfCtlg9YrR98O6ThCAEA1uzUfsrTeDQ== +nqwhbcjeRqyPEM2UWshbP4eCC8EZzDAptGOG0refjvhHlEh32UCAp2Vi/GEKCGOLC3peRW8dRUgwCOXwtEm8Cg== diff --git a/iqpilot/system/hardware/tici/agnos_tici_15_1.json b/iqpilot/system/hardware/tici/agnos_tici_15_1.json index 985cb5efd..4ae6c623d 100644 --- a/iqpilot/system/hardware/tici/agnos_tici_15_1.json +++ b/iqpilot/system/hardware/tici/agnos_tici_15_1.json @@ -56,14 +56,14 @@ }, { "name": "boot", - "url": "https://git.konn3kt.com/IQ.Lvbs/iqos/raw/branch/master/boot-4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906.img.xz", - "hash": "4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906", - "hash_raw": "4258981292746f5622f4a03a1e0c1d77379660e46bbaa455c504d638b5307906", + "url": "https://git.konn3kt.com/IQ.Lvbs/iqos/raw/branch/master/boot-595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4.img.xz", + "hash": "595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4", + "hash_raw": "595f6696c47c17d70a654d8ca04abca946a16dbb1da6250e464a23dff82258a4", "size": 18216960, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "7be61ba2be5617ac22d6beb8ba8b896109a554347021f5e44b72458748336975" + "ondevice_hash": "b0ee082b2e63a49fcfd1ca2adfb49275e1bb567889cbb9985827fb1de50f943b" }, { "name": "system", diff --git a/iqpilot/system/hardware/tici/agnos_tici_15_1.json.sig b/iqpilot/system/hardware/tici/agnos_tici_15_1.json.sig index af20db1df..972561316 100644 --- a/iqpilot/system/hardware/tici/agnos_tici_15_1.json.sig +++ b/iqpilot/system/hardware/tici/agnos_tici_15_1.json.sig @@ -1 +1 @@ -aGtkoy//jP3MWJmd5uvb/mxKu1KsE8ZlhrTUr1CPCotwjIVzsxOncUGN9HnUcMXoUWZbUWz9H8Q+ARYxz41/Ag== +JqXKQi3b6oUtR8CYSq6qoeGjE4SRViVSqYcL8dbMgwXGBkEY2fVCYDrO3nhzhEu7yv8EWGuvK4WiMdGB52iJAQ== diff --git a/iqpilot/system/manager/process_config.py b/iqpilot/system/manager/process_config.py index 2b1de9aee..be81b4942 100644 --- a/iqpilot/system/manager/process_config.py +++ b/iqpilot/system/manager/process_config.py @@ -248,7 +248,7 @@ procs += [ PythonProcess("mapd_manager", "iqpilot.iq_maps.orchestrator", and_(only_offroad, not_low_power)), # locationd - NativeProcess("iqlocd", "iqpilot/selfdrive/iqlocd", ["./iqlocd"], only_onroad), + NativeProcess("iqlocd", "iqpilot/selfdrive/iqlocd", ["./iqlocd"], only_onroad, restart_if_crash=True), ] managed_processes = {p.name: p for p in procs} diff --git a/iqpilot/system/ui/iqwidgets/lib/canvas.py b/iqpilot/system/ui/iqwidgets/lib/canvas.py index b4f4ccf2f..3496d1350 100644 --- a/iqpilot/system/ui/iqwidgets/lib/canvas.py +++ b/iqpilot/system/ui/iqwidgets/lib/canvas.py @@ -13,9 +13,9 @@ Rgba = _p.Color Box = _p.Rectangle Pt = _p.Vector2 -WHITE = _p.WHITE -BLACK = _p.BLACK -RED = _p.RED +WHITE = _p.Color(255, 255, 255, 255) +BLACK = _p.Color(0, 0, 0, 255) +RED = _p.Color(230, 41, 55, 255) CLEAR = _p.Color(0, 0, 0, 0) @@ -24,7 +24,8 @@ def shade(r: int, g: int, b: int, a: int = 255) -> Rgba: def with_opacity(color: Rgba, alpha: float) -> Rgba: - return _p.Color(color.r, color.g, color.b, int(alpha)) + r, g, b = (color.r, color.g, color.b) if hasattr(color, "r") else color[:3] + return _p.Color(r, g, b, int(alpha)) # --- filled / stroked shapes ------------------------------------------------- diff --git a/iqpilot/ui/mici/onroad/hud_renderer.py b/iqpilot/ui/mici/onroad/hud_renderer.py index 4dfc6fa21..a392ba686 100644 --- a/iqpilot/ui/mici/onroad/hud_renderer.py +++ b/iqpilot/ui/mici/onroad/hud_renderer.py @@ -6,21 +6,26 @@ import pyray as rl from iqpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer from iqpilot.ui.onroad.hud_overlays import IQBlindSpotOverlay from iqpilot.ui.mici.onroad.emac_source import EmacSourceIndicator +from iqpilot.ui.mici.onroad.speed_limit_sign import MiciSpeedLimitSign class IQMiciHudRenderer(HudRenderer): def __init__(self): super().__init__() self._overlays = [IQBlindSpotOverlay(), EmacSourceIndicator()] + self._speed_limit_sign = MiciSpeedLimitSign() def _update_state(self) -> None: super()._update_state() for overlay in self._overlays: overlay.update() + self._speed_limit_sign.update() def _render(self, rect: rl.Rectangle) -> None: super()._render(rect) for overlay in self._overlays: overlay.render(rect) + self._speed_limit_sign.set_obscured(self.drawing_top_icons() or not self._can_draw_top_icons) + self._speed_limit_sign.render(rect) def _has_blind_spot_detected(self) -> bool: return any(getattr(overlay, "detected", False) for overlay in self._overlays) diff --git a/iqpilot/ui/mici/onroad/speed_limit_sign.py b/iqpilot/ui/mici/onroad/speed_limit_sign.py new file mode 100644 index 000000000..565f939b6 --- /dev/null +++ b/iqpilot/ui/mici/onroad/speed_limit_sign.py @@ -0,0 +1,75 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import math + +from iqpilot.common.filter_simple import FirstOrderFilter +from iqpilot.selfdrive.ui.ui_state import ui_state +from iqpilot.system.ui.iqwidgets.lib import canvas +from iqpilot.system.ui.lib.application import gui_app +from iqpilot.ui.onroad.hud_overlays import IQSpeedLimitOverlay, _SL_ASSIST, _SL_DARK, _dim + +_SIGN_X = 16 +_SIGN_Y = 108 +_SIGN_W = 60 +_SIGN_H = 64 +_BADGE_SIDE = 24 + + +class MiciSpeedLimitSign(IQSpeedLimitOverlay): + def __init__(self): + super().__init__() + self._visible = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) + self._obscured = False + + def set_obscured(self, obscured: bool) -> None: + self._obscured = obscured + + def _badge(self) -> str: + offset = round(self.speed_limit_offset) + return f"{offset:+d}" if offset != 0 else "" + + def _render(self, rect): + shown = ui_state.speed_limit_mode != 0 and ui_state.is_onroad() and not self._obscured + alpha = self._visible.update(1.0 if shown else 0.0) + if alpha < 1e-2: + return + if self.assist_state == _SL_ASSIST.preActive: + self.assist_frame += 1 + pulse = 0.65 + 0.35 * math.sin(self.assist_frame * math.pi / gui_app.target_fps) + alpha *= self._pulse_ema.update(pulse) + else: + self.assist_frame = 0 + self._pulse_ema.update(1.0) + box = canvas.Box(rect.x + _SIGN_X, rect.y + _SIGN_Y, _SIGN_W, _SIGN_H) + value, _, tint, has_limit = self._spec() + badge = self._badge() if has_limit else "" + (self._vienna if ui_state.is_metric else self._mutcd)(box, value, badge, tint, has_limit, alpha) + + def _vienna(self, rect, value, badge, tint, has_limit, alpha=1.0): + hub = canvas.Pt(rect.x + rect.width / 2, rect.y + rect.height / 2) + radius = rect.width / 2 + canvas.disc_at(hub, radius, _dim(canvas.WHITE, alpha)) + canvas.annulus(hub, radius * 0.78, radius, 0, 360, 36, _dim(canvas.RED, alpha)) + canvas.glyphs_centered(self._bold, value, 22 if len(value) >= 3 else 28, hub, _dim(tint, alpha)) + if badge: + br = _BADGE_SIDE / 2 + bc = canvas.Pt(rect.x + rect.width - br * 0.35, rect.y + br * 0.35) + canvas.disc_at(bc, br, _dim(canvas.BLACK, alpha)) + canvas.annulus(bc, br - 2, br, 0, 360, 24, _dim(_SL_DARK, alpha)) + canvas.glyphs_centered(self._bold, badge, 11 if len(badge) >= 3 else 13, bc, _dim(canvas.WHITE, alpha)) + + def _mutcd(self, rect, value, badge, tint, has_limit, alpha=1.0): + canvas.panel(rect, 0.25, 8, _dim(canvas.WHITE, alpha)) + inner = canvas.Box(rect.x + 4, rect.y + 4, rect.width - 8, rect.height - 8) + canvas.panel_outline(inner, 0.25, 8, 2, _dim(canvas.BLACK, alpha)) + mid = rect.x + rect.width / 2 + canvas.glyphs_centered(self._demi, "SPEED", 12, canvas.Pt(mid, rect.y + 14), _dim(canvas.BLACK, alpha)) + canvas.glyphs_centered(self._demi, "LIMIT", 12, canvas.Pt(mid, rect.y + 25), _dim(canvas.BLACK, alpha)) + canvas.glyphs_centered(self._bold, value, 30 if len(value) <= 2 else 24, canvas.Pt(mid, rect.y + 45), _dim(tint, alpha)) + if badge: + chip = canvas.Box(rect.x + rect.width - _BADGE_SIDE * 0.55, rect.y - _BADGE_SIDE * 0.55, _BADGE_SIDE, _BADGE_SIDE) + canvas.panel(chip, 0.35, 8, _dim(canvas.BLACK, alpha)) + canvas.panel_outline(chip, 0.35, 8, 2, _dim(_SL_DARK, alpha)) + canvas.glyphs_centered(self._bold, badge, 11 if len(badge) >= 3 else 13, + canvas.Pt(chip.x + _BADGE_SIDE / 2, chip.y + _BADGE_SIDE / 2), _dim(canvas.WHITE, alpha))