From dcd434abf0694a05c14f47ef7e0e3804677f1156 Mon Sep 17 00:00:00 2001 From: neokii Date: Wed, 7 Sep 2022 17:01:17 +0900 Subject: [PATCH 01/15] merge changes --- SConstruct | 11 ++- cereal/car.capnp | 34 +++---- docs/CARS.md | 16 ++-- panda/examples/query_fw_versions.py | 1 - release/files_common | 1 - selfdrive/car/hyundai/scc_smoother.py | 2 +- selfdrive/controls/controlsd.py | 6 +- selfdrive/controls/lib/lane_planner.py | 15 +-- selfdrive/controls/lib/lateral_planner.py | 13 ++- selfdrive/controls/lib/longcontrol.py | 93 +++++++++++-------- .../lib/longitudinal_mpc_lib/long_mpc.py | 59 +++++++----- .../controls/lib/longitudinal_planner.py | 3 +- selfdrive/controls/plannerd.py | 12 +-- selfdrive/controls/tests/test_cruise_speed.py | 39 ++++++++ .../controls/tests/test_following_distance.py | 41 ++++++++ selfdrive/manager/manager.py | 1 + .../test/longitudinal_maneuvers/maneuver.py | 4 +- .../test/longitudinal_maneuvers/plant.py | 20 +++- .../test_longitudinal.py | 11 ++- selfdrive/ui/qt/offroad/settings.cc | 12 +-- selfdrive/ui/qt/sidebar.h | 2 - selfdrive/ui/tests/test_translations.py | 13 ++- selfdrive/ui/update_translations.py | 2 +- system/camerad/cameras/camera_qcom2.cc | 16 ++-- tools/replay/consoleui.cc | 11 ++- tools/replay/replay.cc | 7 +- tools/replay/replay.h | 3 + 27 files changed, 301 insertions(+), 147 deletions(-) create mode 100755 selfdrive/controls/tests/test_cruise_speed.py create mode 100755 selfdrive/controls/tests/test_following_distance.py diff --git a/SConstruct b/SConstruct index 7008bb055cafc3..5420130ea432cb 100644 --- a/SConstruct +++ b/SConstruct @@ -75,7 +75,7 @@ lenv = { "ACADOS_SOURCE_DIR": Dir("#third_party/acados/include/acados").abspath, "ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath, - "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer", + "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer" } rpath = lenv["LD_LIBRARY_PATH"].copy() @@ -112,6 +112,9 @@ else: # MacOS if arch == "Darwin": + if real_arch == "x86_64": + lenv["TERA_PATH"] = Dir("#").abspath + f"/third_party/acados/Darwin_x86_64/t_renderer" + brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip() yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64" libpath = [ @@ -120,9 +123,13 @@ else: f"{brew_prefix}/Library", f"{brew_prefix}/opt/openssl/lib", f"{brew_prefix}/Cellar", - f"#third_party/acados/{arch}/lib", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] + if real_arch == "x86_64": + libpath.append(f"#third_party/acados/Darwin_x86_64/lib") + else: + libpath.append(f"#third_party/acados/{arch}/lib") + cflags += ["-DGL_SILENCE_DEPRECATION"] cxxflags += ["-DGL_SILENCE_DEPRECATION"] cpppath += [ diff --git a/cereal/car.capnp b/cereal/car.capnp index 3cae6d339ea37e..ac03a0f8abbd9a 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -370,8 +370,7 @@ struct CarControl { off @0; pid @1; stopping @2; - - startingDEPRECATED @3; + starting @3; } } @@ -494,11 +493,13 @@ struct CarParams { vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state vEgoStarting @59 :Float32; # Speed at which the car goes into starting state directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake - stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping - stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary + stoppingControl @31 :Bool; # Does the car allow full control even at lows speeds when stopping steerControlType @34 :SteerControlType; radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN + stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop + startAccel @32 :Float32; # Required acceleration to get car moving + startingState @70 :Bool; # Does this car make use of special starting state steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds longitudinalActuatorDelayLowerBound @61 :Float32; # Gas/Brake actuator delay in seconds, lower bound @@ -522,18 +523,18 @@ struct CarParams { safetyParam2DEPRECATED @2 :UInt32; } - mdpsBus @70: Int8; - sasBus @71: Int8; - sccBus @72: Int8; - enableAutoHold @73 :Bool; - hasScc13 @74 :Bool; - hasScc14 @75 :Bool; - hasEms @76 :Bool; - hasLfaHda @77 :Bool; - steerFaultMaxAngle @78 :Int16; - steerFaultMaxFrames @79 :Int16; - - disableLateralLiveTuning @80 :Bool; + mdpsBus @71: Int8; + sasBus @72: Int8; + sccBus @73: Int8; + enableAutoHold @74 :Bool; + hasScc13 @75 :Bool; + hasScc14 @76 :Bool; + hasEms @77 :Bool; + hasLfaHda @78 :Bool; + steerFaultMaxAngle @79 :Int16; + steerFaultMaxFrames @80 :Int16; + + disableLateralLiveTuning @81 :Bool; struct LateralParams { torqueBP @0 :List(Int32); @@ -704,7 +705,6 @@ struct CarParams { safetyModelDEPRECATED @9 :SafetyModel; safetyModelPassiveDEPRECATED @42 :SafetyModel = silent; minSpeedCanDEPRECATED @51 :Float32; - startAccelDEPRECATED @32 :Float32; communityFeatureDEPRECATED @46: Bool; startingAccelRateDEPRECATED @53 :Float32; } diff --git a/docs/CARS.md b/docs/CARS.md index aa7b5ecc5b0440..1f0e24865deffd 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -17,10 +17,10 @@ A supported vehicle is one that just works when you install a comma three. All s |Audi|Q3 2020-21|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| |Audi|RS3 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| |Audi|S3 2015-17|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| -|Cadillac|Escalade ESV 2016[1](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|Cadillac|Escalade ESV 2016[1](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |Chrysler|Pacifica 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Chrysler|Pacifica 2019-20|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| @@ -31,8 +31,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Genesis|G70 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Genesis|G90 2017-18|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| +|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Honda|Accord 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Accord Hybrid 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| @@ -85,7 +85,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| |Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Kia|Forte 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| +|Kia|Forte 2019-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| |Kia|K5 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| @@ -185,7 +185,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Volkswagen|Arteon 2018-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Volkswagen|Arteon eHybrid 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Volkswagen|Arteon R 2020-22[7,8](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas 2018-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Atlas 2018-23[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Volkswagen|Atlas Cross Sport 2021-22[7](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Volkswagen|California 2021[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Volkswagen|Caravelle 2020[7](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| diff --git a/panda/examples/query_fw_versions.py b/panda/examples/query_fw_versions.py index 11aba600a253f1..2d4509745c381d 100644 --- a/panda/examples/query_fw_versions.py +++ b/panda/examples/query_fw_versions.py @@ -34,7 +34,6 @@ panda = Panda() panda.set_safety_mode(Panda.SAFETY_ELM327) - panda.set_heartbeat_disabled() print("querying addresses ...") with tqdm(addrs) as t: for addr in t: diff --git a/release/files_common b/release/files_common index 10c66a8960fc43..ad99af752f815f 100644 --- a/release/files_common +++ b/release/files_common @@ -173,7 +173,6 @@ selfdrive/controls/lib/alerts_offroad.json selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py selfdrive/controls/lib/events.py -selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_torque.py diff --git a/selfdrive/car/hyundai/scc_smoother.py b/selfdrive/car/hyundai/scc_smoother.py index e3ff9bc8020926..062a93299e1caf 100644 --- a/selfdrive/car/hyundai/scc_smoother.py +++ b/selfdrive/car/hyundai/scc_smoother.py @@ -10,7 +10,7 @@ from selfdrive.car.hyundai.values import Buttons from common.params import Params from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_MIN, V_CRUISE_DELTA_KM, V_CRUISE_DELTA_MI, CONTROL_N -from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE +from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import AUTO_TR_CRUISE_GAP from selfdrive.ntune import ntune_scc_get diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3aeb31332c6f6b..88e80fab2b17fa 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -15,7 +15,7 @@ from system.version import get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can -from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET +from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.latcontrol import LatControl @@ -739,8 +739,8 @@ def publish_logs(self, CS, start_time, CC, lac_log): model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: - right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 - left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 + right_lane_visible = model_v2.laneLineProbs[2] > 0.5 + left_lane_visible = model_v2.laneLineProbs[1] > 0.5 l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1] r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1] diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 9cbd047ab22e0a..497776326e9265 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -5,18 +5,15 @@ from common.realtime import DT_MDL from system.swaglog import cloudlog -ENABLE_ZORROBYTE = True -ENABLE_INC_LANE_PROB = True - TRAJECTORY_SIZE = 33 -# camera offset is meters from center car to camera -# model path is in the frame of the camera -PATH_OFFSET = 0.00 CAMERA_OFFSET = 0.04 +ENABLE_ZORROBYTE = True +ENABLE_INC_LANE_PROB = True + class LanePlanner: - def __init__(self, wide_camera=False): + def __init__(self,): self.ll_t = np.zeros((TRAJECTORY_SIZE,)) self.ll_x = np.zeros((TRAJECTORY_SIZE,)) self.lll_y = np.zeros((TRAJECTORY_SIZE,)) @@ -35,8 +32,7 @@ def __init__(self, wide_camera=False): self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. - self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET - self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET + self.camera_offset = CAMERA_OFFSET self.readings = [] self.frame = 0 @@ -62,7 +58,6 @@ def parse_model(self, md): def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - path_xyz[:, 1] += self.path_offset l_prob, r_prob = self.lll_prob, self.rll_prob width_pts = self.rll_y - self.lll_y prob_mods = [] diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index dd842db8dce044..ebb2e813a11177 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,20 +1,23 @@ import numpy as np from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp +from selfdrive.controls.lib.lane_planner import LanePlanner from selfdrive.ntune import ntune_common_get from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N -from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from selfdrive.controls.lib.desire_helper import DesireHelper, AUTO_LCA_START_TIME import cereal.messaging as messaging from cereal import log +from common.params import Params +TRAJECTORY_SIZE = 33 +CAMERA_OFFSET = 0.04 class LateralPlanner: - def __init__(self, CP, use_lanelines=True, wide_camera=False): - self.use_lanelines = use_lanelines - self.LP = LanePlanner(wide_camera) + def __init__(self, CP): + self.use_lanelines = not Params().get_bool('EndToEndToggle') + self.LP = LanePlanner() self.DH = DesireHelper() # Vehicle model parameters used to calculate lateral movement of car @@ -88,7 +91,7 @@ def update(self, sm): heading_pts, curv_rate_pts) # init state for next - # mpc.u_sol is the desired curvature rate given x0 curv state. + # mpc.u_sol is the desired curvature rate given x0 curv state. # with x0[3] = measured_curvature, this would be the actual desired rate. # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 75bce5f1ff4b48..52f81e7078fe9a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -11,16 +11,21 @@ ACCEL_MIN_ISO = -3.5 # m/s^2 ACCEL_MAX_ISO = 2.0 # m/s^2 - def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, - v_target_future, brake_pressed, cruise_standstill, radar_state): - """Update longitudinal control state machine""" - accelerating = v_target_future > v_target - stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ - (v_ego < CP.vEgoStopping and - ((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed)) - - starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill + v_target_1sec, brake_pressed, cruise_standstill, radar_state): + accelerating = v_target_1sec > v_target + planned_stop = (v_target < CP.vEgoStopping and + v_target_1sec < CP.vEgoStopping and + not accelerating) + stay_stopped = (v_ego < CP.vEgoStopping and + (brake_pressed or cruise_standstill)) + stopping_condition = planned_stop or stay_stopped + + starting_condition = (v_target_1sec > CP.vEgoStarting and + accelerating and + not cruise_standstill and + not brake_pressed) + started_condition = v_ego > CP.vEgoStarting # neokii if radar_state is not None and radar_state.leadOne is not None and radar_state.leadOne.status: @@ -38,9 +43,21 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, long_control_state = LongCtrlState.stopping elif long_control_state == LongCtrlState.stopping: - if starting_condition: + if starting_condition and CP.startingState: + long_control_state = LongCtrlState.starting + elif starting_condition: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif started_condition: long_control_state = LongCtrlState.pid + + + + return long_control_state @@ -64,65 +81,63 @@ def update(self, active, CS, long_plan, accel_limits, t_since_plan, radar_state) # Interp control trajectory speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) + v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) + a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target + a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target + a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now + + v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) - v_target_future = speeds[-1] + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds) else: v_target = 0.0 - v_target_future = 0.0 + v_target_1sec = 0.0 a_target = 0.0 - # TODO: This check is not complete and needs to be enforced by MPC - a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO) - self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - # Update state machine output_accel = self.last_output_accel self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, - v_target, v_target_future, CS.brakePressed, + v_target, v_target_1sec, CS.brakePressed, CS.cruiseState.standstill, radar_state) if self.long_control_state == LongCtrlState.off: self.reset(CS.vEgo) output_accel = 0. - # tracking objects and driving + elif self.long_control_state == LongCtrlState.stopping: + if output_accel > self.CP.stopAccel: + output_accel -= self.CP.stoppingDecelRate * DT_CTRL * \ + interp(output_accel, [self.CP.stopAccel/2., 0., 1.], [0.5, 1., 2.]) + self.reset(CS.vEgo) + + elif self.long_control_state == LongCtrlState.starting: + output_accel = self.CP.startAccel + self.reset(CS.vEgo) + elif self.long_control_state == LongCtrlState.pid: - self.v_pid = v_target + self.v_pid = v_target_now # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration - prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid + # TODO too complex, needs to be simplified and tested on toyotas + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) freeze_integrator = prevent_overshoot error = self.v_pid - CS.vEgo error_deadzone = apply_deadzone(error, deadzone) - output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target, freeze_integrator=freeze_integrator) - - if prevent_overshoot: - output_accel = min(output_accel, 0.0) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, + feedforward=a_target, + freeze_integrator=freeze_integrator) - # Intention is to stop, switch to a different brake control until we stop - elif self.long_control_state == LongCtrlState.stopping: - # Keep applying brakes until the car is stopped - if not CS.standstill or output_accel > self.CP.stopAccel: - output_accel -= self.CP.stoppingDecelRate * DT_CTRL * \ - interp(output_accel, [self.CP.stopAccel/2., 0., 1.], [0.5, 1., 2.]) - output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - self.reset(CS.vEgo) - self.last_output_accel = output_accel - final_accel = clip(output_accel, accel_limits[0], accel_limits[1]) + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) - return final_accel + return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8a238839457867..d4b058f088ab0c 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -25,7 +25,7 @@ X_DIM = 3 U_DIM = 1 -PARAM_DIM = 5 +PARAM_DIM = 6 COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 @@ -38,6 +38,7 @@ A_CHANGE_COST = 20. # 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 +LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -65,16 +66,16 @@ MAX_ACCEL = 2.0 T_FOLLOW = 1.45 COMFORT_BRAKE = 2.2 -STOP_DISTANCE = 6.3 +STOP_DISTANCE = 6.0 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) -def get_safe_obstacle_distance(v_ego, tr): - return (v_ego**2) / (2 * COMFORT_BRAKE) + tr * v_ego + STOP_DISTANCE +def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE -def desired_follow_distance(v_ego, v_lead, tr): - return get_safe_obstacle_distance(v_ego, tr) - get_stopped_equivalence_factor(v_lead) +def desired_follow_distance(v_ego, v_lead): + return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) def gen_long_model(): @@ -102,8 +103,9 @@ def gen_long_model(): a_max = SX.sym('a_max') x_obstacle = SX.sym('x_obstacle') prev_a = SX.sym('prev_a') - tr = SX.sym('tr') - model.p = vertcat(a_min, a_max, x_obstacle, prev_a, tr) + lead_t_follow = SX.sym('lead_t_follow') + lead_danger_factor = SX.sym('lead_danger_factor') + model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -137,12 +139,13 @@ def gen_long_ocp(): a_min, a_max = ocp.model.p[0], ocp.model.p[1] x_obstacle = ocp.model.p[2] prev_a = ocp.model.p[3] - tr = ocp.model.p[4] + lead_t_follow = ocp.model.p[4] + lead_danger_factor = ocp.model.p[5] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego, tr) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -163,12 +166,12 @@ def gen_long_ocp(): constraints = vertcat(v_ego, (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) + ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) @@ -226,7 +229,7 @@ def reset(self): self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N,1)) self.params = np.zeros((N+1, PARAM_DIM)) - self.param_tr = T_FOLLOW + self.t_follow = T_FOLLOW for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -264,7 +267,7 @@ def set_weights(self, prev_accel_constraint=True): constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 5.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] elif self.mode == 'e2e': cost_weights = [0., 0.2, 0.25, 1., 0.0, .1] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] @@ -329,10 +332,10 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): else: tr = interp(float(cruise_gap), CRUISE_GAP_BP, CRUISE_GAP_V) - if self.mode is not 'acc': + if self.mode != 'acc': tr *= E2E_TR_FACTOR - self.param_tr = tr + self.t_follow = tr # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance @@ -344,6 +347,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): if self.mode == 'acc': self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a self.params[:,1] = self.cruise_max_a + self.params[:,5] = LEAD_DANGER_FACTOR # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. @@ -352,7 +356,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.param_tr) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.t_follow) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -362,6 +366,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): elif self.mode == 'blended': self.params[:,0] = MIN_ACCEL self.params[:,1] = MAX_ACCEL + self.params[:,5] = 1.0 x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) @@ -371,7 +376,8 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): x_and_cruise = np.column_stack([x, cruise_target]) x = np.min(x_and_cruise, axis=1) - self.source = 'e2e' + + self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise' else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') @@ -386,8 +392,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) - - self.params[:,4] = self.param_tr + self.params[:,4] = self.t_follow self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and @@ -396,10 +401,23 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): else: self.crash_cnt = 0 + # Check if it got within lead comfort range + # TODO This should be done cleaner + if self.mode == 'blended': + if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow)) - self.x_sol[:, 0] < 0.0): + self.source = 'lead0' + if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow)) - self.x_sol[:, 0] < 0.0) and \ + (lead_1_obstacle[0] - lead_0_obstacle[0]): + self.source = 'lead1' + + + def update_with_xva(self, x, v, a): self.params[:,0] = -10. self.params[:,1] = 10. self.params[:,2] = 1e5 + self.params[:,4] = self.t_follow + self.params[:,5] = LEAD_DANGER_FACTOR # v, and a are in local frame, but x is wrt the x[0] position # In >90degree turns, x goes to 0 (and may even be -ve) @@ -413,7 +431,6 @@ def update_with_xva(self, x, v, a): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) self.params[:,3] = np.copy(self.prev_a) - self.params[:,4] = self.param_tr self.run() def run(self): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4c2e67bcb637a9..4e3880eb44f8f1 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -45,7 +45,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -class Planner: +class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP params = Params() @@ -135,6 +135,7 @@ def update(self, sm): self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone + # TODO write fcw in e2e_long mode self.fcw = self.mpc.mode == 'acc' and self.mpc.crash_cnt > 5 if self.fcw: cloudlog.info("FCW triggered") diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 559da013263be4..2b5917cdb26785 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -3,7 +3,7 @@ from common.params import Params from common.realtime import Priority, config_realtime_process from system.swaglog import cloudlog -from selfdrive.controls.lib.longitudinal_planner import Planner +from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging @@ -15,14 +15,8 @@ def plannerd_thread(sm=None, pm=None): params = Params() CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) - - use_lanelines = not params.get_bool('EndToEndToggle') - wide_camera = params.get_bool('WideCameraOnly') - - cloudlog.event("e2e mode", on=use_lanelines) - - longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) + longitudinal_planner = LongitudinalPlanner(CP) + lateral_planner = LateralPlanner(CP) if sm is None: sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py new file mode 100755 index 00000000000000..a972bfb0734f75 --- /dev/null +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from common.params import Params + + +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + +def run_cruise_simulation(cruise, t_end=20.): + man = Maneuver( + '', + duration=t_end, + initial_speed=max(cruise - 1., 0.0), + lead_relevancy=True, + initial_distance_lead=100, + cruise_values=[cruise], + prob_lead_values=[0.0], + breakpoints=[0.], + ) + valid, output = man.evaluate() + assert valid + return output[-1,3] + + +class TestCruiseSpeed(unittest.TestCase): + def test_cruise_speed(self): + params = Params() + for e2e in [False, True]: + params.put_bool("EndToEndLong", e2e) + for speed in np.arange(5, 40, 5): + print(f'Testing {speed} m/s') + cruise_speed = float(speed) + + simulation_steady_state = run_cruise_simulation(cruise_speed) + self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s') + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py new file mode 100755 index 00000000000000..3534f58235e2d4 --- /dev/null +++ b/selfdrive/controls/tests/test_following_distance.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from common.params import Params + + +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + +def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): + man = Maneuver( + '', + duration=t_end, + initial_speed=float(v_lead), + lead_relevancy=True, + initial_distance_lead=100, + speed_lead_values=[v_lead], + breakpoints=[0.], + e2e=e2e + ) + valid, output = man.evaluate() + assert valid + return output[-1,2] - output[-1,1] + + +class TestFollowingDistance(unittest.TestCase): + def test_following_distance(self): + params = Params() + for e2e in [False, True]: + params.put_bool("EndToEndLong", e2e) + for speed in np.arange(0, 40, 5): + print(f'Testing {speed} m/s') + v_lead = float(speed) + simulation_steady_state = run_following_distance_simulation(v_lead) + correct_steady_state = desired_follow_distance(v_lead, v_lead) + err_ratio = 0.2 if e2e else 0.1 + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 56fd95c01249e6..85388d6ab9d63b 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -40,6 +40,7 @@ def manager_init() -> None: ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), ("HasAcceptedTerms", "0"), + ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), ("IsMetric", "1"), diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 0d605a5fc7e8cc..dad5d898447820 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -27,7 +27,7 @@ def evaluate(self): speed=self.speed, distance_lead=self.distance_lead, only_lead2=self.only_lead2, - only_radar=self.only_radar + only_radar=self.only_radar, ) valid = True @@ -54,7 +54,7 @@ def evaluate(self): valid = False if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: - print('Planner not starting!') + print('LongitudinalPlanner not starting!') valid = False print("maneuver end", valid) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 7c58a2812cc8ed..dbbd1da5f291c1 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -6,7 +6,8 @@ import cereal.messaging as messaging from common.realtime import Ratekeeper, DT_MDL from selfdrive.controls.lib.longcontrol import LongCtrlState -from selfdrive.controls.lib.longitudinal_planner import Planner +from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner class Plant(): @@ -43,7 +44,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, from selfdrive.car.hyundai.values import CAR from selfdrive.car.hyundai.interface import CarInterface - self.planner = Planner(CarInterface.get_params(CAR.GRANDEUR_IG), init_v=self.speed) + self.planner = LongitudinalPlanner(CarInterface.get_params(CAR.GRANDEUR_IG), init_v=self.speed) def current_time(self): return float(self.rk.frame) / self.rate @@ -89,6 +90,21 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): radar.radarState.leadOne = lead radar.radarState.leadTwo = lead + # Simulate model predicting slightly faster speed + # this is to ensure lead policy is effective when model + # does not predict slowdown in e2e mode + position = log.ModelDataV2.XYZTData.new_message() + position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)] + model.modelV2.position = position + velocity = log.ModelDataV2.XYZTData.new_message() + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)] + model.modelV2.velocity = velocity + acceleration = log.ModelDataV2.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)] + model.modelV2.acceleration = acceleration + + + control.controlsState.longControlState = LongCtrlState.pid control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 58218d2ddc9f4a..9b151d70d582d1 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -140,16 +140,23 @@ def test_longitudinal_setup(self): def run_maneuver_worker(k): def run(self): + params = Params() + man = maneuvers[k] - print(man.title) + params.put_bool("EndToEndLong", True) + print(man.title, ' in e2e mode') + valid, _ = man.evaluate() + self.assertTrue(valid, msg=man.title) + params.put_bool("EndToEndLong", False) + print(man.title, ' in acc mode') valid, _ = man.evaluate() self.assertTrue(valid, msg=man.title) return run - for k in range(len(maneuvers)): setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}", run_maneuver_worker(k)) + if __name__ == "__main__": unittest.main(failfast=True) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index ad23790f971951..18161ff45e9b96 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -65,18 +65,18 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("In this mode openpilot will ignore lanelines and just drive how it thinks a human would."), "../assets/offroad/icon_road.png", }, - { - "EndToEndLong", - tr("๐ŸŒฎ End-to-end longitudinal (extremely alpha) ๐ŸŒฎ"), - tr("Let the driving model control the gas and brakes, openpilot will drive as it thinks a human would. Super experimental."), - "../assets/offroad/icon_road.png", - }, { "DisengageOnAccelerator", tr("Disengage On Accelerator Pedal"), tr("When enabled, pressing the accelerator pedal will disengage openpilot."), "../assets/offroad/icon_disengage_on_accelerator.svg", }, + { + "EndToEndLong", + tr("๐ŸŒฎ End-to-end longitudinal (extremely alpha) ๐ŸŒฎ"), + tr("Let the driving model control the gas and brakes, openpilot will drive as it thinks a human would. Super experimental."), + "../assets/offroad/icon_road.png", + }, #ifdef ENABLE_MAPS { "NavSettingTime24h", diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index acb3555972e75e..8ac436a763163d 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -3,7 +3,6 @@ #include #include -#include "common/params.h" #include "selfdrive/ui/ui.h" typedef QPair, QColor> ItemStatus; @@ -50,7 +49,6 @@ public slots: const QColor warning_color = QColor(218, 202, 37); const QColor danger_color = QColor(201, 34, 49); - Params params; ItemStatus storage_status, panda_status, temp_status; QString net_type; int net_strength = 0; diff --git a/selfdrive/ui/tests/test_translations.py b/selfdrive/ui/tests/test_translations.py index fead288dfcad13..d8609f110b999c 100755 --- a/selfdrive/ui/tests/test_translations.py +++ b/selfdrive/ui/tests/test_translations.py @@ -9,6 +9,7 @@ from selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations TMP_TRANSLATIONS_DIR = os.path.join(TRANSLATIONS_DIR, "tmp") +LOCATION_TAG = "" not in cur_translations, + self.assertTrue("" not in cur_translations, f"{file} ({name}) translation file has unfinished translations. Finish translations or mark them as completed in Qt Linguist") def test_vanished_translations(self): for name, file in self.translation_files.items(): with self.subTest(name=name, file=file): cur_translations = self._read_translation_file(TRANSLATIONS_DIR, file) - self.assertTrue(b"" not in cur_translations, + self.assertTrue("" not in cur_translations, f"{file} ({name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them") def test_plural_translations(self): diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index 05781589db160c..4f2ab30461d280 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -19,7 +19,7 @@ def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLA for file in translation_files.values(): tr_file = os.path.join(translations_dir, f"{file}.ts") - args = f"lupdate -recursive {UI_DIR} -ts {tr_file}" + args = f"lupdate -locations relative -recursive {UI_DIR} -ts {tr_file}" if vanish: args += " -no-obsolete" if file in plural_only: diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index c06aeb43604507..a86218c06e8448 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -1101,14 +1101,18 @@ void CameraState::set_camera_exposure(float grey_frac) { } else if (enable_dc_gain && target_grey > 0.3) { enable_dc_gain = false; } + + std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { + gain_bytes = Params().get("CameraDebugExpGain"); + time_bytes = Params().get("CameraDebugExpTime"); + } + + if (gain_bytes.size() > 0 && time_bytes.size() > 0) { // Override gain and exposure time - if (buf.cur_frame_data.frame_id % 5 == 0) { - std::string gain_bytes = Params().get("CameraDebugExpGain"); - std::string time_bytes = Params().get("CameraDebugExpTime"); - gain_idx = std::stoi(gain_bytes); - exposure_time = std::stoi(time_bytes); - } + gain_idx = std::stoi(gain_bytes); + exposure_time = std::stoi(time_bytes); + new_g = gain_idx; new_t = exposure_time; enable_dc_gain = false; diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc index 6b419ca9d80908..5f165ac31226d6 100644 --- a/tools/replay/consoleui.cc +++ b/tools/replay/consoleui.cc @@ -18,7 +18,10 @@ const std::initializer_list> keyboard_shortc {"space", "Pause/Resume"}, {"e", "Next Engagement"}, {"d", "Next Disengagement"}, - {"t", "Next User Tag"} + {"t", "Next User Tag"}, + {"i", "Next Info"}, + {"w", "Next Warning"}, + {"c", "Next Critical"}, }, { {"enter", "Enter seek request"}, @@ -344,6 +347,12 @@ void ConsoleUI::handleKey(char c) { replay->seekToFlag(FindFlag::nextDisEngagement); } else if (c == 't') { replay->seekToFlag(FindFlag::nextUserFlag); + } else if (c == 'i') { + replay->seekToFlag(FindFlag::nextInfo); + } else if (c == 'w') { + replay->seekToFlag(FindFlag::nextWarning); + } else if (c == 'c') { + replay->seekToFlag(FindFlag::nextCritical); } else if (c == 'm') { replay->seekTo(+60, true); } else if (c == 'M') { diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 4b983fff85969d..9dc1f03fc71cb2 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -166,8 +166,11 @@ std::optional Replay::find(FindFlag flag) { } else if (flag == FindFlag::nextDisEngagement && end_ts > cur_ts) { return end_ts; } - } else if (type == TimelineType::UserFlag) { - if (flag == FindFlag::nextUserFlag && start_ts > cur_ts) { + } else if (start_ts > cur_ts) { + if ((flag == FindFlag::nextUserFlag && type == TimelineType::UserFlag) || + (flag == FindFlag::nextInfo && type == TimelineType::AlertInfo) || + (flag == FindFlag::nextWarning && type == TimelineType::AlertWarning) || + (flag == FindFlag::nextCritical && type == TimelineType::AlertCritical)) { return start_ts; } } diff --git a/tools/replay/replay.h b/tools/replay/replay.h index e4217736d57c8d..e86c453f7e83d1 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -26,6 +26,9 @@ enum class FindFlag { nextEngagement, nextDisEngagement, nextUserFlag, + nextInfo, + nextWarning, + nextCritical }; enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag }; From 6b4dabe99a6cc44aab81c11eef71fef5c82353f7 Mon Sep 17 00:00:00 2001 From: neokii Date: Wed, 7 Sep 2022 19:02:45 +0900 Subject: [PATCH 02/15] long tuning --- selfdrive/car/hyundai/interface.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 21 ++++++++----------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ddf49658eb519f..3a1f81611ac077 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -35,7 +35,7 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed): v_current_kph = current_speed * CV.MS_TO_KPH gas_max_bp = [10., 20., 50., 70., 130., 150.] - gas_max_v = [1.4, 1.2, 0.63, 0.44, 0.15, 0.1] + gas_max_v = [1.3, 1.1, 0.63, 0.44, 0.15, 0.1] return CarControllerParams.ACCEL_MIN, interp(v_current_kph, gas_max_bp, gas_max_v) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d4b058f088ab0c..d411457f1a1915 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -44,11 +44,11 @@ CRUISE_GAP_BP = [1., 2., 3., 4.] -CRUISE_GAP_V = [1.1, 1.3, 1.58, 2.10] -AUTO_TR_BP = [0., 30.*CV.KPH_TO_MS, 70.*CV.KPH_TO_MS, 110.*CV.KPH_TO_MS] -AUTO_TR_V = [1.1, 1.2, 1.3, 1.45] +CRUISE_GAP_V = [1.2, 1.4, 1.6, 1.8] +CRUISE_GAP_E2E_V = [1.3, 1.45, 1.6, 1.8] -E2E_TR_FACTOR = 1.3 +AUTO_TR_BP = [0., 30.*CV.KPH_TO_MS, 70.*CV.KPH_TO_MS, 110.*CV.KPH_TO_MS] +AUTO_TR_V = [1.2, 1.3, 1.4, 1.5] AUTO_TR_CRUISE_GAP = 4 DIFF_RADAR_VISION = 1.0 @@ -65,8 +65,8 @@ MIN_ACCEL = -3.5 MAX_ACCEL = 2.0 T_FOLLOW = 1.45 -COMFORT_BRAKE = 2.2 -STOP_DISTANCE = 6.0 +COMFORT_BRAKE = 2.5 +STOP_DISTANCE = 4.5 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -326,14 +326,11 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): lead_xv_1 = self.process_lead(radarstate.leadTwo) # neokii - cruise_gap = int(clip(carstate.cruiseGap, 1., 4.)) + cruise_gap = int(clip(carstate.cruiseGap, 1., 4.)) if carstate.cruiseGap > 0 else AUTO_TR_CRUISE_GAP if cruise_gap == AUTO_TR_CRUISE_GAP: - tr = interp(carstate.vEgo, AUTO_TR_BP, AUTO_TR_V) + tr = interp(carstate.vEgo, AUTO_TR_BP, AUTO_TR_V) if self.mode == 'acc' else T_FOLLOW else: - tr = interp(float(cruise_gap), CRUISE_GAP_BP, CRUISE_GAP_V) - - if self.mode != 'acc': - tr *= E2E_TR_FACTOR + tr = interp(float(cruise_gap), CRUISE_GAP_BP, CRUISE_GAP_V if self.mode == 'acc' else CRUISE_GAP_E2E_V) self.t_follow = tr From 3ec537478eee7799fba33ded5b559032baf04014 Mon Sep 17 00:00:00 2001 From: neokii Date: Thu, 8 Sep 2022 16:12:43 +0900 Subject: [PATCH 03/15] long tuning --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d411457f1a1915..7eb3f98963fd6f 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -66,7 +66,7 @@ MAX_ACCEL = 2.0 T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 4.5 +STOP_DISTANCE = 4.0 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) From 3dbb011d29f5b031c8e2ba390df8b94df3a6efa7 Mon Sep 17 00:00:00 2001 From: neokii Date: Thu, 8 Sep 2022 18:06:22 +0900 Subject: [PATCH 04/15] long - boost --- selfdrive/car/hyundai/scc_smoother.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/hyundai/scc_smoother.py b/selfdrive/car/hyundai/scc_smoother.py index 062a93299e1caf..66e05abaeab427 100644 --- a/selfdrive/car/hyundai/scc_smoother.py +++ b/selfdrive/car/hyundai/scc_smoother.py @@ -357,11 +357,12 @@ def get_apply_accel(self, CS, sm, accel, stopping): gas_factor = ntune_scc_get("sccGasFactor") brake_factor = ntune_scc_get("sccBrakeFactor") - if not self.e2e_long: - start_boost = interp(CS.out.vEgo, [0.0, CREEP_SPEED, 2 * CREEP_SPEED], [0.6, 0.6, 0.0]) - is_accelerating = interp(accel, [0.0, 0.2], [0.0, 1.0]) - boost = start_boost * is_accelerating - accel += boost + boost_v = 0.3 if self.e2e_long else 0.6 + + start_boost = interp(CS.out.vEgo, [CREEP_SPEED, 2 * CREEP_SPEED], [boost_v, 0.0]) + is_accelerating = interp(accel, [0.0, 0.2], [0.0, 1.0]) + boost = start_boost * is_accelerating + accel += boost if accel > 0: accel *= gas_factor From f161ca6be105e1316221b9a6140de9692c7a67f5 Mon Sep 17 00:00:00 2001 From: neokii Date: Thu, 8 Sep 2022 21:51:45 +0900 Subject: [PATCH 05/15] fix safety check --- panda/board/safety/safety_hyundai_community.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h index f14c5d18d315d7..85d40ad38cdbef 100644 --- a/panda/board/safety/safety_hyundai_community.h +++ b/panda/board/safety/safety_hyundai_community.h @@ -200,9 +200,9 @@ static int hyundai_community_tx_hook(CANPacket_t *to_send, bool longitudinal_all if (addr == 832) { LKAS11_op = 20; int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024; - //bool steer_req = GET_BIT(to_send, 27U) != 0U; + bool steer_req = GET_BIT(to_send, 27U) != 0U; - if (steer_torque_cmd_checks(desired_torque, 1, HYUNDAI_STEERING_LIMITS)) { + if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_STEERING_LIMITS)) { tx = 0; } } From ae5cf9e028e8b768a0a6d3fcd2b423bc60400dff Mon Sep 17 00:00:00 2001 From: neokii Date: Fri, 9 Sep 2022 04:13:19 +0900 Subject: [PATCH 06/15] add missing files --- selfdrive/modeld/__init__.py | 0 selfdrive/modeld/runners/onnx_runner.py | 73 ++++++++++++ selfdrive/modeld/runners/onnxmodel.cc | 142 +++++++++++++++++++++++ selfdrive/modeld/runners/onnxmodel.h | 45 +++++++ selfdrive/modeld/thneed/thneed_pc.cc | 32 ----- selfdrive/modeld/transforms/transform.cc | 2 +- 6 files changed, 261 insertions(+), 33 deletions(-) create mode 100644 selfdrive/modeld/__init__.py create mode 100755 selfdrive/modeld/runners/onnx_runner.py create mode 100644 selfdrive/modeld/runners/onnxmodel.cc create mode 100644 selfdrive/modeld/runners/onnxmodel.h delete mode 100644 selfdrive/modeld/thneed/thneed_pc.cc diff --git a/selfdrive/modeld/__init__.py b/selfdrive/modeld/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py new file mode 100755 index 00000000000000..ac7cc68814df22 --- /dev/null +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import os +import sys +import numpy as np + +os.environ["OMP_NUM_THREADS"] = "4" +os.environ["OMP_WAIT_POLICY"] = "PASSIVE" + +import onnxruntime as ort # pylint: disable=import-error + +def read(sz, tf8=False): + dd = [] + gt = 0 + szof = 1 if tf8 else 4 + while gt < sz * szof: + st = os.read(0, sz * szof - gt) + assert(len(st) > 0) + dd.append(st) + gt += len(st) + r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) + if tf8: + r = r / 255. + return r + +def write(d): + os.write(1, d.tobytes()) + +def run_loop(m, tf8_input=False): + ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] + keys = [x.name for x in m.get_inputs()] + + # run once to initialize CUDA provider + if "CUDAExecutionProvider" in m.get_providers(): + m.run(None, dict(zip(keys, [np.zeros(shp, dtype=np.float32) for shp in ishapes]))) + + print("ready to run onnx model", keys, ishapes, file=sys.stderr) + while 1: + inputs = [] + for k, shp in zip(keys, ishapes): + ts = np.product(shp) + #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) + inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) + ret = m.run(None, dict(zip(keys, inputs))) + #print(ret, file=sys.stderr) + for r in ret: + write(r) + + +if __name__ == "__main__": + print(sys.argv, file=sys.stderr) + print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr) + options = ort.SessionOptions() + options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL + if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: + provider = 'OpenVINOExecutionProvider' + elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: + options.intra_op_num_threads = 2 + provider = 'CUDAExecutionProvider' + else: + options.intra_op_num_threads = 2 + options.inter_op_num_threads = 8 + options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL + options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL + provider = 'CPUExecutionProvider' + + try: + print("Onnx selected provider: ", [provider], file=sys.stderr) + ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider]) + print("Onnx using ", ort_session.get_providers(), file=sys.stderr) + run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv)) + except KeyboardInterrupt: + pass diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc new file mode 100644 index 00000000000000..447d90fd7efeb5 --- /dev/null +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -0,0 +1,142 @@ +#include "selfdrive/modeld/runners/onnxmodel.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8, cl_context context) { + LOGD("loading model %s", path); + + output = _output; + output_size = _output_size; + use_extra = _use_extra; + use_tf8 = _use_tf8; + + int err = pipe(pipein); + assert(err == 0); + err = pipe(pipeout); + assert(err == 0); + + std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); + std::string onnx_runner = exe_dir + "/runners/onnx_runner.py"; + std::string tf8_arg = use_tf8 ? "--use_tf8" : ""; + + proc_pid = fork(); + if (proc_pid == 0) { + LOGD("spawning onnx process %s", onnx_runner.c_str()); + char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, (char*)tf8_arg.c_str(), nullptr}; + dup2(pipein[0], 0); + dup2(pipeout[1], 1); + close(pipein[0]); + close(pipein[1]); + close(pipeout[0]); + close(pipeout[1]); + execvp(onnx_runner.c_str(), argv); + } + + // parent + close(pipein[0]); + close(pipeout[1]); +} + +ONNXModel::~ONNXModel() { + close(pipein[1]); + close(pipeout[0]); + kill(proc_pid, SIGTERM); +} + +void ONNXModel::pwrite(float *buf, int size) { + char *cbuf = (char *)buf; + int tw = size*sizeof(float); + while (tw > 0) { + int err = write(pipein[1], cbuf, tw); + //printf("host write %d\n", err); + assert(err >= 0); + cbuf += err; + tw -= err; + } + LOGD("host write of size %d done", size); +} + +void ONNXModel::pread(float *buf, int size) { + char *cbuf = (char *)buf; + int tr = size*sizeof(float); + struct pollfd fds[1]; + fds[0].fd = pipeout[0]; + fds[0].events = POLLIN; + while (tr > 0) { + int err; + err = poll(fds, 1, 10000); // 10 second timeout + assert(err == 1 || (err == -1 && errno == EINTR)); + LOGD("host read remaining %d/%d poll %d", tr, size*sizeof(float), err); + err = read(pipeout[0], cbuf, tr); + assert(err > 0 || (err == 0 && errno == EINTR)); + cbuf += err; + tr -= err; + } + LOGD("host read done"); +} + +void ONNXModel::addRecurrent(float *state, int state_size) { + rnn_input_buf = state; + rnn_state_size = state_size; +} + +void ONNXModel::addDesire(float *state, int state_size) { + desire_input_buf = state; + desire_state_size = state_size; +} + +void ONNXModel::addTrafficConvention(float *state, int state_size) { + traffic_convention_input_buf = state; + traffic_convention_size = state_size; +} + +void ONNXModel::addCalib(float *state, int state_size) { + calib_input_buf = state; + calib_size = state_size; +} + +void ONNXModel::addImage(float *image_buf, int buf_size) { + image_input_buf = image_buf; + image_buf_size = buf_size; +} + +void ONNXModel::addExtra(float *image_buf, int buf_size) { + extra_input_buf = image_buf; + extra_buf_size = buf_size; +} + +void ONNXModel::execute() { + // order must be this + if (image_input_buf != NULL) { + pwrite(image_input_buf, image_buf_size); + } + if (extra_input_buf != NULL) { + pwrite(extra_input_buf, extra_buf_size); + } + if (desire_input_buf != NULL) { + pwrite(desire_input_buf, desire_state_size); + } + if (traffic_convention_input_buf != NULL) { + pwrite(traffic_convention_input_buf, traffic_convention_size); + } + if (calib_input_buf != NULL) { + pwrite(calib_input_buf, calib_size); + } + if (rnn_input_buf != NULL) { + pwrite(rnn_input_buf, rnn_state_size); + } + pread(output, output_size); +} + diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h new file mode 100644 index 00000000000000..d5b7bfecf0cc05 --- /dev/null +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "selfdrive/modeld/runners/runmodel.h" + +class ONNXModel : public RunModel { +public: + ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false, cl_context context = NULL); + ~ONNXModel(); + void addRecurrent(float *state, int state_size); + void addDesire(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); + void addCalib(float *state, int state_size); + void addImage(float *image_buf, int buf_size); + void addExtra(float *image_buf, int buf_size); + void execute(); +private: + int proc_pid; + + float *output; + size_t output_size; + + float *rnn_input_buf = NULL; + int rnn_state_size; + float *desire_input_buf = NULL; + int desire_state_size; + float *traffic_convention_input_buf = NULL; + int traffic_convention_size; + float *calib_input_buf = NULL; + int calib_size; + float *image_input_buf = NULL; + int image_buf_size; + bool use_tf8; + float *extra_input_buf = NULL; + int extra_buf_size; + bool use_extra; + + // pipe to communicate to keras subprocess + void pread(float *buf, int size); + void pwrite(float *buf, int size); + int pipein[2]; + int pipeout[2]; +}; + diff --git a/selfdrive/modeld/thneed/thneed_pc.cc b/selfdrive/modeld/thneed/thneed_pc.cc deleted file mode 100644 index 8d0037628e2f3d..00000000000000 --- a/selfdrive/modeld/thneed/thneed_pc.cc +++ /dev/null @@ -1,32 +0,0 @@ -#include "selfdrive/modeld/thneed/thneed.h" - -#include - -#include "common/clutil.h" -#include "common/timing.h" - -Thneed::Thneed(bool do_clinit, cl_context _context) { - context = _context; - if (do_clinit) clinit(); - char *thneed_debug_env = getenv("THNEED_DEBUG"); - debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; -} - -void Thneed::execute(float **finputs, float *foutput, bool slow) { - uint64_t tb, te; - if (debug >= 1) tb = nanos_since_boot(); - - // ****** copy inputs - copy_inputs(finputs); - - // ****** run commands - clexec(); - - // ****** copy outputs - copy_output(foutput); - - if (debug >= 1) { - te = nanos_since_boot(); - printf("model exec in %lu us\n", (te-tb)/1000); - } -} diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc index cabc58a46d8337..f341314144ec02 100644 --- a/selfdrive/modeld/transforms/transform.cc +++ b/selfdrive/modeld/transforms/transform.cc @@ -32,7 +32,7 @@ void transform_queue(Transform* s, const int zero = 0; // sampled using pixel center origin - // (because thats how fastcv and opencv does it) + // (because that's how fastcv and opencv does it) mat3 projection_y = projection; From b96b97f90a54bc4988f55759e4b21d70175d5f90 Mon Sep 17 00:00:00 2001 From: neokii Date: Fri, 9 Sep 2022 05:03:57 +0900 Subject: [PATCH 07/15] Revert "fix safety check" This reverts commit f161ca6be105e1316221b9a6140de9692c7a67f5. --- panda/board/safety/safety_hyundai_community.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h index 85d40ad38cdbef..f14c5d18d315d7 100644 --- a/panda/board/safety/safety_hyundai_community.h +++ b/panda/board/safety/safety_hyundai_community.h @@ -200,9 +200,9 @@ static int hyundai_community_tx_hook(CANPacket_t *to_send, bool longitudinal_all if (addr == 832) { LKAS11_op = 20; int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024; - bool steer_req = GET_BIT(to_send, 27U) != 0U; + //bool steer_req = GET_BIT(to_send, 27U) != 0U; - if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_STEERING_LIMITS)) { + if (steer_torque_cmd_checks(desired_torque, 1, HYUNDAI_STEERING_LIMITS)) { tx = 0; } } From 240056de082cf7f717d1f737f11cbd49aba4071a Mon Sep 17 00:00:00 2001 From: neokii Date: Fri, 9 Sep 2022 05:09:35 +0900 Subject: [PATCH 08/15] fix --- panda/board/safety/safety_hyundai_community.h | 50 ++++++++++++++++++- 1 file changed, 48 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h index f14c5d18d315d7..36e9174c6f0a8e 100644 --- a/panda/board/safety/safety_hyundai_community.h +++ b/panda/board/safety/safety_hyundai_community.h @@ -200,9 +200,55 @@ static int hyundai_community_tx_hook(CANPacket_t *to_send, bool longitudinal_all if (addr == 832) { LKAS11_op = 20; int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024; - //bool steer_req = GET_BIT(to_send, 27U) != 0U; + uint32_t ts = microsecond_timer_get(); + bool violation = 0; - if (steer_torque_cmd_checks(desired_torque, 1, HYUNDAI_STEERING_LIMITS)) { + if (controls_allowed) { + // *** global torque limit check *** + bool torque_check = 0; + violation |= torque_check = max_limit_check(desired_torque, HYUNDAI_STEERING_LIMITS.max_steer, -HYUNDAI_STEERING_LIMITS.max_steer); + if (torque_check) { + puts(" LKAS TX not allowed: torque limit check failed!\n");} + + // *** torque rate limit check *** + bool torque_rate_check = 0; + violation |= torque_rate_check = driver_limit_check(desired_torque, desired_torque_last, &torque_driver, + HYUNDAI_STEERING_LIMITS.max_steer, HYUNDAI_STEERING_LIMITS.max_rate_up, HYUNDAI_STEERING_LIMITS.max_rate_down, + HYUNDAI_STEERING_LIMITS.driver_torque_allowance, HYUNDAI_STEERING_LIMITS.driver_torque_factor); + if (torque_rate_check) { + puts(" LKAS TX not allowed: torque rate limit check failed!\n");} + + // used next time + desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + bool torque_rt_check = 0; + violation |= torque_rt_check = rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_STEERING_LIMITS.max_rt_delta); + if (torque_rt_check) { + puts(" LKAS TX not allowed: torque real time rate limit check failed!\n");} + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + if (ts_elapsed > HYUNDAI_STEERING_LIMITS.max_rt_interval) { + rt_torque_last = desired_torque; + ts_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + puts(" LKAS torque not allowed: controls not allowed!\n"); + } + + // reset to 0 if either controls is not allowed or there's a violation + if (!controls_allowed) { // a reset worsen the issue of Panda blocking some valid LKAS messages + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = ts; + } + + if (violation) { tx = 0; } } From 6066d335a9e2cabee6cd7e3f7a6dd4f6e014022c Mon Sep 17 00:00:00 2001 From: neokii Date: Fri, 9 Sep 2022 05:38:49 +0900 Subject: [PATCH 09/15] long tuning --- selfdrive/car/hyundai/interface.py | 2 +- selfdrive/car/hyundai/scc_smoother.py | 2 +- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 3a1f81611ac077..48f30c04f266cb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -98,7 +98,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl ret.longitudinalActuatorDelayLowerBound = 0.3 ret.longitudinalActuatorDelayUpperBound = 0.3 - ret.stopAccel = -2.5 + ret.stopAccel = -2.0 ret.stoppingDecelRate = 0.4 # brake_travel/s while trying to stop ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.5 diff --git a/selfdrive/car/hyundai/scc_smoother.py b/selfdrive/car/hyundai/scc_smoother.py index 66e05abaeab427..4add09c3bae072 100644 --- a/selfdrive/car/hyundai/scc_smoother.py +++ b/selfdrive/car/hyundai/scc_smoother.py @@ -357,7 +357,7 @@ def get_apply_accel(self, CS, sm, accel, stopping): gas_factor = ntune_scc_get("sccGasFactor") brake_factor = ntune_scc_get("sccBrakeFactor") - boost_v = 0.3 if self.e2e_long else 0.6 + boost_v = 0.2 if self.e2e_long else 0.5 start_boost = interp(CS.out.vEgo, [CREEP_SPEED, 2 * CREEP_SPEED], [boost_v, 0.0]) is_accelerating = interp(accel, [0.0, 0.2], [0.0, 1.0]) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7eb3f98963fd6f..9e8c8f20f42d9e 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -44,7 +44,7 @@ CRUISE_GAP_BP = [1., 2., 3., 4.] -CRUISE_GAP_V = [1.2, 1.4, 1.6, 1.8] +CRUISE_GAP_V = [1.1, 1.3, 1.6, 1.8] CRUISE_GAP_E2E_V = [1.3, 1.45, 1.6, 1.8] AUTO_TR_BP = [0., 30.*CV.KPH_TO_MS, 70.*CV.KPH_TO_MS, 110.*CV.KPH_TO_MS] @@ -66,7 +66,7 @@ MAX_ACCEL = 2.0 T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 4.0 +STOP_DISTANCE = 5.0 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) From 8fe3688d0195b85ece06439b7ead1bb295e4d1af Mon Sep 17 00:00:00 2001 From: neokii Date: Sun, 11 Sep 2022 23:03:35 +0900 Subject: [PATCH 10/15] stop distance for e2e --- .../lib/longitudinal_mpc_lib/long_mpc.py | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9e8c8f20f42d9e..55747bba159447 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -25,7 +25,7 @@ X_DIM = 3 U_DIM = 1 -PARAM_DIM = 6 +PARAM_DIM = 7 COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 @@ -67,12 +67,13 @@ T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 STOP_DISTANCE = 5.0 +STOP_DISTANCE_E2E = 4.0 def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) -def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE +def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW, stop_dist=STOP_DISTANCE): + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_dist def desired_follow_distance(v_ego, v_lead): return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) @@ -105,7 +106,9 @@ def gen_long_model(): prev_a = SX.sym('prev_a') lead_t_follow = SX.sym('lead_t_follow') lead_danger_factor = SX.sym('lead_danger_factor') - model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor) + stop_dist = SX.sym('stop_dist') + + model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor, stop_dist) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) @@ -141,11 +144,12 @@ def gen_long_ocp(): prev_a = ocp.model.p[3] lead_t_follow = ocp.model.p[4] lead_danger_factor = ocp.model.p[5] + stop_dist = ocp.model.p[6] ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow) + desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow, stop_dist) # The main cost in normal operation is how close you are to the "desired" distance # from an obstacle at every timestep. This obstacle can be a lead car @@ -230,6 +234,7 @@ def reset(self): self.u_sol = np.zeros((N,1)) self.params = np.zeros((N+1, PARAM_DIM)) self.t_follow = T_FOLLOW + self.stop_dist = STOP_DISTANCE for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 @@ -334,6 +339,8 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): self.t_follow = tr + self.stop_dist = STOP_DISTANCE if self.mode == 'acc' else STOP_DISTANCE_E2E + # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. @@ -353,7 +360,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.t_follow) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, self.t_follow, self.stop_dist) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -390,6 +397,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) self.params[:,4] = self.t_follow + self.params[:,6] = self.stop_dist self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and @@ -401,9 +409,9 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): # Check if it got within lead comfort range # TODO This should be done cleaner if self.mode == 'blended': - if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow)) - self.x_sol[:, 0] < 0.0): + if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow, self.stop_dist)) - self.x_sol[:, 0] < 0.0): self.source = 'lead0' - if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow)) - self.x_sol[:, 0] < 0.0) and \ + if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.t_follow, self.stop_dist)) - self.x_sol[:, 0] < 0.0) and \ (lead_1_obstacle[0] - lead_0_obstacle[0]): self.source = 'lead1' @@ -415,6 +423,7 @@ def update_with_xva(self, x, v, a): self.params[:,2] = 1e5 self.params[:,4] = self.t_follow self.params[:,5] = LEAD_DANGER_FACTOR + self.params[:,6] = self.stop_dist # v, and a are in local frame, but x is wrt the x[0] position # In >90degree turns, x goes to 0 (and may even be -ve) From a791550cad0ec652ce09b7637c8648388f223189 Mon Sep 17 00:00:00 2001 From: neokii Date: Mon, 12 Sep 2022 00:08:05 +0900 Subject: [PATCH 11/15] merge changes --- cereal/car.capnp | 41 +- common/params.cc | 4 +- docs/CARS.md | 56 +- docs/assets/icon-star-empty.svg | 56 -- docs/assets/icon-star-full.svg | 56 -- docs/assets/icon-star-half.svg | 66 -- opendbc/can/parser.cc | 12 +- opendbc/hyundai_canfd.dbc | 2 + panda/board/build_all.sh | 6 - panda/board/drivers/fan.h | 4 +- panda/board/flash_h7.sh | 5 - panda/board/recover_h7.sh | 11 - panda/board/safety.h | 40 +- panda/board/safety/safety_hyundai_community.h | 53 +- panda/board/safety_declarations.h | 9 +- release/files_common | 1 + release/files_tici | 2 + selfdrive/car/car_helpers.py | 4 +- selfdrive/car/fw_query_definitions.py | 66 ++ selfdrive/car/fw_versions.py | 278 +------ selfdrive/car/hyundai/scc_smoother.py | 20 +- selfdrive/car/hyundai/values.py | 28 +- selfdrive/car/interfaces.py | 2 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/torque_data/override.yaml | 1 + selfdrive/car/vin.py | 11 +- selfdrive/controls/controlsd.py | 6 +- selfdrive/controls/lib/desire_helper.py | 2 +- selfdrive/controls/lib/longcontrol.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 18 +- selfdrive/debug/set_car_params.py | 17 +- selfdrive/loggerd/encoderd.cc | 28 +- selfdrive/manager/manager.py | 3 - selfdrive/ui/qt/maps/map.cc | 4 +- selfdrive/ui/qt/maps/map_helpers.cc | 52 +- selfdrive/ui/qt/maps/map_helpers.h | 6 +- selfdrive/ui/qt/offroad/settings.cc | 73 +- selfdrive/ui/qt/offroad/settings.h | 7 + selfdrive/ui/qt/onroad.cc | 20 +- selfdrive/ui/qt/widgets/controls.h | 22 +- selfdrive/ui/qt/widgets/prime.cc | 16 +- selfdrive/ui/qt/widgets/prime.h | 2 + selfdrive/ui/ui.cc | 1 + selfdrive/ui/ui.h | 2 +- system/camerad/SConscript | 1 + system/camerad/cameras/camera_common.cc | 6 +- system/camerad/cameras/camera_common.h | 2 +- system/camerad/cameras/camera_qcom2.cc | 370 ++++----- system/camerad/cameras/camera_qcom2.h | 37 +- system/camerad/cameras/camera_util.cc | 138 ++++ system/camerad/cameras/camera_util.h | 30 + system/camerad/cameras/real_debayer.cl | 35 + system/camerad/cameras/sensor2_i2c.h | 772 +++++++++++++++++- third_party/SConscript | 2 +- 55 files changed, 1569 insertions(+), 943 deletions(-) delete mode 100644 docs/assets/icon-star-empty.svg delete mode 100644 docs/assets/icon-star-full.svg delete mode 100644 docs/assets/icon-star-half.svg delete mode 100755 panda/board/build_all.sh delete mode 100755 panda/board/flash_h7.sh delete mode 100755 panda/board/recover_h7.sh create mode 100755 selfdrive/car/fw_query_definitions.py create mode 100644 system/camerad/cameras/camera_util.cc create mode 100644 system/camerad/cameras/camera_util.h diff --git a/cereal/car.capnp b/cereal/car.capnp index ac03a0f8abbd9a..aeac8f071d9c36 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -448,6 +448,7 @@ struct CarParams { enableApgs @6 :Bool; # advanced parking guidance system enableBsm @56 :Bool; # blind spot monitoring flags @64 :UInt32; # flags for car specific quirks + experimentalLongitudinalAvailable @71 :Bool; minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; @@ -457,14 +458,7 @@ struct CarParams { # Car docs fields maxLateralAccel @68 :Float32; - autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically - - steerMaxBPDEPRECATED @11 :List(Float32); - steerMaxVDEPRECATED @12 :List(Float32); - gasMaxBPDEPRECATED @13 :List(Float32); - gasMaxVDEPRECATED @14 :List(Float32); - brakeMaxBPDEPRECATED @15 :List(Float32); - brakeMaxVDEPRECATED @16 :List(Float32); + autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically # things about the car in the manual mass @17 :Float32; # [kg] curb weight: all fluids no cargo @@ -523,18 +517,17 @@ struct CarParams { safetyParam2DEPRECATED @2 :UInt32; } - mdpsBus @71: Int8; - sasBus @72: Int8; - sccBus @73: Int8; - enableAutoHold @74 :Bool; - hasScc13 @75 :Bool; - hasScc14 @76 :Bool; - hasEms @77 :Bool; - hasLfaHda @78 :Bool; - steerFaultMaxAngle @79 :Int16; - steerFaultMaxFrames @80 :Int16; - - disableLateralLiveTuning @81 :Bool; + mdpsBus @72: Int8; + sasBus @73: Int8; + sccBus @74: Int8; + enableAutoHold @75 :Bool; + hasScc13 @76 :Bool; + hasScc14 @77 :Bool; + hasEms @78 :Bool; + hasLfaHda @79 :Bool; + steerFaultMaxAngle @80 :Int16; + steerFaultMaxFrames @81 :Int16; + disableLateralLiveTuning @82 :Bool; struct LateralParams { torqueBP @0 :List(Int32); @@ -659,7 +652,7 @@ struct CarParams { enum Ecu { eps @0; - esp @1; + abs @1; fwdRadar @2; fwdCamera @3; engine @4; @@ -707,4 +700,10 @@ struct CarParams { minSpeedCanDEPRECATED @51 :Float32; communityFeatureDEPRECATED @46: Bool; startingAccelRateDEPRECATED @53 :Float32; + steerMaxBPDEPRECATED @11 :List(Float32); + steerMaxVDEPRECATED @12 :List(Float32); + gasMaxBPDEPRECATED @13 :List(Float32); + gasMaxVDEPRECATED @14 :List(Float32); + brakeMaxBPDEPRECATED @15 :List(Float32); + brakeMaxVDEPRECATED @16 :List(Float32); } diff --git a/common/params.cc b/common/params.cc index 726ec20b07ea66..de4efae764aa96 100644 --- a/common/params.cc +++ b/common/params.cc @@ -93,6 +93,7 @@ std::unordered_map keys = { {"CarBatteryCapacity", PERSISTENT}, {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, + {"CarParamsPersistent", PERSISTENT}, {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CompletedTrainingVersion", PERSISTENT}, {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, @@ -101,8 +102,7 @@ std::unordered_map keys = { {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"DisablePowerDown", PERSISTENT}, {"EndToEndLong", PERSISTENT}, - {"DisableRadar_Allow", PERSISTENT}, - {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB + {"ExperimentalLongitudinalEnabled", PERSISTENT}, // WARNING: THIS MAY DISABLE AEB {"DisableUpdates", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT}, {"DongleId", PERSISTENT}, diff --git a/docs/CARS.md b/docs/CARS.md index 1f0e24865deffd..47694fffeecfba 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -10,7 +10,7 @@ A supported vehicle is one that just works when you install a comma three. All s |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| |Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Acura|RDX 2019-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Acura|RDX 2019-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Audi|A3 2014-19|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| |Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| |Audi|Q2 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|VW| @@ -26,77 +26,77 @@ A supported vehicle is one that just works when you install a comma three. All s |Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| -|comma|body|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None| +|comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None| |Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Genesis|G70 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Genesis|G90 2017-18|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Genesis|G90 2017-18|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |GMC|Acadia 2018[1](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| -|Honda|Accord 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Honda|Accord Hybrid 2018-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Accord 2018-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Accord Hybrid 2018-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Honda|Civic 2019-21|All|Stock|0 mph|2 mph[2](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Civic 2019-21|All|openpilot|0 mph|2 mph[2](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Civic 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B| -|Honda|Civic Hatchback 2017-21|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B| |Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Honda|CR-V 2017-22|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Honda|CR-V Hybrid 2017-19|Honda Sensing|Stock|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Honda|e 2020|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|CR-V 2017-22|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|e 2020|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| -|Honda|Insight 2019-22|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Honda|Inspire 2018|All|Stock|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Insight 2019-22|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| +|Honda|Inspire 2018|All|openpilot|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| |Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Hyundai|Elantra 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| -|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J| |Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| |Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Hyundai|Ioniq Electric 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC) & LFA|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| +|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Hyundai|Ioniq Plug-in Hybrid 2020-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Kona 2020|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| |Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| |Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O| -|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I| +|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I| |Hyundai|Palisade 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Hyundai|Santa Fe 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D| -|Hyundai|Santa Fe 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Santa Fe Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Santa Fe Plug-in Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Santa Fe 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Santa Fe Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Hyundai|Sonata 2018-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Hyundai|Sonata 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Hyundai|Sonata Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Hyundai|Tucson Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| -|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| -|Kia|Forte 2019-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| -|Kia|K5 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| +|Kia|Forte 2019-21|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| +|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Kia|Niro Electric 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Niro Electric 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| -|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| +|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| +|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Kia|Niro Plug-in Hybrid 2018-19|Smart Cruise Control (SCC) & LKAS|openpilot|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Optima 2017|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B| |Kia|Optima 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| -|Kia|Seltos 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Kia|Sorento 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Sorento 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Kia|Stinger 2018-20|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| diff --git a/docs/assets/icon-star-empty.svg b/docs/assets/icon-star-empty.svg deleted file mode 100644 index 5d3c32d6711aba..00000000000000 --- a/docs/assets/icon-star-empty.svg +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - image/svg+xml - - - - - - - - diff --git a/docs/assets/icon-star-full.svg b/docs/assets/icon-star-full.svg deleted file mode 100644 index 294db2b7f27c01..00000000000000 --- a/docs/assets/icon-star-full.svg +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - image/svg+xml - - - - - - - - diff --git a/docs/assets/icon-star-half.svg b/docs/assets/icon-star-half.svg deleted file mode 100644 index ab905fddcb4de2..00000000000000 --- a/docs/assets/icon-star-half.svg +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - image/svg+xml - - - - - - - - - - diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index 8ea4e5ef9d2da6..471958c0608255 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -21,7 +21,7 @@ int64_t get_raw_value(const std::vector &msg, const Signal &sig) { int msb = (int)(sig.msb / 8) == i ? sig.msb : (i+1)*8 - 1; int size = msb - lsb + 1; - uint8_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1); + uint64_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1); ret |= d << (bits - size); bits -= size; @@ -287,10 +287,12 @@ void CANParser::UpdateValid(uint64_t sec) { const bool missing = state.last_seen_nanos == 0; const bool timed_out = (sec - state.last_seen_nanos) > state.check_threshold; if (state.check_threshold > 0 && (missing || timed_out)) { - if (missing) { - LOGE("0x%X MISSING", state.address); - } else if (show_missing) { - LOGE("0x%X TIMEOUT", state.address); + if (show_missing && !bus_timeout) { + if (missing) { + LOGE("0x%X NOT SEEN", state.address); + } else if (timed_out) { + LOGE("0x%X TIMED OUT", state.address); + } } _valid = false; } diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc index 83c5f833f849c0..7ac9e71a367907 100644 --- a/opendbc/hyundai_canfd.dbc +++ b/opendbc/hyundai_canfd.dbc @@ -67,6 +67,8 @@ BO_ 96 ESP_STATUS: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ ESP_DISABLED : 42|3@1+ (1,0) [0|63] "" XXX + SG_ BRAKE_POSITION : 128|10@1+ (1,0) [0|65535] "" XXX + SG_ BRAKE_PRESSED : 148|1@1+ (1,0) [0|3] "" XXX BO_ 101 BRAKE: 32 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/panda/board/build_all.sh b/panda/board/build_all.sh deleted file mode 100755 index 1c3a92de44f15c..00000000000000 --- a/panda/board/build_all.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env sh -set -e - -scons -u -j$(nproc) -PEDAL=1 scons -u -j$(nproc) -PEDAL=1 PEDAL_USB=1 scons -u -j$(nproc) diff --git a/panda/board/drivers/fan.h b/panda/board/drivers/fan.h index 9d33df70af4bfe..5fc2c52779e49e 100644 --- a/panda/board/drivers/fan.h +++ b/panda/board/drivers/fan.h @@ -37,7 +37,9 @@ void fan_tick(void){ if (fan_state.stall_counter > FAN_STALL_THRESHOLD) { // Stall detected, power cycling fan controller current_board->set_fan_enabled(false); - fan_state.error_integral = 0U; + + // clip integral, can't fully reset otherwise we may always be stuck in stall detection + fan_state.error_integral = MIN(50.0f, MAX(0.0f, fan_state.error_integral)); } } else { fan_state.stall_counter = 0U; diff --git a/panda/board/flash_h7.sh b/panda/board/flash_h7.sh deleted file mode 100755 index 6fb28d2b80c535..00000000000000 --- a/panda/board/flash_h7.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env sh -set -e - -scons -u -j$(nproc) -PYTHONPATH=.. python3 -c "from python import Panda; Panda().flash('obj/panda_h7.bin.signed')" diff --git a/panda/board/recover_h7.sh b/panda/board/recover_h7.sh deleted file mode 100755 index c39a24ec7b74f4..00000000000000 --- a/panda/board/recover_h7.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env sh -set -e - -DFU_UTIL="dfu-util" - -scons -u -j$(nproc) - -PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true -sleep 1 -$DFU_UTIL -d 0483:df11 -a 0 -s 0x08020000 -D obj/panda_h7.bin.signed -$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.panda_h7.bin diff --git a/panda/board/safety.h b/panda/board/safety.h index 32d80cf5f23399..d2100b0fc71e72 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -299,7 +299,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { rt_torque_last = 0; ts_angle_last = 0; desired_angle_last = 0; - ts_last = 0; + ts_torque_check_last = 0; + ts_steer_req_mismatch_last = 0; + valid_steer_req_count = 0; torque_meas.max = 0; torque_meas.max = 0; @@ -473,10 +475,10 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi violation |= rt_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); if (ts_elapsed > limits.max_rt_interval) { rt_torque_last = desired_torque; - ts_last = ts; + ts_torque_check_last = ts; } } @@ -485,16 +487,40 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi violation = true; } - // no torque if request bit isn't high - if ((steer_req == 0) && (desired_torque != 0)) { - violation = true; + // certain safety modes set their steer request bit low for one frame at a + // predefined max frequency to avoid steering faults in certain situations + bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0); + if (steer_req_mismatch) { + // no torque if request bit isn't high + if (!limits.has_steer_req_tolerance) { + violation = true; + + } else { + // disallow torque cut if not enough recent matching steer_req messages + if (valid_steer_req_count < limits.min_valid_request_frames) { + violation = true; + } + + // or we've cut torque too recently in time + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last); + if (ts_elapsed < limits.min_valid_request_rt_interval) { + violation = true; + } + + valid_steer_req_count = 0; + ts_steer_req_mismatch_last = ts; + } + } else { + valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames); } // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { + valid_steer_req_count = 0; desired_torque_last = 0; rt_torque_last = 0; - ts_last = ts; + ts_torque_check_last = ts; + ts_steer_req_mismatch_last = ts; } return violation; diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h index 36e9174c6f0a8e..44d5d3577b22e1 100644 --- a/panda/board/safety/safety_hyundai_community.h +++ b/panda/board/safety/safety_hyundai_community.h @@ -198,57 +198,10 @@ static int hyundai_community_tx_hook(CANPacket_t *to_send, bool longitudinal_all // LKA STEER: safety check if (addr == 832) { - LKAS11_op = 20; - int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024; - uint32_t ts = microsecond_timer_get(); - bool violation = 0; + int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ffU) - 1024U; + bool steer_req = GET_BIT(to_send, 27U) != 0U; - if (controls_allowed) { - // *** global torque limit check *** - bool torque_check = 0; - violation |= torque_check = max_limit_check(desired_torque, HYUNDAI_STEERING_LIMITS.max_steer, -HYUNDAI_STEERING_LIMITS.max_steer); - if (torque_check) { - puts(" LKAS TX not allowed: torque limit check failed!\n");} - - // *** torque rate limit check *** - bool torque_rate_check = 0; - violation |= torque_rate_check = driver_limit_check(desired_torque, desired_torque_last, &torque_driver, - HYUNDAI_STEERING_LIMITS.max_steer, HYUNDAI_STEERING_LIMITS.max_rate_up, HYUNDAI_STEERING_LIMITS.max_rate_down, - HYUNDAI_STEERING_LIMITS.driver_torque_allowance, HYUNDAI_STEERING_LIMITS.driver_torque_factor); - if (torque_rate_check) { - puts(" LKAS TX not allowed: torque rate limit check failed!\n");} - - // used next time - desired_torque_last = desired_torque; - - // *** torque real time rate limit check *** - bool torque_rt_check = 0; - violation |= torque_rt_check = rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_STEERING_LIMITS.max_rt_delta); - if (torque_rt_check) { - puts(" LKAS TX not allowed: torque real time rate limit check failed!\n");} - - // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); - if (ts_elapsed > HYUNDAI_STEERING_LIMITS.max_rt_interval) { - rt_torque_last = desired_torque; - ts_last = ts; - } - } - - // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { - violation = 1; - puts(" LKAS torque not allowed: controls not allowed!\n"); - } - - // reset to 0 if either controls is not allowed or there's a violation - if (!controls_allowed) { // a reset worsen the issue of Panda blocking some valid LKAS messages - desired_torque_last = 0; - rt_torque_last = 0; - ts_last = ts; - } - - if (violation) { + if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_STEERING_LIMITS)) { tx = 0; } } diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 3beeecc6b0bfea..fdccc436bfd191 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -46,6 +46,11 @@ typedef struct { // motor torque limits const int max_torque_error; + + // safety around steer req bit + const int min_valid_request_frames; + const uint32_t min_valid_request_rt_interval; + const bool has_steer_req_tolerance; } SteeringLimits; typedef struct { @@ -143,9 +148,11 @@ int cruise_button_prev = 0; // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque int rt_torque_last = 0; // last desired torque for real time check +int valid_steer_req_count = 0; // counter for steer request bit matching non-zero torque struct sample_t torque_meas; // last 6 motor torques produced by the eps struct sample_t torque_driver; // last 6 driver torques measured -uint32_t ts_last = 0; +uint32_t ts_torque_check_last = 0; +uint32_t ts_steer_req_mismatch_last = 0; // last timestamp steer req was mismatched with torque // state for controls_allowed timeout logic bool heartbeat_engaged = false; // openpilot enabled, passed in heartbeat USB command diff --git a/release/files_common b/release/files_common index ad99af752f815f..be0a4f0f03ba9e 100644 --- a/release/files_common +++ b/release/files_common @@ -101,6 +101,7 @@ selfdrive/car/interfaces.py selfdrive/car/vin.py selfdrive/car/disable_ecu.py selfdrive/car/fw_versions.py +selfdrive/car/fw_query_definitions.py selfdrive/car/ecu_addrs.py selfdrive/car/isotp_parallel_query.py selfdrive/car/tests/__init__.py diff --git a/release/files_tici b/release/files_tici index 8e3249cdfd55fb..c8abd720d5d637 100644 --- a/release/files_tici +++ b/release/files_tici @@ -9,6 +9,8 @@ selfdrive/assets/training_wide/* system/camerad/cameras/camera_qcom2.cc system/camerad/cameras/camera_qcom2.h +system/camerad/cameras/camera_util.cc +system/camerad/cameras/camera_util.h system/camerad/cameras/real_debayer.cl selfdrive/sensord/rawgps/* diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 7deacc0ba112eb..abacf5aebbce5d 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -175,14 +175,14 @@ def get_car(logcan, sendcan): cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" - disable_radar = Params().get_bool("DisableRadar") + experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled") selected_car = Params().get("SelectedCar") if selected_car: candidate = selected_car.decode("utf-8") CarInterface, CarController, CarState = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_radar) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py new file mode 100755 index 00000000000000..c3b74da92049e5 --- /dev/null +++ b/selfdrive/car/fw_query_definitions.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import capnp +from dataclasses import dataclass, field +import struct +from typing import Dict, List + +import panda.python.uds as uds + + +def p16(val): + return struct.pack("!H", val) + + +class StdQueries: + # FW queries + TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0]) + TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0]) + + SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT]) + SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) + + DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.DEFAULT]) + DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) + + EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) + EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, + uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) + + MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + + OBD_VERSION_REQUEST = b'\x09\x04' + OBD_VERSION_RESPONSE = b'\x49\x04' + + # VIN queries + OBD_VIN_REQUEST = b'\x09\x02' + OBD_VIN_RESPONSE = b'\x49\x02\x01' + + UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN) + + +@dataclass +class Request: + request: List[bytes] + response: List[bytes] + whitelist_ecus: List[int] = field(default_factory=list) + rx_offset: int = 0x8 + bus: int = 1 + + +@dataclass +class FwQueryConfig: + requests: List[Request] + # Overrides and removes from essential ecus for specific models and ecus (exact matching) + non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 4213b1a2121d26..9c0c406f14991c 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,9 +1,7 @@ #!/usr/bin/env python3 -import struct import traceback from collections import defaultdict -from dataclasses import dataclass, field -from typing import Any, List, Optional, Set, Tuple +from typing import Any, Optional, Set, Tuple from tqdm import tqdm import panda.python.uds as uds @@ -15,233 +13,13 @@ from system.swaglog import cloudlog Ecu = car.CarParams.Ecu -ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] +ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] +FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True) +VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True) -def p16(val): - return struct.pack("!H", val) - - -TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0]) -TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0]) - -SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT]) -SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40]) - -DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, - uds.SESSION_TYPE.DEFAULT]) -DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, - uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4]) - -EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, - uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC]) -EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, - uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4]) - -UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) -UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) - - -HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf100) # Long description -HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ - p16(0xf100) -HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) - - -TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01' -TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01' - -VOLKSWAGEN_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) -VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) - -OBD_VERSION_REQUEST = b'\x09\x04' -OBD_VERSION_RESPONSE = b'\x49\x04' - -DEFAULT_RX_OFFSET = 0x8 -VOLKSWAGEN_RX_OFFSET = 0x6a - -MAZDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -MAZDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - -NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xc0]) -NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xc0]) - -NISSAN_VERSION_REQUEST_KWP = b'\x21\x83' -NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83' - -NISSAN_VERSION_REQUEST_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -NISSAN_VERSION_RESPONSE_STANDARD = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - -NISSAN_RX_OFFSET = 0x20 - -SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) -SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) - -CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(0xf132) -CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(0xf132) - -CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) -CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER) - -CHRYSLER_RX_OFFSET = -0x280 - -FORD_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) -FORD_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) - - -@dataclass -class Request: - brand: str - request: List[bytes] - response: List[bytes] - whitelist_ecus: List[int] = field(default_factory=list) - rx_offset: int = DEFAULT_RX_OFFSET - bus: int = 1 - - -REQUESTS: List[Request] = [ - # Subaru - Request( - "subaru", - [TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], - ), - # Hyundai - Request( - "hyundai", - [HYUNDAI_VERSION_REQUEST_LONG], - [HYUNDAI_VERSION_RESPONSE], - ), - Request( - "hyundai", - [HYUNDAI_VERSION_REQUEST_MULTI], - [HYUNDAI_VERSION_RESPONSE], - ), - # Honda - Request( - "honda", - [UDS_VERSION_REQUEST], - [UDS_VERSION_RESPONSE], - ), - # Toyota - Request( - "toyota", - [SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST], - [SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE], - bus=0, - ), - Request( - "toyota", - [SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST], - [SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE], - bus=0, - ), - Request( - "toyota", - [TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE], - bus=0, - ), - # Volkswagen - Request( - "volkswagen", - [VOLKSWAGEN_VERSION_REQUEST_MULTI], - [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], - rx_offset=VOLKSWAGEN_RX_OFFSET, - ), - Request( - "volkswagen", - [VOLKSWAGEN_VERSION_REQUEST_MULTI], - [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.transmission], - ), - # Mazda - Request( - "mazda", - [MAZDA_VERSION_REQUEST], - [MAZDA_VERSION_RESPONSE], - ), - # Nissan - Request( - "nissan", - [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], - ), - Request( - "nissan", - [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], - rx_offset=NISSAN_RX_OFFSET, - ), - Request( - "nissan", - [NISSAN_VERSION_REQUEST_STANDARD], - [NISSAN_VERSION_RESPONSE_STANDARD], - rx_offset=NISSAN_RX_OFFSET, - ), - # Body - Request( - "body", - [TESTER_PRESENT_REQUEST, UDS_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, UDS_VERSION_RESPONSE], - bus=0, - ), - # Chrysler / FCA / Stellantis - Request( - "chrysler", - [CHRYSLER_VERSION_REQUEST], - [CHRYSLER_VERSION_RESPONSE], - whitelist_ecus=[Ecu.esp, Ecu.eps, Ecu.srs, Ecu.gateway, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter], - rx_offset=CHRYSLER_RX_OFFSET, - ), - Request( - "chrysler", - [CHRYSLER_VERSION_REQUEST], - [CHRYSLER_VERSION_RESPONSE], - whitelist_ecus=[Ecu.esp, Ecu.hcp, Ecu.engine, Ecu.transmission], - ), - Request( - "chrysler", - [CHRYSLER_SOFTWARE_VERSION_REQUEST], - [CHRYSLER_SOFTWARE_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.transmission], - ), - # Ford - Request( - "ford", - [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine], - ), - Request( - "ford", - [TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST], - [TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE], - bus=0, - whitelist_ecus=[Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera], - ), -] +MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e} +REQUESTS = [(brand, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests] def chunks(l, n=128): @@ -260,9 +38,8 @@ def build_fw_dict(fw_versions, filter_brand=None): def get_brand_addrs(): - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) brand_addrs = defaultdict(set) - for brand, cars in versions.items(): + for brand, cars in VERSIONS.items(): for fw in cars.values(): brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()} return brand_addrs @@ -324,13 +101,19 @@ def match_fw_to_car_exact(fw_versions_dict): for candidate, fws in candidates.items(): for ecu, expected_versions in fws.items(): + config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]] ecu_type = ecu[0] addr = ecu[1:] + found_versions = fw_versions_dict.get(addr, set()) + if not len(found_versions): + # Some models can sometimes miss an ecu, or show on two different addresses + if candidate in config.non_essential_ecus.get(ecu_type, []): + continue - # Ignore non essential ecus - if ecu_type not in ESSENTIAL_ECUS and not len(found_versions): - continue + # Ignore non essential ecus + if ecu_type not in ESSENTIAL_ECUS: + continue # Virtual debug ecu doesn't need to match the database if ecu_type == Ecu.debug: @@ -351,11 +134,10 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True): if allow_fuzzy: exact_matches.append((False, match_fw_to_car_fuzzy)) - brands = get_interface_attr('FW_VERSIONS', ignore_none=True).keys() for exact_match, match_func in exact_matches: # For each brand, attempt to fingerprint using all FW returned from its queries matches = set() - for brand in brands: + for brand in VERSIONS.keys(): fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand) matches |= match_func(fw_versions_dict) @@ -369,13 +151,9 @@ def get_present_ecus(logcan, sendcan): queries = list() parallel_queries = list() responses = set() - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) - - for r in REQUESTS: - if r.brand not in versions: - continue - for brand_versions in versions[r.brand].values(): + for brand, r in REQUESTS: + for brand_versions in VERSIONS[brand].values(): for ecu_type, addr, sub_addr in brand_versions: # Only query ecus in whitelist if whitelist is not empty if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus: @@ -404,9 +182,9 @@ def get_brand_ecu_matches(ecu_rx_addrs): """Returns dictionary of brands and matches with ECUs in their FW versions""" brand_addrs = get_brand_addrs() - brand_matches = {r.brand: set() for r in REQUESTS} + brand_matches = {brand: set() for brand, _ in REQUESTS} - brand_rx_offsets = set((r.brand, r.rx_offset) for r in REQUESTS) + brand_rx_offsets = set((brand, r.rx_offset) for brand, r in REQUESTS) for addr, sub_addr, _ in ecu_rx_addrs: # Since we can't know what request an ecu responded to, add matches for all possible rx offsets for brand, rx_offset in brand_rx_offsets: @@ -435,8 +213,8 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, debug=Fa def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, debug=False, progress=False): - versions = get_interface_attr('FW_VERSIONS', ignore_none=True) - if query_brand is not None and query_brand in versions.keys(): + versions = VERSIONS.copy() + if query_brand is not None: versions = {query_brand: versions[query_brand]} if extra is not None: @@ -466,12 +244,12 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, # Get versions and build capnp list to put into CarParams car_fw = [] - requests = [r for r in REQUESTS if query_brand is None or r.brand == query_brand] + requests = [(brand, r) for brand, r in REQUESTS if query_brand is None or brand == query_brand] for addr in tqdm(addrs, disable=not progress): for addr_chunk in chunks(addr): - for r in requests: + for brand, r in requests: try: - addrs = [(a, s) for (b, a, s) in addr_chunk if b in (r.brand, 'any') and + addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] if addrs: @@ -479,12 +257,12 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, for (addr, rx_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() - f.ecu = ecu_types.get((r.brand, addr[0], addr[1]), Ecu.unknown) + f.ecu = ecu_types.get((brand, addr[0], addr[1]), Ecu.unknown) f.fwVersion = version f.address = addr[0] f.responseAddress = rx_addr f.request = r.request - f.brand = r.brand + f.brand = brand f.bus = r.bus if addr[1] is not None: diff --git a/selfdrive/car/hyundai/scc_smoother.py b/selfdrive/car/hyundai/scc_smoother.py index 4add09c3bae072..060cef8bf85fed 100644 --- a/selfdrive/car/hyundai/scc_smoother.py +++ b/selfdrive/car/hyundai/scc_smoother.py @@ -63,13 +63,10 @@ def kph_to_clu(self, kph): def __init__(self): - params = Params() + self.params = Params() + self.read_param() - self.longcontrol = params.get_bool('LongControlEnabled') - self.slow_on_curves = params.get_bool('SccSmootherSlowOnCurves') - self.sync_set_speed_while_gas_pressed = params.get_bool('SccSmootherSyncGasPressed') - self.is_metric = params.get_bool('IsMetric') - self.e2e_long = params.get_bool('EndToEndLong') + self.param_read_counter = 0 self.speed_conv_to_ms = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS self.speed_conv_to_clu = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH @@ -99,6 +96,13 @@ def __init__(self): self.curve_speed_ms = 0. self.stock_weight = 0. + def read_param(self): + self.longcontrol = self.params.get_bool('LongControlEnabled') + self.slow_on_curves = self.params.get_bool('SccSmootherSlowOnCurves') + self.sync_set_speed_while_gas_pressed = self.params.get_bool('SccSmootherSyncGasPressed') + self.is_metric = self.params.get_bool('IsMetric') + self.e2e_long = self.params.get_bool('EndToEndLong') + def reset(self): self.wait_timer = 0 @@ -202,6 +206,10 @@ def cal_max_speed(self, frame, CC, CS, sm, clu11_speed, controls): def update(self, enabled, can_sends, packer, CC, CS, frame, controls): + if self.param_read_counter % 100 == 0: + self.read_param() + self.param_read_counter += 1 + # mph or kph clu11_speed = CS.clu11["CF_Clu_Vanz"] diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 5f9d5a46b018a7..42e91316cc0a5b 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,7 +1,10 @@ from enum import IntFlag from cereal import car +from panda.python import uds from selfdrive.car import dbc_dict +from selfdrive.car.fw_query_definitions import FwQueryConfig, p16, Request + Ecu = car.CarParams.Ecu class CarControllerParams: @@ -341,9 +344,30 @@ class Buttons: }], } +HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf100) # Long description +HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ + p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ + p16(0xf100) +HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + +FW_QUERY_CONFIG = FwQueryConfig( + requests=[ + Request( + [HYUNDAI_VERSION_REQUEST_LONG], + [HYUNDAI_VERSION_RESPONSE], + ), + Request( + [HYUNDAI_VERSION_REQUEST_MULTI], + [HYUNDAI_VERSION_RESPONSE], + ), + ], +) + FW_VERSIONS = { CAR.EV6: { - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x8758520CV100\xf1\x00CV IEB \x02 101!\x10\x18 58520-CV100', ], (Ecu.eps, 0x7d4, None): [ @@ -358,7 +382,7 @@ class Buttons: ], }, CAR.IONIQ_5: { - (Ecu.esp, 0x7d1, None): [ + (Ecu.abs, 0x7d1, None): [ b'\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', b'\xf1\x8758520GI010\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010', ], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index d199b6fb4b68fa..ee1470f7cde9ae 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -86,7 +86,7 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed): @staticmethod @abstractmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): pass @staticmethod diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index c7cedbc264de38..cf9b4c454a83ff 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -33,7 +33,7 @@ def compute_gb(accel, speed): return accel @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "mock" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 14ee7e223962f8..fcc762c2b85750 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan] # Guess FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan] +FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan] FORD FOCUS 4TH GEN: [.nan, 1.5, .nan] ### diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index ae3aa675c763dd..fba0c54eba165f 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,19 +1,12 @@ #!/usr/bin/env python3 import re -import struct import traceback import cereal.messaging as messaging -import panda.python.uds as uds from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery +from selfdrive.car.fw_query_definitions import StdQueries from system.swaglog import cloudlog -OBD_VIN_REQUEST = b'\x09\x02' -OBD_VIN_RESPONSE = b'\x49\x02\x01' - -UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) -UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + struct.pack("!H", uds.DATA_IDENTIFIER_TYPE.VIN) - VIN_UNKNOWN = "0" * 17 VIN_RE = "[A-HJ-NPR-Z0-9]{17}" @@ -25,7 +18,7 @@ def is_valid_vin(vin: str): def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1] # engine, VMCU, 29-bit engine, PGM-FI for i in range(retry): - for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE), (OBD_VIN_REQUEST, OBD_VIN_RESPONSE)): + for request, response in ((StdQueries.UDS_VIN_REQUEST, StdQueries.UDS_VIN_RESPONSE), (StdQueries.OBD_VIN_REQUEST, StdQueries.OBD_VIN_RESPONSE)): try: query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], debug=debug) for (addr, rx_addr), vin in query.get_data(timeout).items(): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 88e80fab2b17fa..f291112a1a8e1f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -12,7 +12,7 @@ from common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE from system.swaglog import cloudlog -from system.version import get_short_branch +from system.version import is_tested_branch, get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET @@ -136,10 +136,14 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] + if is_tested_branch(): + self.CP.experimentalLongitudinalAvailable = False + # Write CarParams for radard cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) + put_nonblocking("CarParamsPersistent", cp_bytes) self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 30244f197bd189..1fc98cbf678d71 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -8,7 +8,7 @@ LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection -LANE_CHANGE_SPEED_MIN = 60 * CV.KPH_TO_MS +LANE_CHANGE_SPEED_MIN = 25 * CV.KPH_TO_MS LANE_CHANGE_TIME_MAX = 10. DESIRES = { diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 52f81e7078fe9a..a95ea839359ac5 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -96,6 +96,7 @@ def update(self, active, CS, long_plan, accel_limits, t_since_plan, radar_state) v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds) else: v_target = 0.0 + v_target_now = 0.0 v_target_1sec = 0.0 a_target = 0.0 @@ -137,7 +138,6 @@ def update(self, active, CS, long_plan, accel_limits, t_since_plan, radar_state) feedforward=a_target, freeze_integrator=freeze_integrator) - self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 55747bba159447..dfb63c5ec77fa8 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -175,7 +175,7 @@ def gen_long_ocp(): x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 - ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR]) + ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, T_FOLLOW, LEAD_DANGER_FACTOR, STOP_DISTANCE]) # We put all constraint cost weights to 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4e3880eb44f8f1..c6cb78b36fd323 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -48,10 +48,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - params = Params() - # TODO read param in the loop for live toggling - mode = 'blended' if params.get_bool('EndToEndLong') else 'acc' - self.mpc = LongitudinalMpc(mode=mode) + self.params = Params() + self.param_read_counter = 0 + + self.mpc = LongitudinalMpc() + self.read_param() self.fcw = False @@ -66,6 +67,10 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.use_cluster_speed = Params().get_bool('UseClusterSpeed') + def read_param(self): + e2e = self.params.get_bool('EndToEndLong') and self.CP.openpilotLongitudinalControl + self.mpc.mode = 'blended' if e2e else 'acc' + def parse_model(self, model_msg): if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and @@ -85,8 +90,11 @@ def parse_model(self, model_msg): return x, v, a, j def update(self, sm): - v_ego = sm['carState'].vEgo + if self.param_read_counter % 100 == 0: + self.read_param() + self.param_read_counter += 1 + v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS diff --git a/selfdrive/debug/set_car_params.py b/selfdrive/debug/set_car_params.py index bcdaed07783a61..24258db9f20523 100755 --- a/selfdrive/debug/set_car_params.py +++ b/selfdrive/debug/set_car_params.py @@ -1,11 +1,22 @@ #!/usr/bin/env python3 import sys +from cereal import car from common.params import Params from tools.lib.route import Route from tools.lib.logreader import LogReader if __name__ == "__main__": - r = Route(sys.argv[1]) - cp = [m for m in LogReader(r.qlog_paths()[0]) if m.which() == 'carParams'] - Params().put("CarParams", cp[0].carParams.as_builder().to_bytes()) + CP = None + if len(sys.argv) > 1: + r = Route(sys.argv[1]) + cps = [m for m in LogReader(r.qlog_paths()[0]) if m.which() == 'carParams'] + CP = cps[0].carParams.as_builder() + else: + CP = car.CarParams.new_message() + CP.openpilotLongitudinalControl = True + CP.experimentalLongitudinalAvailable = False + + cp_bytes = CP.to_bytes() + for p in ("CarParams", "CarParamsCache", "CarParamsPersistent"): + Params().put(p, cp_bytes) diff --git a/selfdrive/loggerd/encoderd.cc b/selfdrive/loggerd/encoderd.cc index 9bd8e2f1d4422b..db5f4b61ab92c0 100644 --- a/selfdrive/loggerd/encoderd.cc +++ b/selfdrive/loggerd/encoderd.cc @@ -52,17 +52,23 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { VisionBuf buf_info = vipc_client.buffers[0]; LOGW("encoder %s init %dx%d", cam_info.filename, buf_info.width, buf_info.height); - // main encoder - encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height, - cam_info.fps, cam_info.bitrate, - cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, - buf_info.width, buf_info.height, false)); - // qcamera encoder - if (cam_info.has_qcamera) { - encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height, - qcam_info.fps, qcam_info.bitrate, - qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, - qcam_info.frame_width, qcam_info.frame_height, false)); + if (buf_info.width > 0 && buf_info.height > 0) { + // main encoder + encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height, + cam_info.fps, cam_info.bitrate, + cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + buf_info.width, buf_info.height, false)); + // qcamera encoder + if (cam_info.has_qcamera) { + encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height, + qcam_info.fps, qcam_info.bitrate, + qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264, + qcam_info.frame_width, qcam_info.frame_height, false)); + } + } else { + LOGE("not initting empty encoder"); + s->max_waiting--; + break; } } diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 85388d6ab9d63b..ba6ace68fc38d7 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -67,9 +67,6 @@ def manager_init() -> None: if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) - if not params.get_bool("DisableRadar_Allow"): - params.remove("DisableRadar") - # set unset params for k, v in default_params: if params.get(k) is None: diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 0a61dfbc36eb3c..b51733bdd2cad0 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -486,9 +486,9 @@ void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruct // for rhd, reflect direction and then flip if (is_rhd) { if (fn.contains("left")) { - fn.replace(QString("left"), QString("right")); + fn.replace("left", "right"); } else if (fn.contains("right")) { - fn.replace(QString("right"), QString("left")); + fn.replace("right", "left"); } } diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index f97137a7f90d9d..8d5d4e1715816d 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -44,61 +44,31 @@ QMapbox::CoordinatesCollections model_to_collection( for (int i = 0; i < x.size(); i++) { Eigen::Vector3d point_ecef = ecef_from_local * Eigen::Vector3d(x[i], y[i], z[i]) + ecef; Geodetic point_geodetic = ecef2geodetic((ECEF){.x = point_ecef[0], .y = point_ecef[1], .z = point_ecef[2]}); - QMapbox::Coordinate coordinate(point_geodetic.lat, point_geodetic.lon); - coordinates.push_back(coordinate); + coordinates.push_back({point_geodetic.lat, point_geodetic.lon}); } - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; + return {QMapbox::CoordinatesCollection{coordinates}}; } -QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c) { - QMapbox::Coordinates coordinates; - coordinates.push_back(c); - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; +QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c) { + QMapbox::Coordinates coordinates{c}; + return {QMapbox::CoordinatesCollection{coordinates}}; } QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader& coordinate_list) { QMapbox::Coordinates coordinates; - for (auto const &c: coordinate_list) { - QMapbox::Coordinate coordinate(c.getLatitude(), c.getLongitude()); - coordinates.push_back(coordinate); + coordinates.push_back({c.getLatitude(), c.getLongitude()}); } - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; - + return {QMapbox::CoordinatesCollection{coordinates}}; } -QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list) { +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list) { QMapbox::Coordinates coordinates; - for (auto &c : coordinate_list) { - QMapbox::Coordinate coordinate(c.latitude(), c.longitude()); - coordinates.push_back(coordinate); + coordinates.push_back({c.latitude(), c.longitude()}); } - - QMapbox::CoordinatesCollection collection; - collection.push_back(coordinates); - - QMapbox::CoordinatesCollections collections; - collections.push_back(collection); - return collections; + return {QMapbox::CoordinatesCollection{coordinates}}; } QList polyline_to_coordinate_list(const QString &polylineString) { @@ -143,7 +113,7 @@ QList polyline_to_coordinate_list(const QString &polylineString) return path; } -std::optional coordinate_from_param(std::string param) { +std::optional coordinate_from_param(const std::string ¶m) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index a78b289487f617..85cf9d9d7e8b7c 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -24,10 +24,10 @@ QMapbox::CoordinatesCollections model_to_collection( const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, const cereal::LiveLocationKalman::Measurement::Reader &positionECEF, const cereal::ModelDataV2::XYZTData::Reader &line); -QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c); +QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c); QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list); -QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); +QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList &coordinate_list); QList polyline_to_coordinate_list(const QString &polylineString); -std::optional coordinate_from_param(std::string param); +std::optional coordinate_from_param(const std::string ¶m); double angle_difference(double angle1, double angle2); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 18161ff45e9b96..36f1625de61b02 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -34,7 +34,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // param, title, desc, icon - std::vector> toggles{ + std::vector> toggle_defs{ { "OpenpilotEnabledToggle", tr("Enable openpilot"), @@ -74,9 +74,15 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { { "EndToEndLong", tr("๐ŸŒฎ End-to-end longitudinal (extremely alpha) ๐ŸŒฎ"), - tr("Let the driving model control the gas and brakes, openpilot will drive as it thinks a human would. Super experimental."), + "", "../assets/offroad/icon_road.png", }, + { + "ExperimentalLongitudinalEnabled", + tr("Experimental openpilot longitudinal control"), + tr("WARNING: openpilot longitudinal control is experimental for this car and will disable AEB."), + "../assets/offroad/icon_speed_limit.png", + }, #ifdef ENABLE_MAPS { "NavSettingTime24h", @@ -94,22 +100,61 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }; - Params params; - - if (params.getBool("DisableRadar_Allow")) { - toggles.push_back({ - "DisableRadar", - tr("openpilot Longitudinal Control"), - tr("openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB!"), - "../assets/offroad/icon_speed_limit.png", - }); - } - - for (auto &[param, title, desc, icon] : toggles) { + for (auto &[param, title, desc, icon] : toggle_defs) { auto toggle = new ParamControl(param, title, desc, icon, this); + bool locked = params.getBool((param + "Lock").toStdString()); toggle->setEnabled(!locked); + addItem(toggle); + toggles[param.toStdString()] = toggle; + } + + connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { + updateToggles(); + }); +} + +void TogglesPanel::showEvent(QShowEvent *event) { + updateToggles(); +} + +void TogglesPanel::updateToggles() { + auto e2e_toggle = toggles["EndToEndLong"]; + auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"]; + const QString e2e_description = tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental."); + + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + if (!CP.getExperimentalLongitudinalAvailable()) { + params.remove("ExperimentalLongitudinalEnabled"); + } + op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); + + const bool op_long = CP.getOpenpilotLongitudinalControl() && !CP.getExperimentalLongitudinalAvailable(); + const bool exp_long_enabled = CP.getExperimentalLongitudinalAvailable() && params.getBool("ExperimentalLongitudinalEnabled"); + if (op_long || exp_long_enabled) { + // normal description and toggle + e2e_toggle->setEnabled(true); + e2e_toggle->setDescription(e2e_description); + } else { + // no long for now + e2e_toggle->setEnabled(false); + params.remove("EndToEndLong"); + + const QString no_long = "openpilot longitudinal control is not currently available for this car."; + const QString exp_long = "Enable experimental longitudinal control to enable this."; + e2e_toggle->setDescription("" + (CP.getExperimentalLongitudinalAvailable() ? exp_long : no_long) + "

" + e2e_description); + } + + e2e_toggle->refresh(); + } else { + e2e_toggle->setDescription(e2e_description); + op_long_toggle->setVisible(false); } } diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index cc2e903297f9c4..1bf7a455928ff3 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -56,6 +56,13 @@ class TogglesPanel : public ListWidget { Q_OBJECT public: explicit TogglesPanel(SettingsWindow *parent); + void showEvent(QShowEvent *event) override; + +private: + Params params; + std::map toggles; + + void updateToggles(); }; class SoftwarePanel : public ListWidget { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 87ab1d556f9e80..cf4633666134f6 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -314,7 +314,25 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, height() / 4); - if (scene.end_to_end) { + float start_hue, end_hue; + if (scene.end_to_end_long) { + const auto &acceleration = (*s->sm)["modelV2"].getModelV2().getAcceleration(); + float acceleration_future = 0; + if (acceleration.getZ().size() > 16) { + acceleration_future = acceleration.getX()[16]; // 2.5 seconds + } + start_hue = 60; + // speed up: 120, slow down: 0 + end_hue = fmax(fmin(start_hue + acceleration_future * 30, 120), 0); + + // FIXME: painter.drawPolygon can be slow if hue is not rounded + end_hue = int(end_hue * 100 + 0.5) / 100; + + bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); + bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); + bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); + } + else if (scene.end_to_end) { const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation(); float orientation_future = 0; if (orientation.getZ().size() > 16) { diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index aed99edae8da5a..5bb69cd25cc01d 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -105,7 +105,10 @@ class ToggleControl : public AbstractControl { QObject::connect(&toggle, &Toggle::stateChanged, this, &ToggleControl::toggleFlipped); } - void setEnabled(bool enabled) { toggle.setEnabled(enabled); toggle.update(); } + void setEnabled(bool enabled) { + toggle.setEnabled(enabled); + toggle.update(); + } signals: void toggleFlipped(bool state); @@ -121,17 +124,21 @@ class ParamControl : public ToggleControl { public: ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, false, parent) { key = param.toStdString(); - QObject::connect(this, &ToggleControl::toggleFlipped, [=](bool state) { + QObject::connect(this, &ParamControl::toggleFlipped, [=](bool state) { params.putBool(key, state); }); } - void showEvent(QShowEvent *event) override { + void refresh() { if (params.getBool(key) != toggle.on) { toggle.togglePosition(); } }; + void showEvent(QShowEvent *event) override { + refresh(); + }; + private: std::string key; Params params; @@ -157,9 +164,12 @@ class ListWidget : public QWidget { QPainter p(this); p.setPen(Qt::gray); for (int i = 0; i < inner_layout.count() - 1; ++i) { - QRect r = inner_layout.itemAt(i)->geometry(); - int bottom = r.bottom() + inner_layout.spacing() / 2; - p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom); + QWidget *widget = inner_layout.itemAt(i)->widget(); + if (widget == nullptr || widget->isVisible()) { + QRect r = inner_layout.itemAt(i)->geometry(); + int bottom = r.bottom() + inner_layout.spacing() / 2; + p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom); + } } } QVBoxLayout outer_layout; diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 5419475262b338..a8ceee4ca9722f 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -18,21 +18,23 @@ using qrcodegen::QrCode; PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) { - QTimer* timer = new QTimer(this); - timer->start(5 * 60 * 1000); + timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &PairingQRWidget::refresh); } void PairingQRWidget::showEvent(QShowEvent *event) { refresh(); + timer->start(5 * 60 * 1000); +} + +void PairingQRWidget::hideEvent(QHideEvent *event) { + timer->stop(); } void PairingQRWidget::refresh() { - if (isVisible()) { - QString pairToken = CommaApi::create_jwt({{"pair", true}}); - QString qrString = "https://connect.comma.ai/?pair=" + pairToken; - this->updateQrCode(qrString); - } + QString pairToken = CommaApi::create_jwt({{"pair", true}}); + QString qrString = "https://connect.comma.ai/?pair=" + pairToken; + this->updateQrCode(qrString); } void PairingQRWidget::updateQrCode(const QString &text) { diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h index ce4baecfa28a73..0a1d93250d96c4 100644 --- a/selfdrive/ui/qt/widgets/prime.h +++ b/selfdrive/ui/qt/widgets/prime.h @@ -25,8 +25,10 @@ class PairingQRWidget : public QWidget { private: QPixmap img; + QTimer *timer; void updateQrCode(const QString &text); void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; private slots: void refresh(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8ec65857378cf3..cae9acd152f4c0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -198,6 +198,7 @@ void ui_update_params(UIState *s) { auto params = Params(); s->scene.is_metric = params.getBool("IsMetric"); s->scene.map_on_left = params.getBool("NavSettingLeftSide"); + s->scene.end_to_end_long = params.getBool("EndToEndLong"); s->show_debug = params.getBool("ShowDebugUI"); s->lat_control = std::string(Params().get("LateralControl")); } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index d370d54cc67e66..64a598ae413696 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -102,7 +102,7 @@ typedef struct UIScene { bool lead_radar[2] = {false, false}; float light_sensor, accel_sensor, gyro_sensor; - bool started, ignition, is_metric, map_on_left, longitudinal_control, end_to_end; + bool started, ignition, is_metric, map_on_left, longitudinal_control, end_to_end, end_to_end_long; uint64_t started_frame; } UIScene; diff --git a/system/camerad/SConscript b/system/camerad/SConscript index b03fbdc9a084c1..65f682dcce652f 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -10,6 +10,7 @@ if arch == "larch64": env.Program('camerad', [ 'main.cc', 'cameras/camera_common.cc', + 'cameras/camera_util.cc', 'imgproc/utils.cc', cameras, ], LIBS=libs) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 1d4ecd526a041a..313af00b5a91ce 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -36,10 +36,10 @@ class Debayer { "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " - "-DCAM_NUM=%d%s", + "-DIS_OX=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, - s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); @@ -348,8 +348,8 @@ void camerad_thread() { MultiCameraState cameras = {}; VisionIpcServer vipc_server("camerad", device_id, context); - cameras_init(&vipc_server, &cameras, device_id, context); cameras_open(&cameras); + cameras_init(&vipc_server, &cameras, device_id, context); vipc_server.start_listener(); diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index e87eaf4b91f2a4..7bbb13c75fc024 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -23,7 +23,7 @@ #define CAMERA_ID_LGC920 6 #define CAMERA_ID_LGC615 7 #define CAMERA_ID_AR0231 8 -#define CAMERA_ID_IMX390 9 +#define CAMERA_ID_OX03C10 9 #define CAMERA_ID_MAX 10 const int YUV_BUFFER_COUNT = 40; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index a86218c06e8448..2f674cb50c4432 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -32,7 +32,8 @@ const size_t FRAME_HEIGHT = 1208; const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) const size_t AR0231_REGISTERS_HEIGHT = 2; -const size_t AR0231_STATS_HEIGHT = 2; +// TODO: this extra height is universal and doesn't apply per camera +const size_t AR0231_STATS_HEIGHT = 2+8; const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py @@ -47,153 +48,60 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_offset = AR0231_REGISTERS_HEIGHT, .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, }, - [CAMERA_ID_IMX390] = { + [CAMERA_ID_OX03C10] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, + .frame_stride = FRAME_STRIDE, // (0xa80*12//8) + .extra_height = 16, // this right? }, }; -const float DC_GAIN = 2.5; -const float sensor_analog_gains[] = { +const float DC_GAIN_AR0231 = 2.5; +const float DC_GAIN_OX03C10 = 7.32; + +const float DC_GAIN_ON_GREY_AR0231= 0.2; +const float DC_GAIN_OFF_GREY_AR0231 = 0.3; +const float DC_GAIN_ON_GREY_OX03C10= 0.3; +const float DC_GAIN_OFF_GREY_OX03C10 = 0.375; + +const int DC_GAIN_MIN_WEIGHT = 0; +const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 32; + +const float sensor_analog_gains_AR0231[] = { 1.0/8.0, 2.0/8.0, 2.0/7.0, 3.0/7.0, // 0, 1, 2, 3 3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, // 4, 5, 6, 7 5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, // 8, 9, 10, 11 7.0/2.0, 8.0/2.0, 8.0/1.0}; // 12, 13, 14, 15 = bypass -const int ANALOG_GAIN_MIN_IDX = 0x1; // 0.25x -const int ANALOG_GAIN_REC_IDX = 0x6; // 0.8x -const int ANALOG_GAIN_MAX_IDX = 0xD; // 4.0x - -const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss -const int EXPOSURE_TIME_MAX = 0x0855; // with HDR, slowest ss, 40ms - -// ************** low level camera helpers **************** -int do_cam_control(int fd, int op_code, void *handle, int size) { - struct cam_control camcontrol = {0}; - camcontrol.op_code = op_code; - camcontrol.handle = (uint64_t)handle; - if (size == 0) { - camcontrol.size = 8; - camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; - } else { - camcontrol.size = size; - camcontrol.handle_type = CAM_HANDLE_USER_POINTER; - } - - int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); - if (ret == -1) { - LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); - } - return ret; -} - -std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1) { - struct cam_acquire_dev_cmd cmd = { - .session_handle = session_handle, - .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), - .resource_hdl = (uint64_t)data, - }; - int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); - return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; -}; - -int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { - struct cam_config_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = dev_handle, - .packet_handle = packet_handle, - }; - return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); -} - -int device_control(int fd, int op_code, int session_handle, int dev_handle) { - // start stop and release are all the same - struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; - return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); -} +// similar gain curve to AR +const float sensor_analog_gains_OX03C10[] = { + 1.0, 1.25, 1.3125, 1.5625, + 1.6875, 2.0, 2.25, 2.625, + 3.125, 3.625, 4.5, 5.0, + 7.25, 8.5, 12.0, 15.5}; -void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, - int mmu_hdl = 0, int mmu_hdl2 = 0) { - struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; - mem_mgr_alloc_cmd.len = len; - mem_mgr_alloc_cmd.align = align; - mem_mgr_alloc_cmd.flags = flags; - mem_mgr_alloc_cmd.num_hdl = 0; - if (mmu_hdl != 0) { - mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; - mem_mgr_alloc_cmd.num_hdl++; - } - if (mmu_hdl2 != 0) { - mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; - mem_mgr_alloc_cmd.num_hdl++; - } +const uint32_t ox03c10_analog_gains_reg[] = { + 0x100, 0x140, 0x150, 0x190, + 0x1B0, 0x200, 0x240, 0x2A0, + 0x320, 0x3A0, 0x480, 0x500, + 0x740, 0x880, 0xC00, 0xF80}; - do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); - *handle = mem_mgr_alloc_cmd.out.buf_handle; +const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x +const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x +const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x - void *ptr = NULL; - if (mem_mgr_alloc_cmd.out.fd > 0) { - ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); - assert(ptr != MAP_FAILED); - } +const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; +const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x5; // 2x +const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0xF; - // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); +const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss +const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms - return ptr; -} - -void release(int video0_fd, uint32_t handle) { - int ret; - struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; - mem_mgr_release_cmd.buf_handle = handle; - - ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); - assert(ret == 0); -} - -void release_fd(int video0_fd, uint32_t handle) { - // handle to fd - close(handle>>16); - release(video0_fd, handle); -} - -void *MemoryManager::alloc(int size, uint32_t *handle) { - lock.lock(); - void *ptr; - if (!cached_allocations[size].empty()) { - ptr = cached_allocations[size].front(); - cached_allocations[size].pop(); - *handle = handle_lookup[ptr]; - } else { - ptr = alloc_w_mmu_hdl(video0_fd, size, handle); - handle_lookup[ptr] = *handle; - size_lookup[ptr] = size; - } - lock.unlock(); - return ptr; -} - -void MemoryManager::free(void *ptr) { - lock.lock(); - cached_allocations[size_lookup[ptr]].push(ptr); - lock.unlock(); -} - -MemoryManager::~MemoryManager() { - for (auto& x : cached_allocations) { - while (!x.second.empty()) { - void *ptr = x.second.front(); - x.second.pop(); - LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); - munmap(ptr, size_lookup[ptr]); - release_fd(video0_fd, handle_lookup[ptr]); - handle_lookup.erase(ptr); - size_lookup.erase(ptr); - } - } -} +const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x +const int EXPOSURE_TIME_MAX_OX03C10 = 2016; +const uint32_t VS_TIME_MIN_OX03C10 = 1; +const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; @@ -213,8 +121,8 @@ void CameraState::sensors_start() { LOGD("starting sensor %d", camera_num); if (camera_id == CAMERA_ID_AR0231) { sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - sensors_i2c(start_reg_array_imx390, std::size(start_reg_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } else { assert(false); } @@ -300,15 +208,15 @@ int CameraState::sensors_init() { switch (camera_num) { case 0: // port 0 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2 break; case 1: // port 1 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x36; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20; break; case 2: // port 2 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; break; } @@ -324,9 +232,9 @@ int CameraState::sensors_init() { if (camera_id == CAMERA_ID_AR0231) { probe->reg_addr = 0x3000; probe->expected_data = 0x354; - } else if (camera_id == CAMERA_ID_IMX390) { - probe->reg_addr = 0x330; - probe->expected_data = 0x1538; + } else if (camera_id == CAMERA_ID_OX03C10) { + probe->reg_addr = 0x300a; + probe->expected_data = 0x5803; } else { assert(false); } @@ -393,8 +301,8 @@ int CameraState::sensors_init() { power->power_settings[1].power_seq_type = 1; power->power_settings[2].power_seq_type = 3; - LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); + LOGD("probing the sensor: %d", ret); mm.free(i2c_info); mm.free(power_settings); @@ -606,12 +514,67 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id_, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, bool enabled_) { - multi_cam_state = multi_cam_state_; - camera_id = camera_id_; - camera_num = camera_num_; - enabled = enabled_; +void CameraState::camera_set_parameters() { + if (camera_id == CAMERA_ID_AR0231) { + dc_gain_factor = DC_GAIN_AR0231; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; + dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231; + dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231; + exposure_time_min = EXPOSURE_TIME_MIN_AR0231; + exposure_time_max = EXPOSURE_TIME_MAX_AR0231; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; + for (int i=0; i<=analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; + } + min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + } else if (camera_id == CAMERA_ID_OX03C10) { + dc_gain_factor = DC_GAIN_OX03C10; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; + dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10; + dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10; + exposure_time_min = EXPOSURE_TIME_MIN_OX03C10; + exposure_time_max = EXPOSURE_TIME_MAX_OX03C10; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; + for (int i=0; i<=analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; + } + min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + } else { + assert(false); + } + + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_fraction = 0.3; + + dc_gain_enabled = false; + dc_gain_weight = DC_GAIN_MIN_WEIGHT; + gain_idx = analog_gain_rec_idx; + exposure_time = 5; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time; +} + +void CameraState::camera_map_bufs(MultiCameraState *s) { + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + int ret = do_cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + enqueue_req_multi(1, FRAME_BUF_COUNT, 0); +} + +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { if (!enabled) return; + camera_id = camera_id_; LOGD("camera init %d", camera_num); assert(camera_id < std::size(cameras_supported)); @@ -621,19 +584,18 @@ void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServe request_id_last = 0; skipped = true; - min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX]; - max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN; - target_grey_fraction = 0.3; - - dc_gain_enabled = false; - gain_idx = ANALOG_GAIN_REC_IDX; - exposure_time = 5; - cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time; + camera_set_parameters(); buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); + camera_map_bufs(s); } -void CameraState::camera_open() { +void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num_, bool enabled_) { + multi_cam_state = multi_cam_state_; + camera_num = camera_num_; + enabled = enabled_; + if (!enabled) return; + int ret; sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num); assert(sensor_fd >= 0); @@ -644,10 +606,12 @@ void CameraState::camera_open() { // probe the sensor LOGD("-- Probing sensor %d", camera_num); + camera_id = CAMERA_ID_AR0231; ret = sensors_init(); if (ret != 0) { - LOGD("AR0231 init failed, trying IMX390"); - camera_id = CAMERA_ID_IMX390; + // TODO: use build flag instead? + LOGD("AR0231 init failed, trying OX03C10"); + camera_id = CAMERA_ID_OX03C10; ret = sensors_init(); } LOGD("-- Probing sensor %d done with %d", camera_num, ret); @@ -671,13 +635,18 @@ void CameraState::camera_open() { LOGD("acquire sensor dev"); LOG("-- Configuring sensor"); + uint32_t dt; if (camera_id == CAMERA_ID_AR0231) { sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - sensors_i2c(init_array_imx390, std::size(init_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + } else if (camera_id == CAMERA_ID_OX03C10) { + sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + // one is 0x2a, two are 0x2b + dt = 0x2c; } else { assert(false); } + printf("dt is %x\n", dt); // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c // If you don't do this, the strobe GPIO is an output (even in reset it seems!) @@ -691,7 +660,7 @@ void CameraState::camera_open() { .lane_cfg = 0x3210, .vc = 0x0, - .dt = 0x12, // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + .dt = dt, .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? @@ -805,29 +774,15 @@ void CameraState::camera_open() { ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); LOGD("start isp: %d", ret); - for (int i = 0; i < FRAME_BUF_COUNT; i++) { - // configure ISP to put the image in place - struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; - mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu; - mem_mgr_map_cmd.num_hdl = 1; - mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE; - mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); - LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); - buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; - } - // TODO: this is unneeded, should we be doing the start i2c in a different way? //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); //LOGD("start sensor: %d", ret); - - enqueue_req_multi(1, FRAME_BUF_COUNT, 0); } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_DRIVER, !env_disable_driver); - s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_ROAD, !env_disable_road); - s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); + s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } @@ -873,11 +828,11 @@ void cameras_open(MultiCameraState *s) { ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); LOGD("req mgr subscribe: %d", ret); - s->driver_cam.camera_open(); + s->driver_cam.camera_open(s, 2, !env_disable_driver); LOGD("driver camera opened"); - s->road_cam.camera_open(); + s->road_cam.camera_open(s, 1, !env_disable_road); LOGD("road camera opened"); - s->wide_road_cam.camera_open(); + s->wide_road_cam.camera_open(s, 0, !env_disable_wide_road); LOGD("wide road camera opened"); } @@ -1044,7 +999,7 @@ void CameraState::handle_camera_event(void *evdat) { meta_data.frame_id = main_id - idx_offset; meta_data.timestamp_sof = timestamp; exp_lock.lock(); - meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac; + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); meta_data.high_conversion_gain = dc_gain_enabled; meta_data.integ_lines = exposure_time; meta_data.measured_grey_fraction = measured_grey_fraction; @@ -1096,12 +1051,17 @@ void CameraState::set_camera_exposure(float grey_frac) { // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < 0.2) { + if (!enable_dc_gain && target_grey < dc_gain_on_grey) { enable_dc_gain = true; - } else if (enable_dc_gain && target_grey > 0.3) { + dc_gain_weight = DC_GAIN_MIN_WEIGHT; + } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { enable_dc_gain = false; + dc_gain_weight = dc_gain_max_weight; } + if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > DC_GAIN_MIN_WEIGHT) {dc_gain_weight -= 1;} + std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { gain_bytes = Params().get("CameraDebugExpGain"); @@ -1119,14 +1079,14 @@ void CameraState::set_camera_exposure(float grey_frac) { } else { // Simple brute force optimizer to choose sensor parameters // to reach desired EV - for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) { - float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1); + for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { + float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX); + int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); // Only go below recommended gain when absolutely necessary to not overexpose - if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) { + if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { continue; } @@ -1134,8 +1094,8 @@ void CameraState::set_camera_exposure(float grey_frac) { float score = std::abs(desired_ev - (t * gain)) * 10; // Going below recommended gain needs lower penalty to not overexpose - float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1; - score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m; + float m = g > analog_gain_rec_idx ? 5.0 : 0.1; + score += std::abs(g - (int)analog_gain_rec_idx) * m; // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev); @@ -1160,7 +1120,7 @@ void CameraState::set_camera_exposure(float grey_frac) { exposure_time = new_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0); + float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; exp_lock.unlock(); @@ -1181,17 +1141,25 @@ void CameraState::set_camera_exposure(float grey_frac) { {0x3012, (uint16_t)exposure_time}, }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_IMX390) { - // if gain is sub 1, we have to use exposure to mimic sub 1 gains - uint32_t real_exposure_time = (gain < 1.0) ? (exposure_time*gain) : exposure_time; - // invert real_exposure_time, max exposure is 2 - real_exposure_time = (exposure_time >= 0x7cf) ? 2 : (0x7cf - exposure_time); - uint32_t real_gain = int((10*log10(fmax(1.0, gain)))/0.3); - //printf("%d expose: %d gain: %f = %d\n", camera_num, exposure_time, gain, real_gain); + } else if (camera_id == CAMERA_ID_OX03C10) { + // t_HCG + t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = std::max((dc_gain_weight * exposure_time / dc_gain_max_weight), 0); + uint32_t lcg_time = std::max(((dc_gain_max_weight - dc_gain_weight) * exposure_time / dc_gain_max_weight), 0); + uint32_t spd_time = std::max(hcg_time / 16, (uint32_t)exposure_time_min); + uint32_t vs_time = std::min(std::max(hcg_time / 64, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + + uint32_t real_gain = ox03c10_analog_gains_reg[new_g]; struct i2c_random_wr_payload exp_reg_array[] = { - {0x000c, real_exposure_time&0xFF}, {0x000d, real_exposure_time>>8}, - {0x0010, real_exposure_time&0xFF}, {0x0011, real_exposure_time>>8}, - {0x0018, real_gain&0xFF}, {0x0019, real_gain>>8}, + + {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, + {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, + {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, + {0x35c1, vs_time>>8}, {0x35c2, vs_time&0xFF}, + + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + {0x3588, real_gain>>8}, {0x3589, real_gain&0xFF}, + {0x3548, real_gain>>8}, {0x3549, real_gain&0xFF}, + {0x35c8, real_gain>>8}, {0x35c9, real_gain&0xFF}, }; sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index e75e7e586a0d16..1b792e7e967867 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -7,25 +7,12 @@ #include #include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_util.h" #include "common/params.h" #include "common/util.h" #define FRAME_BUF_COUNT 4 -class MemoryManager { - public: - void init(int _video0_fd) { video0_fd = _video0_fd; } - void *alloc(int len, uint32_t *handle); - void free(void *ptr); - ~MemoryManager(); - private: - std::mutex lock; - std::map handle_lookup; - std::map size_lookup; - std::map > cached_allocations; - int video0_fd; -}; - class CameraState { public: MultiCameraState *multi_cam_state; @@ -36,14 +23,28 @@ class CameraState { int exposure_time; bool dc_gain_enabled; + int dc_gain_weight; + int gain_idx; float analog_gain_frac; + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[16]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + float cur_ev[3]; float min_ev, max_ev; float measured_grey_fraction; float target_grey_fraction; - int gain_idx; unique_fd sensor_fd; unique_fd csiphy_fd; @@ -55,8 +56,10 @@ class CameraState { void sensors_start(); - void camera_open(); - void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, bool enabled); + void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); + void camera_set_parameters(); + void camera_map_bufs(MultiCameraState *s); + void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc new file mode 100644 index 00000000000000..6f7da56cd110dd --- /dev/null +++ b/system/camerad/cameras/camera_util.cc @@ -0,0 +1,138 @@ +#include "system/camerad/cameras/camera_util.h" + +#include + +#include + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +// ************** low level camera helpers **************** +int do_cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { + camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); + if (ret == -1) { + LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); + } + return ret; +} + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { + struct cam_acquire_dev_cmd cmd = { + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, + }; + int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); + return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; +}; + +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { + struct cam_config_dev_cmd cmd = { + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, + }; + return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; + return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { + struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; + mem_mgr_alloc_cmd.len = len; + mem_mgr_alloc_cmd.align = align; + mem_mgr_alloc_cmd.flags = flags; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void release(int video0_fd, uint32_t handle) { + int ret; + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} + +void release_fd(int video0_fd, uint32_t handle) { + // handle to fd + close(handle>>16); + release(video0_fd, handle); +} + +void *MemoryManager::alloc(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + memset(ptr, 0, size); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h new file mode 100644 index 00000000000000..e408f6c0e2c5c4 --- /dev/null +++ b/system/camerad/cameras/camera_util.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include + +#include + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); +int device_control(int fd, int op_code, int session_handle, int dev_handle); +int do_cam_control(int fd, int op_code, void *handle, int size); +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, + int mmu_hdl = 0, int mmu_hdl2 = 0); +void release(int video0_fd, uint32_t handle); + +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + void *alloc(int len, uint32_t *handle); + void free(void *ptr); + ~MemoryManager(); + private: + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/cameras/real_debayer.cl index fc9f41049495f1..4a36a03bf51218 100644 --- a/system/camerad/cameras/real_debayer.cl +++ b/system/camerad/cameras/real_debayer.cl @@ -8,9 +8,15 @@ float3 color_correct(float3 rgb) { // color correction + #if IS_OX + float3 x = rgb.x * (float3)(1.81485125, -0.51650643, -0.06985117); + x += rgb.y * (float3)(-0.51681964, 1.85935946, -0.49871889); + x += rgb.z * (float3)(-0.29803161, -0.34285304, 1.56857006); + #else float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); + #endif // tone mapping params const float gamma_k = 0.75; @@ -36,14 +42,43 @@ float get_vignetting_s(float r) { } } +constant float ox03c10_lut[] = { + 0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05, + 1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05, + 3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05, + 6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04, + 1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04, + 1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04, + 2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04, + 4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04, + 9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03, + 1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03, + 3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03, + 7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02, + 1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02, + 6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01, + 1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01, + 3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00 +}; + float4 val4_from_12(uchar8 pvs, float gain) { uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit ((uint)pvs.s2<<4) + (pvs.s4&0xF), ((uint)pvs.s3<<4) + (pvs.s4>>4), ((uint)pvs.s5<<4) + (pvs.s7&0xF)); + #if IS_OX + // PWL + //float4 pv = (convert_float4(parsed) - 64.0) / (4096.0 - 64.0); + float4 pv = {ox03c10_lut[parsed.s0], ox03c10_lut[parsed.s1], ox03c10_lut[parsed.s2], ox03c10_lut[parsed.s3]}; + + // it's a 24 bit signal, center in the middle 8 bits + return pv*256.0; + #else // AR // normalize and scale float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0); return clamp(pv*gain, 0.0, 1.0); + #endif + } float get_k(float a, float b, float c, float d) { diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 284347623b32cb..9df99552e18207 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -1,48 +1,754 @@ struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; -struct i2c_random_wr_payload start_reg_array_imx390[] = {{0x0, 0}}; -struct i2c_random_wr_payload stop_reg_array_imx390[] = {{0x0, 1}}; +struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; +struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; -struct i2c_random_wr_payload init_array_imx390[] = { - {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames - {0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX +struct i2c_random_wr_payload init_array_ox03c10[] = { + {0x103, 1}, + {0x107, 1}, - // crop - {0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE - {0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE - {0x0078, 1}, {0x03c0, 1}, + // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200 - // external trigger (off) - // while images still come in, they are blank with this - {0x3650, 0}, // CU_MODE + // TPM + {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf}, - // exposure - {0x000c, 0xc0}, {0x000d, 0x07}, - {0x0010, 0xc0}, {0x0011, 0x07}, + /*) + // group 4 + {0x3208, 0x04}, + {0x4620, 0x04}, + {0x3208, 0x14}, - // WUXGA mode - // not in datasheet, from https://github.com/bogsen/STLinux-Kernel/blob/master/drivers/media/platform/tegra/imx185.c - {0x0086, 0xc4}, {0x0087, 0xff}, // WND_SHIFT_V = -60 - {0x03c6, 0xc4}, {0x03c7, 0xff}, // SM_WND_SHIFT_V_APL = -60 + // group 5 + {0x3208, 0x05}, + {0x4620, 0x04}, + {0x3208, 0x15}, - {0x201c, 0xe1}, {0x201d, 0x12}, // image read amount - {0x21ee, 0xc4}, {0x21ef, 0x04}, // image send amount (1220 is the end) - {0x21f0, 0xc4}, {0x21f1, 0x04}, // image processing amount + // group 2 + {0x3208, 0x02}, + {0x3507, 0x00}, + {0x3208, 0x12}, - // disable a bunch of errors causing blanking - {0x0390, 0x00}, {0x0391, 0x00}, {0x0392, 0x00}, + // delay launch group 2 + {0x3208, 0xa2},*/ - // flip bayer - {0x2D64, 0x64 + 2}, + // PLL setup + {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix + {0x0303, 0x01}, // pll1_prediv + {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300 + {0x0306, 0x04}, // pll1_divmipi = 4 + {0x0307, 0x01}, // pll1_divm = 1 + {0x0316, 0x00}, + {0x0317, 0x00}, + {0x0318, 0x00}, + {0x0323, 0x05}, // pll2_prediv + {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300 - // color correction - {0x0030, 0xf8}, {0x0031, 0x00}, // red gain - {0x0032, 0x9a}, {0x0033, 0x00}, // gr gain - {0x0034, 0x9a}, {0x0035, 0x00}, // gb gain - {0x0036, 0x22}, {0x0037, 0x01}, // blue gain + // SCLK/PCLK + {0x0400, 0xe0}, {0x0401, 0x80}, + {0x0403, 0xde}, {0x0404, 0x34}, + {0x0405, 0x3b}, {0x0406, 0xde}, + {0x0407, 0x08}, + {0x0408, 0xe0}, {0x0409, 0x7f}, + {0x040a, 0xde}, {0x040b, 0x34}, + {0x040c, 0x47}, {0x040d, 0xd8}, + {0x040e, 0x08}, - // hdr enable (noise with this on for now) - {0x00f9, 0} + // xchk + {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79}, + + // SC ctrl + {0x3001, 0x03}, // io_pad_oen + {0x3002, 0xf8}, // io_pad_oen + {0x3005, 0x80}, // io_pad_out + {0x3007, 0x01}, // io_pad_sel + {0x3008, 0x80}, // io_pad_sel + + // FSIN first frame + /* + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x3822, 0x20}, + {0x3823, 0x58}, + + {0x3826, 0x0}, {0x3827, 0x8}, + {0x3881, 0x4}, + + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + */ + + // FSIN with external pulses + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x383E, 0x80}, + {0x3881, 0x4}, + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + + {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI + {0x3020, 0x05}, // SC_CTRL_20 + + // this is not in the datasheet, listed as RSVD + // but the camera doesn't work without it + {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23}, + {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d}, + {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96}, + {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73}, + {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36}, + {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21}, + {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28}, + {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c}, + {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13}, + {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02}, + {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00}, + {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e}, + {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02}, + {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00}, + {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00}, + {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00}, + {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19}, + + // also RSVD + {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01}, + {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21}, + {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3}, + {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03}, + {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80}, + {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c}, + {0x3c73, 0x2c}, {0x3c76, 0x12}, + + // PEC checks + {0x3182, 0x12}, + + {0x320e, 0x00}, {0x320f, 0x00}, // RSVD + {0x3211, 0x61}, + {0x3215, 0xcd}, + {0x3219, 0x08}, + + {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure + {0x350a, 0x04}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + + {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure + {0x358a, 0x04}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain + + {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure + {0x354a, 0x04}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain + + {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure + {0x35ca, 0x04}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain + + // also RSVD + {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01}, + {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a}, + {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b}, + {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f}, + {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00}, + {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a}, + {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10}, + {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a}, + {0x3679, 0x19}, + + // Y_ADDR_START = 4 + {0x3802, 0x00}, {0x3803, 0x04}, + // Y_ADDR_END = 0x50b + {0x3806, 0x05}, {0x3807, 0x0b}, + + // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928) + {0x3808, 0x07}, {0x3809, 0x88}, + + // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208) + {0x380a, 0x04}, {0x380b, 0xb8}, + + // horizontal timing 0x447 + {0x380c, 0x04}, {0x380d, 0x47}, + + // rows per frame (was 0x2ae) + // 0x8ae = 53.65 ms + {0x380e, 0x08}, {0x380f, 0x15}, + // this should be triggered by FSIN, not free running + + {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff + {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff + {0x3816, 0x01}, + {0x3817, 0x01}, + {0x381c, 0x18}, + {0x381e, 0x01}, + {0x381f, 0x01}, + + // don't mirror, just flip + {0x3820, 0x04}, + + {0x3821, 0x19}, + {0x3832, 0x00}, + {0x3834, 0x00}, + {0x384c, 0x02}, + {0x384d, 0x0d}, + {0x3850, 0x00}, + {0x3851, 0x42}, + {0x3852, 0x00}, + {0x3853, 0x40}, + {0x3858, 0x04}, + {0x388c, 0x02}, + {0x388d, 0x2b}, + + // APC + {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90}, + {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20}, + {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e}, + {0x3b4c, 0x00}, {0x3b4d, 0x00}, + {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08}, + {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80}, + {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00}, + {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09}, + + // OTP + {0x3d82, 0x73}, + {0x3d85, 0x05}, + {0x3d8a, 0x03}, + {0x3d8b, 0xff}, + {0x3d99, 0x00}, + {0x3d9a, 0x9f}, + {0x3d9b, 0x00}, + {0x3d9c, 0xa0}, + {0x3da4, 0x00}, + {0x3da7, 0x50}, + + // DTR + {0x420e, 0x6b}, + {0x420f, 0x6e}, + {0x4210, 0x06}, + {0x4211, 0xc1}, + {0x421e, 0x02}, + {0x421f, 0x45}, + {0x4220, 0xe1}, + {0x4221, 0x01}, + {0x4301, 0xff}, + {0x4307, 0x03}, + {0x4308, 0x13}, + {0x430a, 0x13}, + {0x430d, 0x93}, + {0x430f, 0x57}, + {0x4310, 0x95}, + {0x4311, 0x16}, + {0x4316, 0x00}, + + {0x4317, 0x38}, // both embedded rows are enabled + + {0x4319, 0x03}, // spd dcg + {0x431a, 0x00}, // 8 bit mipi + {0x431b, 0x00}, + {0x431d, 0x2a}, + {0x431e, 0x11}, + + {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits + //{0x431f, 0x00}, // disable PWL + + {0x4320, 0x19}, + {0x4323, 0x80}, + {0x4324, 0x00}, + {0x4503, 0x4e}, + {0x4505, 0x00}, + {0x4509, 0x00}, + {0x450a, 0x00}, + {0x4580, 0xf8}, + {0x4583, 0x07}, + {0x4584, 0x6a}, + {0x4585, 0x08}, + {0x4586, 0x05}, + {0x4587, 0x04}, + {0x4588, 0x73}, + {0x4589, 0x05}, + {0x458a, 0x1f}, + {0x458b, 0x02}, + {0x458c, 0xdc}, + {0x458d, 0x03}, + {0x458e, 0x02}, + {0x4597, 0x07}, + {0x4598, 0x40}, + {0x4599, 0x0e}, + {0x459a, 0x0e}, + {0x459b, 0xfb}, + {0x459c, 0xf3}, + {0x4602, 0x00}, + {0x4603, 0x13}, + {0x4604, 0x00}, + {0x4609, 0x0a}, + {0x460a, 0x30}, + {0x4610, 0x00}, + {0x4611, 0x70}, + {0x4612, 0x01}, + {0x4613, 0x00}, + {0x4614, 0x00}, + {0x4615, 0x70}, + {0x4616, 0x01}, + {0x4617, 0x00}, + + {0x4800, 0x04}, // invert output PCLK + {0x480a, 0x22}, + {0x4813, 0xe4}, + + // mipi + {0x4814, 0x2a}, + {0x4837, 0x0d}, + {0x484b, 0x47}, + {0x484f, 0x00}, + {0x4887, 0x51}, + {0x4d00, 0x4a}, + {0x4d01, 0x18}, + {0x4d05, 0xff}, + {0x4d06, 0x88}, + {0x4d08, 0x63}, + {0x4d09, 0xdf}, + {0x4d15, 0x7d}, + {0x4d1a, 0x20}, + {0x4d30, 0x0a}, + {0x4d31, 0x00}, + {0x4d34, 0x7d}, + {0x4d3c, 0x7d}, + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x00}, + {0x4f03, 0x20}, + {0x4f04, 0xe0}, + {0x6a00, 0x00}, + {0x6a01, 0x20}, + {0x6a02, 0x00}, + {0x6a03, 0x20}, + {0x6a04, 0x02}, + {0x6a05, 0x80}, + {0x6a06, 0x01}, + {0x6a07, 0xe0}, + {0x6a08, 0xcf}, + {0x6a09, 0x01}, + {0x6a0a, 0x40}, + {0x6a20, 0x00}, + {0x6a21, 0x02}, + {0x6a22, 0x00}, + {0x6a23, 0x00}, + {0x6a24, 0x00}, + {0x6a25, 0x00}, + {0x6a26, 0x00}, + {0x6a27, 0x00}, + {0x6a28, 0x00}, + + // isp + {0x5000, 0x8f}, + {0x5001, 0x75}, + {0x5002, 0x7f}, // PWL0 + //{0x5002, 0x3f}, // PWL disable + {0x5003, 0x7a}, + + {0x5004, 0x3e}, + {0x5005, 0x1e}, + {0x5006, 0x1e}, + {0x5007, 0x1e}, + + {0x5008, 0x00}, + {0x500c, 0x00}, + {0x502c, 0x00}, + {0x502e, 0x00}, + {0x502f, 0x00}, + {0x504b, 0x00}, + {0x5053, 0x00}, + {0x505b, 0x00}, + {0x5063, 0x00}, + {0x5070, 0x00}, + {0x5074, 0x04}, + {0x507a, 0x04}, + {0x507b, 0x09}, + {0x5500, 0x02}, + {0x5700, 0x02}, + {0x5900, 0x02}, + {0x6007, 0x04}, + {0x6008, 0x05}, + {0x6009, 0x02}, + {0x600b, 0x08}, + {0x600c, 0x07}, + {0x600d, 0x88}, + {0x6016, 0x00}, + {0x6027, 0x04}, + {0x6028, 0x05}, + {0x6029, 0x02}, + {0x602b, 0x08}, + {0x602c, 0x07}, + {0x602d, 0x88}, + {0x6047, 0x04}, + {0x6048, 0x05}, + {0x6049, 0x02}, + {0x604b, 0x08}, + {0x604c, 0x07}, + {0x604d, 0x88}, + {0x6067, 0x04}, + {0x6068, 0x05}, + {0x6069, 0x02}, + {0x606b, 0x08}, + {0x606c, 0x07}, + {0x606d, 0x88}, + {0x6087, 0x04}, + {0x6088, 0x05}, + {0x6089, 0x02}, + {0x608b, 0x08}, + {0x608c, 0x07}, + {0x608d, 0x88}, + + // 12-bit PWL0 + {0x5e00, 0x00}, + + // m_ndX_exp[0:32] + // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518 + {0x5e01, 0x09}, + {0x5e02, 0x09}, + {0x5e03, 0x0a}, + {0x5e04, 0x0a}, + {0x5e05, 0x0a}, + {0x5e06, 0x0b}, + {0x5e07, 0x0b}, + {0x5e08, 0x0c}, + {0x5e09, 0x0c}, + {0x5e0a, 0x0d}, + {0x5e0b, 0x0d}, + {0x5e0c, 0x0e}, + {0x5e0d, 0x0e}, + {0x5e0e, 0x0f}, + {0x5e0f, 0x0f}, + {0x5e10, 0x10}, + {0x5e11, 0x10}, + {0x5e12, 0x11}, + {0x5e13, 0x11}, + {0x5e14, 0x12}, + {0x5e15, 0x12}, + {0x5e16, 0x12}, + {0x5e17, 0x12}, + {0x5e18, 0x13}, + {0x5e19, 0x13}, + {0x5e1a, 0x13}, + {0x5e1b, 0x14}, + {0x5e1c, 0x14}, + {0x5e1d, 0x14}, + {0x5e1e, 0x15}, + {0x5e1f, 0x15}, + {0x5e20, 0x15}, + {0x5e21, 0x16}, + + // m_ndY_val[0:32] + // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095 + {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00}, + {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff}, + {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00}, + {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00}, + {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00}, + {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80}, + {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80}, + {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80}, + {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80}, + {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80}, + {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80}, + {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80}, + {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80}, + {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80}, + {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80}, + {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80}, + {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80}, + {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40}, + {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40}, + {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40}, + {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40}, + {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40}, + {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40}, + {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40}, + {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40}, + {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40}, + {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40}, + {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40}, + {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40}, + {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40}, + {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40}, + {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40}, + {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40}, + + // disable PWL + /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00}, + {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00}, + {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00}, + {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00}, + {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00}, + {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00}, + {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00}, + {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00}, + {0x5e21, 0x00}, + + {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/ + + {0x4001, 0x2b}, // BLC_CTRL_1 + {0x4008, 0x02}, {0x4009, 0x03}, + {0x4018, 0x12}, + {0x4022, 0x40}, + {0x4023, 0x20}, + + // all black level targets are 0x40 + {0x4026, 0x00}, {0x4027, 0x40}, + {0x4028, 0x00}, {0x4029, 0x40}, + {0x402a, 0x00}, {0x402b, 0x40}, + {0x402c, 0x00}, {0x402d, 0x40}, + + {0x407e, 0xcc}, + {0x407f, 0x18}, + {0x4080, 0xff}, + {0x4081, 0xff}, + {0x4082, 0x01}, + {0x4083, 0x53}, + {0x4084, 0x01}, + {0x4085, 0x2b}, + {0x4086, 0x00}, + {0x4087, 0xb3}, + + {0x4640, 0x40}, + {0x4641, 0x11}, + {0x4642, 0x0e}, + {0x4643, 0xee}, + {0x4646, 0x0f}, + {0x4648, 0x00}, + {0x4649, 0x03}, + + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x80}, + {0x4f03, 0x2c}, + {0x4f04, 0xf8}, + + {0x4d09, 0xff}, + {0x4d09, 0xdf}, + + {0x5003, 0x7a}, + {0x5b80, 0x08}, + {0x5c00, 0x08}, + {0x5c80, 0x00}, + {0x5bbe, 0x12}, + {0x5c3e, 0x12}, + {0x5cbe, 0x12}, + {0x5b8a, 0x80}, + {0x5b8b, 0x80}, + {0x5b8c, 0x80}, + {0x5b8d, 0x80}, + {0x5b8e, 0x60}, + {0x5b8f, 0x80}, + {0x5b90, 0x80}, + {0x5b91, 0x80}, + {0x5b92, 0x80}, + {0x5b93, 0x20}, + {0x5b94, 0x80}, + {0x5b95, 0x80}, + {0x5b96, 0x80}, + {0x5b97, 0x20}, + {0x5b98, 0x00}, + {0x5b99, 0x80}, + {0x5b9a, 0x40}, + {0x5b9b, 0x20}, + {0x5b9c, 0x00}, + {0x5b9d, 0x00}, + {0x5b9e, 0x80}, + {0x5b9f, 0x00}, + {0x5ba0, 0x00}, + {0x5ba1, 0x00}, + {0x5ba2, 0x00}, + {0x5ba3, 0x00}, + {0x5ba4, 0x00}, + {0x5ba5, 0x00}, + {0x5ba6, 0x00}, + {0x5ba7, 0x00}, + {0x5ba8, 0x02}, + {0x5ba9, 0x00}, + {0x5baa, 0x02}, + {0x5bab, 0x76}, + {0x5bac, 0x03}, + {0x5bad, 0x08}, + {0x5bae, 0x00}, + {0x5baf, 0x80}, + {0x5bb0, 0x00}, + {0x5bb1, 0xc0}, + {0x5bb2, 0x01}, + {0x5bb3, 0x00}, + + // m_nNormCombineWeight + {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60}, + {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20}, + {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00}, + {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00}, + {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00}, + {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00}, + + // m_nCombinThreL + {0x5c28, 0x02}, {0x5c29, 0x00}, + {0x5c2a, 0x02}, {0x5c2b, 0x76}, + {0x5c2c, 0x03}, {0x5c2d, 0x08}, + + // m_nCombinThreS + {0x5c2e, 0x00}, {0x5c2f, 0x80}, + {0x5c30, 0x00}, {0x5c31, 0xc0}, + {0x5c32, 0x01}, {0x5c33, 0x00}, + + // m_nNormCombineWeight + {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80}, + {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60}, + {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40}, + {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00}, + {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00}, + {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00}, + + {0x5ca8, 0x01}, {0x5ca9, 0x00}, + {0x5caa, 0x02}, {0x5cab, 0x00}, + {0x5cac, 0x03}, {0x5cad, 0x08}, + + {0x5cae, 0x01}, {0x5caf, 0x00}, + {0x5cb0, 0x02}, {0x5cb1, 0x00}, + {0x5cb2, 0x03}, {0x5cb3, 0x08}, + + // combine ISP + {0x5be7, 0x80}, + {0x5bc9, 0x80}, + {0x5bca, 0x80}, + {0x5bcb, 0x80}, + {0x5bcc, 0x80}, + {0x5bcd, 0x80}, + {0x5bce, 0x80}, + {0x5bcf, 0x80}, + {0x5bd0, 0x80}, + {0x5bd1, 0x80}, + {0x5bd2, 0x20}, + {0x5bd3, 0x80}, + {0x5bd4, 0x40}, + {0x5bd5, 0x20}, + {0x5bd6, 0x00}, + {0x5bd7, 0x00}, + {0x5bd8, 0x00}, + {0x5bd9, 0x00}, + {0x5bda, 0x00}, + {0x5bdb, 0x00}, + {0x5bdc, 0x00}, + {0x5bdd, 0x00}, + {0x5bde, 0x00}, + {0x5bdf, 0x00}, + {0x5be0, 0x00}, + {0x5be1, 0x00}, + {0x5be2, 0x00}, + {0x5be3, 0x00}, + {0x5be4, 0x00}, + {0x5be5, 0x00}, + {0x5be6, 0x00}, + + // m_nSPDCombineWeight + {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40}, + {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20}, + {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00}, + {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00}, + {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00}, + {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00}, + + // m_nSPDCombineWeight + {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80}, + {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60}, + {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40}, + {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20}, + {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00}, + {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00}, + + {0x5d74, 0x01}, + {0x5d75, 0x00}, + + {0x5d1f, 0x81}, + {0x5d11, 0x00}, + {0x5d12, 0x10}, + {0x5d13, 0x10}, + {0x5d15, 0x05}, + {0x5d16, 0x05}, + {0x5d17, 0x05}, + {0x5d08, 0x03}, + {0x5d09, 0xb6}, + {0x5d0a, 0x03}, + {0x5d0b, 0xb6}, + {0x5d18, 0x03}, + {0x5d19, 0xb6}, + {0x5d62, 0x01}, + {0x5d40, 0x02}, + {0x5d41, 0x01}, + {0x5d63, 0x1f}, + {0x5d64, 0x00}, + {0x5d65, 0x80}, + {0x5d56, 0x00}, + {0x5d57, 0x20}, + {0x5d58, 0x00}, + {0x5d59, 0x20}, + {0x5d5a, 0x00}, + {0x5d5b, 0x0c}, + {0x5d5c, 0x02}, + {0x5d5d, 0x40}, + {0x5d5e, 0x02}, + {0x5d5f, 0x40}, + {0x5d60, 0x03}, + {0x5d61, 0x40}, + {0x5d4a, 0x02}, + {0x5d4b, 0x40}, + {0x5d4c, 0x02}, + {0x5d4d, 0x40}, + {0x5d4e, 0x02}, + {0x5d4f, 0x40}, + {0x5d50, 0x18}, + {0x5d51, 0x80}, + {0x5d52, 0x18}, + {0x5d53, 0x80}, + {0x5d54, 0x18}, + {0x5d55, 0x80}, + {0x5d46, 0x20}, + {0x5d47, 0x00}, + {0x5d48, 0x22}, + {0x5d49, 0x00}, + {0x5d42, 0x20}, + {0x5d43, 0x00}, + {0x5d44, 0x22}, + {0x5d45, 0x00}, + + {0x5004, 0x1e}, + {0x4221, 0x03}, // this is changed from 1 -> 3 + + // DCG exposure coarse + {0x3501, 0x01}, {0x3502, 0xc8}, + // SPD exposure coarse + {0x3541, 0x01}, {0x3542, 0xc8}, + // VS exposure coarse + {0x35c1, 0x00}, {0x35c2, 0x01}, + + // crc reference + {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, + // crc stat check + {0x507a, 0x5f}, {0x507b, 0x46}, + + // watchdog control + {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c}, + + // color balance gains + // blue + {0x5280, 0x06}, {0x5281, 0x4A}, // hcg + {0x5480, 0x06}, {0x5481, 0x4A}, // lcg + {0x5680, 0x07}, {0x5681, 0xDD}, // spd + {0x5880, 0x06}, {0x5881, 0x4A}, // vs + + // green(blue) + {0x5282, 0x04}, {0x5283, 0x00}, + {0x5482, 0x04}, {0x5483, 0x00}, + {0x5682, 0x04}, {0x5683, 0x00}, + {0x5882, 0x04}, {0x5883, 0x00}, + + // green(red) + {0x5284, 0x04}, {0x5285, 0x00}, + {0x5484, 0x04}, {0x5485, 0x00}, + {0x5684, 0x04}, {0x5685, 0x00}, + {0x5884, 0x04}, {0x5885, 0x00}, + + // red + {0x5286, 0x08}, {0x5287, 0x6C}, + {0x5486, 0x08}, {0x5487, 0x6C}, + {0x5686, 0x08}, {0x5687, 0xAA}, + {0x5886, 0x08}, {0x5887, 0x6C}, }; struct i2c_random_wr_payload init_array_ar0231[] = { diff --git a/third_party/SConscript b/third_party/SConscript index 5a7a62ae5ada06..e5bbfaa07a8d67 100644 --- a/third_party/SConscript +++ b/third_party/SConscript @@ -1,6 +1,6 @@ Import('env') -env.Library('json11', ['json11/json11.cpp']) +env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call']) env.Append(CPPPATH=[Dir('json11')]) env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) From ae43cd4b89982d490ca197e2b0e1fe4b1ce77ef9 Mon Sep 17 00:00:00 2001 From: neokii Date: Mon, 12 Sep 2022 01:05:24 +0900 Subject: [PATCH 12/15] update translations --- selfdrive/ui/translations/main_ko.ts | 454 ++++++++++++++------------- 1 file changed, 229 insertions(+), 225 deletions(-) diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index f478b267c49685..1fd76d3117c3ca 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -4,17 +4,17 @@ AbstractAlert - + Close ๋‹ซ๊ธฐ - + Snooze Update ์—…๋ฐ์ดํŠธ ๋‹ค์‹œ์•Œ๋ฆผ - + Reboot and Update ์žฌ๋ถ€ํŒ…๋ฐ ์—…๋ฐ์ดํŠธ @@ -22,53 +22,53 @@ AdvancedNetworking - + Back ๋’ค๋กœ - + Enable Tethering ํ…Œ๋”๋ง ์ผœ๊ธฐ - + Tethering Password ํ…Œ๋”๋ง ์•”ํ˜ธ - - + + EDIT ํŽธ์ง‘ - + Enter new tethering password ์ƒˆ๋กœ์šด ์•”ํ˜ธ๋ฅผ ์ž…๋ ฅํ•˜์„ธ์š” - + IP Address IP ์ฃผ์†Œ - + Enable Roaming ๋กœ๋ฐ ์ผœ๊ธฐ - + APN Setting APN ์„ค์ • - + Enter APN APN ์ž…๋ ฅ - + leave blank for automatic configuration ์ž๋™์„ค์ •์„ ํ•˜๋ ค๋ฉด ๊ณต๋ฐฑ์œผ๋กœ ๋‘์„ธ์š” @@ -76,103 +76,103 @@ CommunityPanel - - + + Select your car ์ฐจ๋Ÿ‰์„ ์„ ํƒํ•˜์„ธ์š” - + Use Cluster Speed ๊ณ„๊ธฐํŒ์†๋„ ์‚ฌ์šฉ - + Use cluster speed instead of wheel speed. ํœ ์†๋„ ๋Œ€์‹  ๊ณ„๊ธฐํŒ์†๋„๋ฅผ ์‚ฌ์šฉํ•ฉ๋‹ˆ๋‹ค.(ํœ ์†๋„ ์‚ฌ์šฉ ์ถ”์ฒœ) - + Enable HKG Long Control Openpilot ์ง€์› ASCC ์‚ฌ์šฉ(Long Control) - + Openpilot will control the speed of your car Openpilot์ด ASCC๋ฅผ ์ปจํŠธ๋กคํ•ฉ๋‹ˆ๋‹ค - + LDWS only LDWS ์ „์šฉ์ฐจ๋Ÿ‰ - + If your car only supports LDWS, turn it on. LKAS, LFA๊ฐ€ ์•„๋‹Œ ๊ตฌํ˜• LDWS์ฐจ๋Ÿ‰์ผ ๊ฒฝ์šฐ ์„ค์ •ํ•˜์„ธ์š” - + Enable Lane Change Assist ์ฐจ์„ ๋ณ€๊ฒฝ - + Perform assisted lane changes with openpilot by checking your surroundings for safety, activating the turn signal and gently nudging the steering wheel towards your desired lane. openpilot is not capable of checking if a lane change is safe. You must continuously observe your surroundings to use this feature. Openpilot์œผ๋กœ ์ฃผ๋ณ€์˜ ์•ˆ์ „์„ ํ™•์ธํ•˜๊ณ  ๋ฐฉํ–ฅ ์ง€์‹œ๋“ฑ์„ ํ™œ์„ฑํ™”ํ•˜๊ณ  ์Šคํ‹ฐ์–ด๋ง ํœ ์„ ์›ํ•˜๋Š” ์ฐจ์„ ์œผ๋กœ ๋ถ€๋“œ๋Ÿฝ๊ฒŒ ์›€์ง์—ฌ ๋ณด์กฐ ์ฐจ์„  ๋ณ€๊ฒฝ์„ ์ˆ˜ํ–‰ํ•˜์‹ญ์‹œ์˜ค. openpilot์€ ์ฐจ์„  ๋ณ€๊ฒฝ์ด ์•ˆ์ „ํ•œ์ง€ ํ™•์ธํ•  ์ˆ˜ ์—†์Šต๋‹ˆ๋‹ค. ์ด ๊ธฐ๋Šฅ์„ ์‚ฌ์šฉํ•˜๋ ค๋ฉด ์ฃผ๋ณ€์„ ์ง€์†์ ์œผ๋กœ ๊ด€์ฐฐํ•ด์•ผ ํ•ฉ๋‹ˆ๋‹ค. - + Enable Auto Lane Change(Nudgeless) ์ž๋™์ฐจ์„ ๋ณ€๊ฒฝ - + Automatically changes lanes at turn signal. ์Šคํ‹ฐ์–ด๋งํœ ์„ ์‚ด์ง ๊บฝ์ง€ ์•Š๊ณ  ํ„ด์‹œ๊ทธ๋„๋งŒ ๋„ฃ์œผ๋ฉด ์ž๋™์œผ๋กœ ์ฐจ์„ ์„ ๋ณ€๊ฒฝํ•ฉ๋‹ˆ๋‹ค. ํ•ญ์ƒ ์ฃผ์˜ํ•˜์„ธ์š”. - + Enable slow on curves ์ปค๋ธŒ ๊ฐ์† - + Sync set speed on gas pressed ๊ฐ€์†์‹œ ์„ค์ •์†๋„๋ฅผ ๊ฐ™์ด ์˜ฌ๋ฆฝ๋‹ˆ๋‹ค - + Stock Navi based deceleration ์ˆœ์ •๋‚ด๋น„๊ธฐ๋ฐ˜ ๊ฐ์† ์‚ฌ์šฉ - + Use the stock navi based deceleration for longcontrol Long Control์ผ ๋•Œ ์ˆœ์ •๋‚ด๋น„์˜ ๊ฐ์†์‹ ํ˜ธ๊ฐ€ ์žˆ์„ ์‹œ ์ ์šฉํ•ฉ๋‹ˆ๋‹ค. - + Keep steering while turn signals ํ„ด ์‹œ๊ทธ๋„์ผ๋•Œ๋„ ๊ณ„์† ์กฐํ–ฅ - + Haptic feedback (speed-cam alert) ํ–…ํ‹ฑ ํ”ผ๋“œ๋ฐฑ์„ ์‚ฌ์šฉํ•ฉ๋‹ˆ๋‹ค - + Haptic feedback when a speed camera is detected NDA ์—ฐ๋™์‹œ ์นด๋ฉ”๋ผ๋ฅผ ๋งŒ๋‚˜๋ฉด ํ–…ํ‹ฑ ํ”ผ๋“œ๋ฐฑ์„ ์‚ฌ์šฉํ•ฉ๋‹ˆ๋‹ค - + Disable Openpilot FCW Openpilot FCW๋ฅผ ๋•๋‹ˆ๋‹ค - + Show Debug UI ๋””๋ฒ„๊ทธ ๋ฉ”์„ธ์ง€ ๋ณด์ด๊ธฐ @@ -180,13 +180,13 @@ ConfirmationDialog - - + + Ok ํ™•์ธ - + Cancel ์ทจ์†Œ @@ -194,17 +194,17 @@ DeclinePage - + You must accept the Terms and Conditions in order to use openpilot. openpilot์„ ์‚ฌ์šฉํ•˜๋ ค๋ฉด ์ด์šฉ ์•ฝ๊ด€์— ๋™์˜ํ•ด์•ผ ํ•ฉ๋‹ˆ๋‹ค. - + Back ๋’ค๋กœ - + Decline, uninstall %1 ๊ฑฐ์ ˆ, %1 ์ œ๊ฑฐ @@ -212,43 +212,43 @@ DevicePanel - + Dongle ID ๋™๊ธ€ ID - + N/A - + Serial ์‹œ๋ฆฌ์–ผ - + Soft restart ์†Œํ”„ํŠธ ์žฌ์‹œ์ž‘ - - + + Reset Calibration ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜ ์žฌ์„ค์ • - + Are you sure you want to reset calibration and live params? ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜๊ณผ ๋ผ์ด๋ธŒ ํŒŒ๋ผ๋ฏธํ„ฐ(AngleOffset, SteerRatio๋“ฑ)์ด ์žฌ์„ค์ •(์ดˆ๊ธฐํ™”)๋ฉ๋‹ˆ๋‹ค. ์ง„ํ–‰ํ• ๊นŒ์š”? - + Driver Camera ์šด์ „์ž ์นด๋ฉ”๋ผ - + PREVIEW ๋ฏธ๋ฆฌ๋ณด๊ธฐ @@ -257,136 +257,136 @@ ์ตœ์ƒ์˜ ์šด์ „์ž ๋ชจ๋‹ˆํ„ฐ๋ง ์œ„ํ•ด ์žฅ์ฐฉ ์œ„์น˜๋ฅผ ์ตœ์ ํ™”ํ•˜๋Š”๋ฐ ๋„์›€์ด ๋˜๋„๋ก ์šด์ „์ž๊ฐ€ ๋ฐ”๋ผ๋ณด๋Š” ์นด๋ฉ”๋ผ๋ฅผ ๋ฏธ๋ฆฌ ๋ด…๋‹ˆ๋‹ค. (์ฐจ๋Ÿ‰์€ ๊บผ์ ธ ์žˆ์–ด์•ผ ํ•จ) - + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) ์ตœ์ƒ์˜ ์šด์ „์ž ๋ชจ๋‹ˆํ„ฐ๋ง ์œ„ํ•ด ์žฅ์ฐฉ ์œ„์น˜๋ฅผ ์ตœ์ ํ™”ํ•˜๋Š”๋ฐ ๋„์›€์ด ๋˜๋„๋ก ์šด์ „์ž๊ฐ€ ๋ฐ”๋ผ๋ณด๋Š” ์นด๋ฉ”๋ผ๋ฅผ ๋ฏธ๋ฆฌ ๋ด…๋‹ˆ๋‹ค. (์ฐจ๋Ÿ‰์€ ๊บผ์ ธ ์žˆ์–ด์•ผ ํ•จ) - + RESET ์žฌ์„ค์ • - + Are you sure you want to reset calibration? ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์žฌ์„ค์ •ํ• ๊นŒ์š”? - + Review Training Guide ํŠธ๋ ˆ์ด๋‹ ๊ฐ€์ด๋“œ ๊ฒ€ํ†  - + REVIEW ๊ฒ€ํ†  - + Review the rules, features, and limitations of openpilot openpilot์˜ ๊ทœ์น™, ๊ธฐ๋Šฅ ๋ฐ ์ œํ•œ ์‚ฌํ•ญ ๊ฒ€ํ†  - + Are you sure you want to review the training guide? ํŠธ๋ ˆ์ด๋‹ ๊ฐ€์ด๋“œ๋ฅผ ๊ฒ€ํ† ํ•˜์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? - + Regulatory ๊ทœ์ œ - + VIEW ๋ณด๊ธฐ - + Change Language ์–ธ์–ด ๋ณ€๊ฒฝ - + CHANGE ๋ณ€๊ฒฝ - + Select a language ์‚ฌ์šฉํ•  ์–ธ์–ด๋ฅผ ์„ ํƒํ•˜์„ธ์š” - + Reboot ์žฌ๋ถ€ํŒ… - + Rebuild ์ „์ฒด ์žฌ๋นŒ๋“œ - + Are you sure you want to rebuild? ์ „์ฒด ์žฌ๋นŒ๋“œ๋ฅผ ํ•˜์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? - + Power Off ์ „์› ๋„๊ธฐ - + openpilot requires the device to be mounted within 4ยฐ left or right and within 5ยฐ up or 8ยฐ down. openpilot is continuously calibrating, resetting is rarely required. ์žฅ์น˜๋Š” ์ขŒ์šฐ 4ยฐ, ์ƒํ•˜ 5ยฐ~8ยฐ ์ด๋‚ด๋กœ ์žฅ์ฐฉ๋˜์–ด์•ผ ํ•ฉ๋‹ˆ๋‹ค. openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. ์žฌ์„ค์ •์€ ๊ฑฐ์˜ ํ•˜์ง€ ์•Š์•„๋„ ๋ฉ๋‹ˆ๋‹ค. - + Your device is pointed %1ยฐ %2 and %3ยฐ %4. ํ˜„์žฌ %2 %1ยฐ ๊ทธ๋ฆฌ๊ณ  %4 %3ยฐ ๋กœ ์žฅ์ฐฉ๋˜์–ด ์žˆ์Šต๋‹ˆ๋‹ค. - + down ์•„๋ž˜๋กœ - + up ์œ„๋กœ - + left ์ขŒ์ธก์œผ๋กœ - + right ์šฐ์ธก์œผ๋กœ - + Are you sure you want to reboot? ์žฌ๋ถ€ํŒ…ํ•˜์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? - + Disengage to Reboot ์žฌ๋ถ€ํŒ…ํ•˜๋ ค๋ฉด ๋””์Šค์ธ๊ฒŒ์ด์ง€ํ•˜์„ธ์š” - + Are you sure you want to power off? ์ „์›์„ ๋„์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? - + Disengage to Power Off ์ „์›์„ ๋„๋ ค๋ฉด ๋””์Šค์ธ๊ฒŒ์ด์ง€ํ•˜์„ธ์š” @@ -394,32 +394,32 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. DriveStats - + Drives ์ฃผํ–‰ - + Hours ์‹œ๊ฐ„ - + ALL TIME ์ „์ฒด - + PAST WEEK ์ง€๋‚œ์ฃผ - + KM KM - + Miles ๋งˆ์ผ @@ -427,7 +427,7 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. DriverViewScene - + camera starting ์นด๋ฉ”๋ผ ์‹œ์ž‘์ค‘ @@ -435,12 +435,12 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. InputDialog - + Cancel ์ทจ์†Œ - + Need at least %n character(s)! ์ตœ์†Œ %n ์ž๊ฐ€ ํ•„์š”ํ•ฉ๋‹ˆ @@ -462,7 +462,7 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. LateralControl - + Back ๋’ค๋กœ @@ -470,17 +470,17 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. MapETA - + eta ์˜ˆ์ƒ๋„์ฐฉ์‹œ๊ฐ„ - + min ๋ถ„ - + hr ์‹œ๊ฐ„ @@ -488,27 +488,27 @@ openpilot์€ ๊ณ„์†ํ•ด์„œ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค. MapPanel - + Current Destination ํ˜„์žฌ ๋ชฉ์ ์ง€ - + CLEAR ์‚ญ์ œ - + Recent Destinations ์ตœ๊ทผ ๋ชฉ์ ์ง€ - + Try the Navigation Beta ๋‚ด๋น„๊ฒŒ์ด์…˜ ๋ฒ ํƒ€๋ฅผ ์‹œ๋„ํ•ด๋ณด์„ธ์š” - + Get turn-by-turn directions displayed and more with a comma prime subscription. Sign up now: https://connect.comma.ai ์ฝค๋งˆ ํ”„๋ผ์ž„ ๊ตฌ๋…์œผ๋กœ ํ„ด๋ฐ”์ดํ„ด ๊ธธ์ฐพ๊ธฐ ๋“ฑ์„ ํ™•์ธํ•˜์„ธ์š”. @@ -521,21 +521,21 @@ prime subscription. Sign up now: https://connect.comma.ai ์ง€๊ธˆ ๊ฐ€์ž…ํ•˜์„ธ์š”: https://connect.comma.ai - + No home location set ์ง‘์ด ์„ค์ •๋˜์ง€ ์•Š์•˜์Šต๋‹ˆ๋‹ค ์œ„์น˜๋ฅผ ์„ค์ •ํ•˜์„ธ์š” - + No work location set ํšŒ์‚ฌ๊ฐ€ ์„ค์ •๋˜์ง€ ์•Š์•˜์Šต๋‹ˆ๋‹ค ์œ„์น˜๋ฅผ ์„ค์ •ํ•˜์„ธ์š” - + no recent destinations ์ตœ๊ทผ ๋ชฉ์ ์ง€๊ฐ€ ์—†์Šต๋‹ˆ๋‹ค @@ -543,12 +543,12 @@ location set MapWindow - + Map Loading ์ง€๋„ ๋กœ๋”ฉ์ค‘ - + Waiting for GPS GPS ์‹ ํ˜ธ ๋Œ€๊ธฐ์ค‘ @@ -556,12 +556,12 @@ location set MultiOptionDialog - + Select ์„ ํƒ - + Cancel ์ทจ์†Œ @@ -569,18 +569,18 @@ location set Networking - + Advanced ๊ณ ๊ธ‰ ์„ค์ • - + Enter password ์•”ํ˜ธ๋ฅผ ์ž…๋ ฅํ•˜์„ธ์š” - - + + for "%1" "%1" @@ -589,7 +589,7 @@ location set - + Wrong password ์•”ํ˜ธ๊ฐ€ ์ž˜๋ชป๋˜์—ˆ์Šต๋‹ˆ๋‹ค @@ -597,17 +597,17 @@ location set OffroadHome - + UPDATE ์—…๋ฐ์ดํŠธ - + ALERTS ์•Œ๋ฆผ - + ALERT ์•Œ๋ฆผ @@ -615,22 +615,22 @@ location set PairingPopup - + Pair your device to your comma account ์ฝค๋งˆ ์ปค๋„ฅํŠธ์™€ ๊ธฐ๊ธฐ๋ฅผ ํŽ˜์–ด๋งํ•˜์„ธ์š” - + Go to https://connect.comma.ai on your phone ํฐ์—์„œ ttps://connect.comma.ai ๋กœ ์ ‘์†ํ•˜์„ธ์š”. - + Click "add new device" and scan the QR code on the right "์ƒˆ์žฅ์น˜ ์ถ”๊ฐ€"๋ฅผ ํด๋ฆญํ•˜๊ณ  ์˜ค๋ฅธ์ชฝ QR์ฝ”๋“œ๋ฅผ ์Šค์บ”ํ•˜์„ธ์š”. - + Bookmark connect.comma.ai to your home screen to use it like an app ํ™ˆ ์Šคํฌ๋ฆฐ์— connect.comma.ai ๋ฅผ ๋“ฑ๋กํ•ด๋‘๋ฉด ์‚ฌ์šฉํ•˜๊ธฐ ์‰ฝ์Šต๋‹ˆ๋‹ค. @@ -654,32 +654,32 @@ location set PrimeAdWidget - + Upgrade Now ์ง€๊ธˆ ์—…๊ทธ๋ ˆ์ด๋“œ - + Become a comma prime member at connect.comma.ai connect.comma.ai์—์„œ ์ฝค๋งˆ ํ”„๋ผ์ž„ ํšŒ์›์ด ๋˜์‹ญ์‹œ์˜ค. - + PRIME FEATURES: ํ”„๋ผ์ž„ ๊ธฐ๋Šฅ - + Remote access ์›๊ฒฉ ์ ‘์† - + 1 year of storage 1๋…„ ์ €์žฅ๊ณต๊ฐ„ - + Developer perks ๊ฐœ๋ฐœ์ž ํŠนํ˜œ @@ -687,22 +687,22 @@ location set PrimeUserWidget - + โœ“ SUBSCRIBED โœ“ ๊ตฌ๋…์ค‘ - + comma prime ์ฝค๋งˆ ํ”„๋ผ์ž„ - + CONNECT.COMMA.AI CONNECT.COMMA.AI - + COMMA POINTS ์ฝค๋งˆ ํฌ์ธํŠธ @@ -710,41 +710,41 @@ location set QObject - + Reboot ์žฌ๋ถ€ํŒ… - + Exit ๋‚˜๊ฐ€๊ธฐ - + dashcam dashcam - + openpilot openpilot - + %n minute(s) ago %n๋ถ„์ „ - + %n hour(s) ago %n์‹œ๊ฐ„์ „ - + %n day(s) ago %n์ผ @@ -766,7 +766,7 @@ location set RichTextDialog - + Ok ํ™•์ธ @@ -774,12 +774,12 @@ location set SelectCar - + Back ๋’ค๋กœ - + [ Not selected ] [์„ ํƒ ์•ˆํ•จ] @@ -787,38 +787,38 @@ location set SettingsWindow - + โ† Back โ† ๋’ค๋กœ - + Device ์žฅ์น˜ - - + + Network ๋„คํŠธ์›Œํฌ - + Toggles ํ† ๊ธ€ - + Software ์†Œํ”„ํŠธ์›จ์–ด - + Community ์ปค๋ฎค๋‹ˆํ‹ฐ - + Navigation ๋‚ด๋น„๊ฒŒ์ด์…˜ @@ -826,17 +826,17 @@ location set SetupWidget - + Finish Setup ์„ค์ • ์™„๋ฃŒ - + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. ์žฅ์น˜๋ฅผ ์ฝค๋งˆ ์ปค๋„ฅํŠธ(connect.comma.ai)๊ณผ ํŽ˜์–ด๋งํ•˜๊ณ  ์ฝค๋งˆ ํ”„๋ผ์ž„ ์ œ์•ˆ์„ ์š”์ฒญํ•˜์‹ญ์‹œ์˜ค. - + Pair device ์žฅ์น˜ ํŽ˜์–ด๋ง @@ -852,7 +852,7 @@ location set ์˜คํ”„๋ผ์ธ - + ONLINE ์˜จ๋ผ์ธ @@ -861,54 +861,54 @@ location set ์—๋Ÿฌ - + FREE ์ €์žฅ๊ณต๊ฐ„ - - - + + + TEMP ์˜จ๋„ - + HIGH ๋†’์Œ - + GOOD ์ข‹์Œ - + OK OK - + VEHICLE ์ฐจ๋Ÿ‰ - + NO NO - + PANDA ํŒ๋‹ค - + GPS - + SEARCH @@ -916,63 +916,63 @@ location set SoftwarePanel - + Git Branch Git ๋ธŒ๋žœ์น˜ - + Git Commit Git ์ปค๋ฐ‹ - + OS Version OS ๋ฒ„์ „ - + Version ๋ฒ„์ „ - + Last Update Check ๋งˆ์ง€๋ง‰ ์—…๋ฐ์ดํŠธ ํ™•์ธ - + The last time openpilot successfully checked for an update. The updater only runs while the car is off. ๋งˆ์ง€๋ง‰์œผ๋กœ openpilot์ด ์—…๋ฐ์ดํŠธ๋ฅผ ์„ฑ๊ณต์ ์œผ๋กœ ํ™•์ธํ–ˆ์Šต๋‹ˆ๋‹ค. ์—…๋ฐ์ดํŠธ๋Š” ์ฐจ๋Ÿ‰ ์‹œ๋™์ด ๊บผ์ ธ ์žˆ๋Š” ๋™์•ˆ์—๋งŒ ์‹คํ–‰๋ฉ๋‹ˆ๋‹ค. - + Check for Update ์—…๋ฐ์ดํŠธ ํ™•์ธ - + CHECKING ํ™•์ธ์ค‘ - + Switch Branch ๋ธŒ๋žœ์น˜ ๋ณ€๊ฒฝ - + ENTER ์ž…๋ ฅ - - + + The new branch will be pulled the next time the updater runs. ๋‹ค์Œ ์—…๋ฐ์ดํ„ฐ๊ฐ€ ์‹คํ–‰๋  ๋•Œ ํ•ด๋‹น ๋ธŒ๋žœ์น˜๋ฅผ ๊ฐ€์ ธ์˜ต๋‹ˆ๋‹ค. - + Enter branch name ๋ธŒ๋žœ์น˜๋ช…์„ ์ž…๋ ฅํ•˜์„ธ์š” @@ -981,28 +981,28 @@ location set ์ œ๊ฑฐ - + UNINSTALL ์ œ๊ฑฐ - + Uninstall %1 %1 ์ œ - + Are you sure you want to uninstall? ์ œ๊ฑฐํ•˜์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? - + failed to fetch update ์—…๋ฐ์ดํŠธ ํŒจ์น˜์— ์‹คํŒจํ•˜์˜€์Šต๋‹ˆ๋‹ค - - + + CHECK ํ™•์ธ @@ -1010,50 +1010,50 @@ location set SshControl - + SSH Keys SSH ํ‚ค - + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. ๊ฒฝ๊ณ : ์ด๋ ‡๊ฒŒ ํ•˜๋ฉด GitHub ์„ค์ •์˜ ๋ชจ๋“  ๊ณต๊ฐœ ํ‚ค์— SSH ์•ก์„ธ์Šค ๊ถŒํ•œ์ด ๋ถ€์—ฌ๋ฉ๋‹ˆ๋‹ค. ์ž์‹ ์˜ GitHub ์‚ฌ์šฉ์ž ์ด๋ฆ„์ด ์•„๋‹Œ ๋‹ค๋ฅธ ์‚ฌ์šฉ์ž ์ด๋ฆ„์„ ์ž…๋ ฅํ•˜์ง€ ๋งˆ์‹ญ์‹œ์˜ค. ์ฝค๋งˆ ์ง์›์€ GitHub ์‚ฌ์šฉ์ž ์ด๋ฆ„์„ ์ถ”๊ฐ€ํ•˜๋„๋ก ์š”์ฒญํ•˜์ง€ ์•Š์Šต๋‹ˆ๋‹ค. - - + + ADD ์ถ”๊ฐ€ - + Enter your GitHub username GitHub ์•„์ด๋””๋ฅผ ์ž…๋ ฅํ•˜์„ธ์š” - + LOADING ์ฝ์–ด์˜ค๋Š”์ค‘ - + REMOVE ์ œ๊ฑฐ - + Username '%1' has no keys on GitHub '%1'์˜ ํ‚ค๊ฐ€ GitHub์— ์—†์Šต๋‹ˆ๋‹ค - + Request timed out ํƒ€์ž„์•„์›ƒ์ด ๋ฐœ์ƒํ•˜์˜€์Šต๋‹ˆ๋‹ค - + Username '%1' doesn't exist on GitHub '%1'์€ GitHub์˜ ํšŒ์›์ด ์•„๋‹™๋‹ˆ๋‹ค @@ -1061,7 +1061,7 @@ location set SshToggle - + Enable SSH SSH ํ™œ์„ฑํ™” @@ -1069,22 +1069,22 @@ location set TermsPage - + Terms & Conditions ์ด์šฉ์•ฝ๊ด€ - + Decline ๊ฑฐ๋ถ€ - + Scroll to accept ์Šน์ธํ•˜๋ ค๋ฉด ์•„๋ž˜๋กœ ์Šคํฌ๋กคํ•˜์„ธ์š” - + Agree ๋™์˜ @@ -1092,22 +1092,22 @@ location set TogglesPanel - + Enable openpilot ์˜คํ”ˆํŒŒ์ผ๋Ÿฟ ์‚ฌ์šฉ - + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. ์–ด๋Œ‘ํ‹ฐ๋ธŒ ํฌ๋ฃจ์ฆˆ ์ปจํŠธ๋กค ๋ฐ ์ฐจ์„  ์œ ์ง€ ์šด์ „์ž ์ง€์›์„ ์œ„ํ•ด ์˜คํ”ˆ ํŒŒ์ผ๋Ÿฟ ์‹œ์Šคํ…œ์„ ์‚ฌ์šฉํ•˜์‹ญ์‹œ์˜ค. ์ด ๊ธฐ๋Šฅ์„ ์‚ฌ์šฉํ•˜๋ ค๋ฉด ํ•ญ์ƒ ์ฃผ์˜๊ฐ€ ํ•„์š”ํ•ฉ๋‹ˆ๋‹ค. ์ด ์„ค์ •์„ ๋ณ€๊ฒฝํ•˜๋ฉด ์ฐจ๋Ÿ‰์˜ ์ „์›์ด ๊บผ์งˆ ๋•Œ ์ ์šฉ๋ฉ๋‹ˆ๋‹ค. - + Enable Lane Departure Warnings ์ฐจ์„  ์ดํƒˆ ๊ฒฝ๊ณ  ํ™œ์„ฑํ™” - + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 31mph(50km/h) ์ด์ƒ์œผ๋กœ ์šด์ „ํ•˜๋Š” ๋™์•ˆ ๋ฐฉํ–ฅ ์‹ ํ˜ธ๊ฐ€ ํ™œ์„ฑํ™”๋˜์ง€ ์•Š์€ ์ƒํƒœ์—์„œ ์ฐจ๋Ÿ‰์ด ๊ฐ์ง€๋œ ์ฐจ์„ ์„ ๋„˜์–ด๊ฐˆ ๋•Œ ์ฐจ์„ ์œผ๋กœ ๋‹ค์‹œ ์กฐํ–ฅํ•˜๋ผ๋Š” ๊ฒฝ๊ณ ๋ฅผ ๋ฐ›์Šต๋‹ˆ๋‹ค. @@ -1120,57 +1120,71 @@ location set openpilot์ด ์™ผ์ชฝ ๊ตํ†ต ๊ทœ์น™์„ ์ค€์ˆ˜ํ•˜๊ณ  ์˜ค๋ฅธ์ชฝ ์šด์ „์„์—์„œ ์šด์ „์ž ๋ชจ๋‹ˆํ„ฐ๋ง์„ ์ˆ˜ํ–‰ํ•˜๋„๋ก ํ—ˆ์šฉํ•ฉ๋‹ˆ๋‹ค. - + Use Metric System ๋ฏธํ„ฐ๋ฒ• ์‚ฌ์šฉ - + Display speed in km/h instead of mph. mph ๋Œ€์‹  km/h๋กœ ์†๋„๋ฅผ ํ‘œ์‹œํ•ฉ๋‹ˆ๋‹ค. - + Record and Upload Driver Camera ์šด์ „์ž ์นด๋ฉ”๋ผ ๋…นํ™” ๋ฐ ์—…๋กœ๋“œ - + Upload data from the driver facing camera and help improve the driver monitoring algorithm. ์šด์ „์ž๋ฅผ ํ–ฅํ•œ ์นด๋ฉ”๋ผ์—์„œ ๋ฐ์ดํ„ฐ๋ฅผ ์—…๋กœ๋“œํ•˜๊ณ  ์šด์ „์ž ๋ชจ๋‹ˆํ„ฐ๋ง ์•Œ๊ณ ๋ฆฌ์ฆ˜์„ ๊ฐœ์„ ํ•˜๋Š” ๋ฐ ๋„์›€์ด ๋ฉ๋‹ˆ๋‹ค. - + Disable use of lanelines ์ฐจ์„  ์‚ฌ์šฉ ๋น„ํ™œ์„ฑํ™”(End to end) - + In this mode openpilot will ignore lanelines and just drive how it thinks a human would. ์ด ๋ชจ๋“œ์—์„œ openpilot์€ ์ฐจ์„ ์„ ๋ฌด์‹œํ•˜๊ณ  ์‚ฌ๋žŒ์ฒ˜๋Ÿผ ์šด์ „ํ•ฉ๋‹ˆ๋‹ค. - + ๐ŸŒฎ End-to-end longitudinal (extremely alpha) ๐ŸŒฎ ๐ŸŒฎ End-to-end ๊ฐ€๊ฐ์† (๋งค์šฐ ์‹คํ—˜์ ์ธ ๊ธฐ๋Šฅ) ๐ŸŒฎ - + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental. + ๋ชจ๋ธ์—์„œ ๊ฐ€๊ฐ์†์„ ์ œ์–ดํ•˜์—ฌ ์‚ฌ๋žŒ์ด ์ƒ๊ฐํ•˜๋Š”๊ฒƒ์ฒ˜๋Ÿผ ์šด์ „ํ•ฉ๋‹ˆ๋‹ค.(๋งค์šฐ ์‹คํ—˜์ ์ธ ๊ธฐ๋Šฅ) + + Let the driving model control the gas and brakes, openpilot will drive as it thinks a human would. Super experimental. - ๋ชจ๋ธ์—์„œ ๊ฐ€์†๊ณผ ๊ฐ์†์„ ์ œ์–ดํ•˜๋„๋ก ํ•˜์—ฌ ์‚ฌ๋žŒ์ด ์ƒ๊ฐํ•˜๋Š”๊ฑฐ ์ฒ˜๋Ÿผ ์šด์ „ํ•ฉ๋‹ˆ๋‹ค.(๋งค์šฐ ์‹คํ—˜์ ์ธ ๊ธฐ๋Šฅ) + ๋ชจ๋ธ์—์„œ ๊ฐ€์†๊ณผ ๊ฐ์†์„ ์ œ์–ดํ•˜๋„๋ก ํ•˜์—ฌ ์‚ฌ๋žŒ์ด ์ƒ๊ฐํ•˜๋Š”๊ฑฐ ์ฒ˜๋Ÿผ ์šด์ „ํ•ฉ๋‹ˆ๋‹ค.(๋งค์šฐ ์‹คํ—˜์ ์ธ ๊ธฐ๋Šฅ) - + Disengage On Accelerator Pedal ๊ฐ€์† ํŽ˜๋‹ฌ์‹œ ๋””์Šค์ธ๊ฒŒ์ด์ง€ - + When enabled, pressing the accelerator pedal will disengage openpilot. ํ™œ์„ฑํ™”์‹œ, ๊ฐ€์†ํŽ˜๋‹ฌ์„ ๋ฐŸ์œผ๋ฉด ๋””์Šค์ธ๊ฒŒ์ด์ง€ ๋ฉ๋‹ˆ๋‹ค. - + + Experimental openpilot longitudinal control + ์‹คํ—˜์ ์ธ ์˜คํ”ˆํŒŒ์ผ๋Ÿฟ longitudinal ์ œ + + + + <b>WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.</b> + <b>๊ฒฝ๊ณ : ์˜คํ”ˆํŒŒ์ผ๋Ÿฟ์˜ long ์ œ์–ด๋Š” ์‹คํ—˜์ ์ด๋ฉฐ ์ˆœ์ • AEB๊ฐ€ ๋น„ํ™œ์„ฑํ™”๋ฉ๋‹ˆ๋‹ค.</b> + + + Show ETA in 24h Format ๋„์ฐฉ์‹œ๊ฐ„์„ 24์‹œ๊ฐ„์œผ๋กœ ํ‘œ์‹œํ•ฉ๋‹ˆ๋‹ค. @@ -1179,51 +1193,41 @@ location set 24์‹œ๊ฐ„์ œ์œผ๋กœ ํ‘œ์‹œํ•ฉ๋‹ˆ๋‹ค. - + Use 24h format instead of am/pm ์˜ค์ „/์˜คํ›„ ๋Œ€์‹  24์‹œ๊ฐ„ ํ˜•์‹ ์‚ฌ์šฉ - + Show Map on Left Side of UI ์™ผ์ชฝ์— ์ง€๋„ ํ‘œ์‹œ - + Show map on left side when in split screen view. ๋ถ„ํ•  ํ™”๋ฉด์˜ ์™ผ์ชฝ์— ์ง€๋„๋ฅผ ํ‘œ์‹œํ•ฉ๋‹ˆ๋‹ค. - - - openpilot Longitudinal Control - - - - - openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! - - WifiUI - - + + Scanning for networks... ๋„คํŠธ์›Œํฌ๋ฅผ ์ฐพ๋Š” ์ค‘ - + CONNECTING... ์ ‘์†์ค‘... - + FORGET ์ œ๊ฑฐ - + Forget Wi-Fi Network "%1"? "%1"์„ ์ œ๊ฑฐํ•˜์‹œ๊ฒ ์Šต๋‹ˆ๊นŒ? From 8ec6c7c03a1685f682be38608487f3c4a4dfc83d Mon Sep 17 00:00:00 2001 From: neokii Date: Mon, 12 Sep 2022 01:53:23 +0900 Subject: [PATCH 13/15] update limit speed --- cereal/log.capnp | 13 ++++--------- selfdrive/road_speed_limiter.py | 32 +++++++++++++------------------- 2 files changed, 17 insertions(+), 28 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index e10290449d6d71..c549396a168e50 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1901,15 +1901,10 @@ struct RoadLimitSpeed { camLimitSpeed @5 :Int16; sectionLimitSpeed @6 :Int16; sectionLeftDist @7 :Int16; - camSpeedFactor @8 :Float32; - restArea @9 :List(RestArea); - - struct RestArea { - image @0 :Text; - title @1 :Text; - oilPrice @2 :Text; - distance @3 :Text; - } + sectionAvgSpeed @8 :Int16; + sectionLeftTime @9 :Int16; + sectionAdjustSpeed @10 :Bool; + camSpeedFactor @11 :Float32; } struct Event { diff --git a/selfdrive/road_speed_limiter.py b/selfdrive/road_speed_limiter.py index b59e0f7941312d..886a98534e2a37 100644 --- a/selfdrive/road_speed_limiter.py +++ b/selfdrive/road_speed_limiter.py @@ -264,25 +264,11 @@ def main(): dat.roadLimitSpeed.camLimitSpeed = server.get_limit_val("cam_limit_speed", 0) dat.roadLimitSpeed.sectionLimitSpeed = server.get_limit_val("section_limit_speed", 0) dat.roadLimitSpeed.sectionLeftDist = server.get_limit_val("section_left_dist", 0) + dat.roadLimitSpeed.sectionAvgSpeed = server.get_limit_val("section_avg_speed", 0) + dat.roadLimitSpeed.sectionLeftTime = server.get_limit_val("section_left_time", 0) + dat.roadLimitSpeed.sectionAdjustSpeed = server.get_limit_val("section_adjust_speed", 0) dat.roadLimitSpeed.camSpeedFactor = server.get_limit_val("cam_speed_factor", CAMERA_SPEED_FACTOR) - try: - json = server.json_road_limit - if json is not None and "rest_area" in json: - - restAreaList = [] - for rest_area in json["rest_area"]: - restArea = log.RoadLimitSpeed.RestArea.new_message() - restArea.image = server.get_json_val(rest_area, "image") - restArea.title = server.get_json_val(rest_area, "title") - restArea.oilPrice = server.get_json_val(rest_area, "oilPrice") - restArea.distance = server.get_json_val(rest_area, "distance") - restAreaList.append(restArea) - - dat.roadLimitSpeed.restArea = restAreaList - except: - pass - roadLimitSpeed.send(dat.to_bytes()) server.send_sdp(sock) server.check() @@ -333,6 +319,10 @@ def get_max_speed(self, cluster_speed, is_metric): section_limit_speed = self.roadLimitSpeed.sectionLimitSpeed section_left_dist = self.roadLimitSpeed.sectionLeftDist + section_avg_speed = self.roadLimitSpeed.sectionAvgSpeed + section_left_time = self.roadLimitSpeed.sectionLeftTime + section_adjust_speed = self.roadLimitSpeed.sectionAdjustSpeed + camSpeedFactor = clip(self.roadLimitSpeed.camSpeedFactor, 1.0, 1.1) if is_highway is not None: @@ -373,7 +363,7 @@ def get_max_speed(self, cluster_speed, is_metric): td = self.started_dist - safe_dist d = cam_limit_speed_left_dist - safe_dist - if d > 0. and td > 0. and diff_speed > 0. and (section_left_dist is None or section_left_dist < 10): + if d > 0. and td > 0. and diff_speed > 0. and (section_left_dist is None or section_left_dist < 10 or cam_type == 2): pp = (d / td) ** 0.6 else: pp = 0 @@ -393,7 +383,11 @@ def get_max_speed(self, cluster_speed, is_metric): else: first_started = False - return section_limit_speed * camSpeedFactor, section_limit_speed, section_left_dist, first_started, log + speed_diff = 0 + if section_adjust_speed is not None and section_adjust_speed: + speed_diff = (section_limit_speed - section_avg_speed) / 2. + + return section_limit_speed * camSpeedFactor + speed_diff, section_limit_speed, section_left_dist, first_started, log self.slowing_down = False return 0, section_limit_speed, section_left_dist, False, log From 74ad0144d901b18bae54c31e5d16dc5f86f00b0f Mon Sep 17 00:00:00 2001 From: neokii Date: Mon, 12 Sep 2022 02:26:10 +0900 Subject: [PATCH 14/15] fix limit speed --- selfdrive/road_speed_limiter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/road_speed_limiter.py b/selfdrive/road_speed_limiter.py index 886a98534e2a37..5a9997955d4ae1 100644 --- a/selfdrive/road_speed_limiter.py +++ b/selfdrive/road_speed_limiter.py @@ -266,7 +266,7 @@ def main(): dat.roadLimitSpeed.sectionLeftDist = server.get_limit_val("section_left_dist", 0) dat.roadLimitSpeed.sectionAvgSpeed = server.get_limit_val("section_avg_speed", 0) dat.roadLimitSpeed.sectionLeftTime = server.get_limit_val("section_left_time", 0) - dat.roadLimitSpeed.sectionAdjustSpeed = server.get_limit_val("section_adjust_speed", 0) + dat.roadLimitSpeed.sectionAdjustSpeed = server.get_limit_val("section_adjust_speed", False) dat.roadLimitSpeed.camSpeedFactor = server.get_limit_val("cam_speed_factor", CAMERA_SPEED_FACTOR) roadLimitSpeed.send(dat.to_bytes()) From fbfd25e69e30dbd1bb36753f7a5c3c8ede2ba002 Mon Sep 17 00:00:00 2001 From: neokii Date: Mon, 12 Sep 2022 12:15:37 +0900 Subject: [PATCH 15/15] revert safety --- panda/board/safety/safety_hyundai_community.h | 53 +++++++++++++++++-- 1 file changed, 50 insertions(+), 3 deletions(-) diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h index 44d5d3577b22e1..905e31000fd268 100644 --- a/panda/board/safety/safety_hyundai_community.h +++ b/panda/board/safety/safety_hyundai_community.h @@ -198,10 +198,57 @@ static int hyundai_community_tx_hook(CANPacket_t *to_send, bool longitudinal_all // LKA STEER: safety check if (addr == 832) { - int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ffU) - 1024U; - bool steer_req = GET_BIT(to_send, 27U) != 0U; + LKAS11_op = 20; + int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024; + uint32_t ts = microsecond_timer_get(); + bool violation = 0; - if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_STEERING_LIMITS)) { + if (controls_allowed) { + // *** global torque limit check *** + bool torque_check = 0; + violation |= torque_check = max_limit_check(desired_torque, HYUNDAI_STEERING_LIMITS.max_steer, -HYUNDAI_STEERING_LIMITS.max_steer); + if (torque_check) { + puts(" LKAS TX not allowed: torque limit check failed!\n");} + + // *** torque rate limit check *** + bool torque_rate_check = 0; + violation |= torque_rate_check = driver_limit_check(desired_torque, desired_torque_last, &torque_driver, + HYUNDAI_STEERING_LIMITS.max_steer, HYUNDAI_STEERING_LIMITS.max_rate_up, HYUNDAI_STEERING_LIMITS.max_rate_down, + HYUNDAI_STEERING_LIMITS.driver_torque_allowance, HYUNDAI_STEERING_LIMITS.driver_torque_factor); + if (torque_rate_check) { + puts(" LKAS TX not allowed: torque rate limit check failed!\n");} + + // used next time + desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + bool torque_rt_check = 0; + violation |= torque_rt_check = rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_STEERING_LIMITS.max_rt_delta); + if (torque_rt_check) { + puts(" LKAS TX not allowed: torque real time rate limit check failed!\n");} + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last); + if (ts_elapsed > HYUNDAI_STEERING_LIMITS.max_rt_interval) { + rt_torque_last = desired_torque; + ts_torque_check_last = ts; + } + } + + // no torque if controls is not allowed + if (!controls_allowed && (desired_torque != 0)) { + violation = 1; + puts(" LKAS torque not allowed: controls not allowed!\n"); + } + + // reset to 0 if either controls is not allowed or there's a violation + if (!controls_allowed) { // a reset worsen the issue of Panda blocking some valid LKAS messages + desired_torque_last = 0; + rt_torque_last = 0; + ts_torque_check_last = ts; + } + + if (violation) { tx = 0; } }