mirror of
https://github.com/dzid26/sunnypilot.git
synced 2026-06-08 07:44:55 +08:00
sunnypilot modeld: remove thneed modeld (#1731)
* sunnypilot modeld: remove unused modeld * more --------- Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
James Vecellio-Grant
committed by
GitHub
GitHub
parent
60ae57a3ed
commit
de0790f912
@@ -184,7 +184,6 @@ jobs:
|
||||
# Copy the model files
|
||||
rsync -avm \
|
||||
--include='*.dlc' \
|
||||
--include='*.thneed' \
|
||||
--include='*.pkl' \
|
||||
--include='*.onnx' \
|
||||
--exclude='*' \
|
||||
|
||||
2
.gitignore
vendored
2
.gitignore
vendored
@@ -65,8 +65,6 @@ cppcheck_report.txt
|
||||
comma*.sh
|
||||
|
||||
selfdrive/modeld/models/*.pkl*
|
||||
sunnypilot/modeld*/thneed/compile
|
||||
sunnypilot/modeld*/models/*.thneed
|
||||
sunnypilot/modeld*/models/*.pkl
|
||||
|
||||
# openpilot log files
|
||||
|
||||
@@ -31,7 +31,7 @@ from openpilot.common.file_chunker import read_file_chunked
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
|
||||
1
sunnypilot/modeld/.gitignore
vendored
1
sunnypilot/modeld/.gitignore
vendored
@@ -1 +0,0 @@
|
||||
*_pyx.cpp
|
||||
@@ -1,44 +0,0 @@
|
||||
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc')
|
||||
lenv = env.Clone()
|
||||
lenvCython = envCython.Clone()
|
||||
|
||||
libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread']
|
||||
frameworks = []
|
||||
|
||||
common_src = [
|
||||
"models/commonmodel.cc",
|
||||
"transforms/loadyuv.cc",
|
||||
"transforms/transform.cc",
|
||||
]
|
||||
|
||||
thneed_src_common = [
|
||||
"thneed/clutil_legacy.cc",
|
||||
"thneed/thneed_common.cc",
|
||||
"thneed/serialize.cc",
|
||||
]
|
||||
|
||||
thneed_src_qcom = thneed_src_common + ["thneed/thneed_qcom2.cc"]
|
||||
thneed_src_pc = thneed_src_common + ["thneed/thneed_pc.cc"]
|
||||
thneed_src = thneed_src_qcom if arch == "larch64" else thneed_src_pc
|
||||
|
||||
# OpenCL is a framework on Mac
|
||||
if arch == "Darwin":
|
||||
frameworks += ['OpenCL']
|
||||
else:
|
||||
libs += ['OpenCL']
|
||||
|
||||
# Set path definitions
|
||||
for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transforms/loadyuv.cl'}.items():
|
||||
for xenv in (lenv, lenvCython):
|
||||
xenv['CXXFLAGS'].append(f'-D{pathdef}_PATH=\\"{File(fn).abspath}\\"')
|
||||
|
||||
cython_libs = envCython["LIBS"] + libs
|
||||
commonmodel_lib = lenv.Library('commonmodel', common_src)
|
||||
|
||||
lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
|
||||
lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
|
||||
|
||||
if arch == "larch64":
|
||||
thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[common, 'OpenCL', 'dl'])
|
||||
thneedmodel_lib = env.Library('thneedmodel', ['runners/thneedmodel.cc'])
|
||||
lenvCython.Program('runners/thneedmodel_pyx.so', 'runners/thneedmodel_pyx.pyx', LIBS=envCython["LIBS"]+[thneedmodel_lib, thneed_lib, common, 'dl', 'OpenCL'])
|
||||
@@ -1,223 +0,0 @@
|
||||
import os
|
||||
import capnp
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
ConfidenceClass = log.ModelDataV2.ConfidenceClass
|
||||
|
||||
class PublishState:
|
||||
def __init__(self):
|
||||
self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
|
||||
|
||||
def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.z = z.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if z_std is not None:
|
||||
builder.zStd = z_std.tolist()
|
||||
|
||||
def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.v = v.tolist()
|
||||
builder.a = a.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if v_std is not None:
|
||||
builder.vStd = v_std.tolist()
|
||||
if a_std is not None:
|
||||
builder.aStd = a_std.tolist()
|
||||
|
||||
def fill_xyz_poly(builder, degree, x, y, z):
|
||||
xyz = np.stack([x, y, z], axis=1)
|
||||
coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
|
||||
builder.xCoefficients = coeffs[:, 0].tolist()
|
||||
builder.yCoefficients = coeffs[:, 1].tolist()
|
||||
builder.zCoefficients = coeffs[:, 2].tolist()
|
||||
|
||||
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
|
||||
net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState,
|
||||
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
|
||||
timestamp_eof: int, model_execution_time: float, valid: bool,
|
||||
v_ego: float, steer_delay: float, meta_const) -> None:
|
||||
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
|
||||
frame_drop_perc = frame_drop * 100
|
||||
extended_msg.valid = valid
|
||||
base_msg.valid = valid
|
||||
|
||||
if 'lat_planner_solution' in net_output_data:
|
||||
x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0, :, i].tolist() for i in range(4)]
|
||||
x_sol = np.column_stack([x, y, yaw, yawRate])
|
||||
v_ego = max(MIN_SPEED, v_ego)
|
||||
psis = x_sol[0:CONTROL_N, 2].tolist()
|
||||
curvatures = (x_sol[0:CONTROL_N, 3] / v_ego).tolist()
|
||||
desired_curvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures)
|
||||
else:
|
||||
desired_curvature = float(net_output_data['desired_curvature'][0, 0])
|
||||
|
||||
driving_model_data = base_msg.drivingModelData
|
||||
|
||||
driving_model_data.frameId = vipc_frame_id
|
||||
driving_model_data.frameIdExtra = vipc_frame_id_extra
|
||||
driving_model_data.frameDropPerc = frame_drop_perc
|
||||
driving_model_data.modelExecutionTime = model_execution_time
|
||||
|
||||
# Populate drivingModelData.action
|
||||
driving_model_data_action = driving_model_data.action
|
||||
driving_model_data_action.desiredAcceleration = action.desiredAcceleration
|
||||
driving_model_data_action.shouldStop = action.shouldStop
|
||||
driving_model_data_action.desiredCurvature = desired_curvature
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
modelV2.frameIdExtra = vipc_frame_id_extra
|
||||
modelV2.frameAge = frame_age
|
||||
modelV2.frameDropPerc = frame_drop_perc
|
||||
modelV2.timestampEof = timestamp_eof
|
||||
modelV2.modelExecutionTime = model_execution_time
|
||||
|
||||
# plan
|
||||
position = modelV2.position
|
||||
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
|
||||
velocity = modelV2.velocity
|
||||
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
|
||||
acceleration = modelV2.acceleration
|
||||
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
|
||||
orientation = modelV2.orientation
|
||||
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
|
||||
orientation_rate = modelV2.orientationRate
|
||||
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
|
||||
|
||||
# temporal pose
|
||||
temporal_pose = modelV2.temporalPoseDEPRECATED
|
||||
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
|
||||
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
|
||||
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
|
||||
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
|
||||
|
||||
# poly path
|
||||
poly_path = driving_model_data.path
|
||||
fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
|
||||
|
||||
# lateral planning
|
||||
modelV2_action = modelV2.action
|
||||
modelV2_action.desiredAcceleration = action.desiredAcceleration
|
||||
modelV2_action.shouldStop = action.shouldStop
|
||||
modelV2_action.desiredCurvature = desired_curvature
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
|
||||
|
||||
# lane lines
|
||||
modelV2.init('laneLines', 4)
|
||||
for i in range(4):
|
||||
lane_line = modelV2.laneLines[i]
|
||||
fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
|
||||
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
|
||||
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
|
||||
|
||||
lane_line_meta = driving_model_data.laneLineMeta
|
||||
lane_line_meta.leftY = modelV2.laneLines[1].y[0]
|
||||
lane_line_meta.leftProb = modelV2.laneLineProbs[1]
|
||||
lane_line_meta.rightY = modelV2.laneLines[2].y[0]
|
||||
lane_line_meta.rightProb = modelV2.laneLineProbs[2]
|
||||
|
||||
# road edges
|
||||
modelV2.init('roadEdges', 2)
|
||||
for i in range(2):
|
||||
road_edge = modelV2.roadEdges[i]
|
||||
fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
|
||||
modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
|
||||
|
||||
# leads
|
||||
modelV2.init('leadsV3', 3)
|
||||
for i in range(3):
|
||||
lead = modelV2.leadsV3[i]
|
||||
fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
|
||||
lead.prob = net_output_data['lead_prob'][0,i].tolist()
|
||||
lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
|
||||
|
||||
# meta
|
||||
meta = modelV2.meta
|
||||
meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
|
||||
meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
|
||||
meta.engagedProb = net_output_data['meta'][0,meta_const.ENGAGED].item()
|
||||
meta.init('disengagePredictions')
|
||||
disengage_predictions = meta.disengagePredictions
|
||||
disengage_predictions.t = ModelConstants.META_T_IDXS
|
||||
disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,meta_const.BRAKE_DISENGAGE].tolist()
|
||||
disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE].tolist()
|
||||
disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,meta_const.STEER_OVERRIDE].tolist()
|
||||
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_3].tolist()
|
||||
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_4].tolist()
|
||||
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_5].tolist()
|
||||
if 'sim_pose' not in net_output_data:
|
||||
disengage_predictions.gasPressProbs = net_output_data['meta'][0,meta_const.GAS_PRESS].tolist()
|
||||
disengage_predictions.brakePressProbs = net_output_data['meta'][0,meta_const.BRAKE_PRESS].tolist()
|
||||
|
||||
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
|
||||
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,meta_const.HARD_BRAKE_5][0]
|
||||
publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
|
||||
publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,meta_const.HARD_BRAKE_3][0]
|
||||
hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
|
||||
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
|
||||
meta.hardBrakePredicted = hard_brake_predicted.item()
|
||||
|
||||
# confidence
|
||||
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
|
||||
# any disengage prob
|
||||
brake_disengage_probs = net_output_data['meta'][0,meta_const.BRAKE_DISENGAGE]
|
||||
gas_disengage_probs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE]
|
||||
steer_override_probs = net_output_data['meta'][0,meta_const.STEER_OVERRIDE]
|
||||
any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
|
||||
# independent disengage prob for each 2s slice
|
||||
ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
|
||||
# rolling buf for 2, 4, 6, 8, 10s
|
||||
publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
|
||||
publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
|
||||
|
||||
score = 0.
|
||||
for i in range(ModelConstants.DISENGAGE_WIDTH):
|
||||
score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
|
||||
if score < ModelConstants.RYG_GREEN:
|
||||
modelV2.confidence = ConfidenceClass.green
|
||||
elif score < ModelConstants.RYG_YELLOW:
|
||||
modelV2.confidence = ConfidenceClass.yellow
|
||||
else:
|
||||
modelV2.confidence = ConfidenceClass.red
|
||||
|
||||
# raw prediction if enabled
|
||||
if SEND_RAW_PRED:
|
||||
modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
|
||||
|
||||
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray],
|
||||
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
|
||||
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
|
||||
cameraOdometry = msg.cameraOdometry
|
||||
|
||||
cameraOdometry.frameId = vipc_frame_id
|
||||
cameraOdometry.timestampEof = timestamp_eof
|
||||
|
||||
cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
|
||||
cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
|
||||
cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
|
||||
cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()
|
||||
@@ -1,28 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import pathlib
|
||||
import onnx
|
||||
import codecs
|
||||
import pickle
|
||||
|
||||
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
|
||||
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
|
||||
name = value_info.name
|
||||
return name, shape
|
||||
|
||||
if __name__ == "__main__":
|
||||
model_path = pathlib.Path(sys.argv[1])
|
||||
model = onnx.load(str(model_path))
|
||||
i = [x.key for x in model.metadata_props].index('output_slices')
|
||||
output_slices = model.metadata_props[i].value
|
||||
|
||||
metadata = {}
|
||||
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
|
||||
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
|
||||
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
|
||||
|
||||
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
|
||||
with open(metadata_path, 'wb') as f:
|
||||
pickle.dump(metadata, f)
|
||||
|
||||
print(f'saved metadata to {metadata_path}')
|
||||
@@ -1,10 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd "$DIR/../../"
|
||||
|
||||
if [ -f "$DIR/libthneed.so" ]; then
|
||||
export LD_PRELOAD="$DIR/libthneed.so"
|
||||
fi
|
||||
|
||||
exec "$DIR/modeld.py" "$@"
|
||||
@@ -1,348 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car, log
|
||||
from pathlib import Path
|
||||
from setproctitle import setproctitle
|
||||
from cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from opendbc.car.car_helpers import get_demo_car_params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL, config_realtime_process
|
||||
from numpy import interp
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
|
||||
from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
MODEL_PATHS = {
|
||||
ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
|
||||
|
||||
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
|
||||
|
||||
|
||||
class FrameMeta:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
timestamp_eof: int = 0
|
||||
|
||||
def __init__(self, vipc=None):
|
||||
if vipc is not None:
|
||||
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
|
||||
|
||||
class ModelState(ModelStateBase):
|
||||
frame: ModelFrame
|
||||
wide_frame: ModelFrame
|
||||
inputs: dict[str, np.ndarray]
|
||||
output: np.ndarray
|
||||
prev_desire: np.ndarray # for tracking the rising edge of the pulse
|
||||
model: ModelRunner
|
||||
|
||||
def __init__(self, context: CLContext):
|
||||
ModelStateBase.__init__(self)
|
||||
self.frame = ModelFrame(context)
|
||||
self.wide_frame = ModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
bundle = get_active_bundle()
|
||||
overrides = {override.key: override.value for override in bundle.overrides}
|
||||
self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
|
||||
self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
|
||||
|
||||
model_paths = get_model_path()
|
||||
self.model_metadata = load_metadata()
|
||||
self.inputs = prepare_inputs(self.model_metadata)
|
||||
self.meta = load_meta_constants(self.model_metadata)
|
||||
|
||||
self.output_slices = self.model_metadata['output_slices']
|
||||
net_output_size = self.model_metadata['output_shapes']['outputs'][1]
|
||||
self.output = np.zeros(net_output_size, dtype=np.float32)
|
||||
self.parser = Parser()
|
||||
|
||||
self.model = ModelRunner(model_paths, self.output, Runtime.GPU, False, context)
|
||||
self.model.addInput("input_imgs", None)
|
||||
self.model.addInput("big_input_imgs", None)
|
||||
for k,v in self.inputs.items():
|
||||
self.model.addInput(k, v)
|
||||
|
||||
def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]:
|
||||
parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
parsed_model_outputs['raw_pred'] = model_outputs.copy()
|
||||
return parsed_model_outputs
|
||||
|
||||
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire'][0] = 0
|
||||
self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
|
||||
self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
for k in self.inputs:
|
||||
if k in inputs and k != 'desire':
|
||||
self.inputs[k][:] = inputs[k]
|
||||
|
||||
# if getCLBuffer is not None, frame will be None
|
||||
self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
|
||||
if wbuf is not None:
|
||||
self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
|
||||
|
||||
if prepare_only:
|
||||
return None
|
||||
|
||||
self.model.execute()
|
||||
outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
|
||||
|
||||
self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
|
||||
self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
|
||||
|
||||
if "lat_planner_solution" in outputs and "lat_planner_state" in self.inputs.keys():
|
||||
self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2])
|
||||
self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
|
||||
|
||||
if "desired_curvature" in outputs:
|
||||
if "prev_desired_curvs" in self.inputs.keys():
|
||||
self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:]
|
||||
self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0]
|
||||
|
||||
if "prev_desired_curv" in self.inputs.keys():
|
||||
self.inputs['prev_desired_curv'][:-1] = self.inputs['prev_desired_curv'][1:]
|
||||
self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :]
|
||||
return outputs
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
long_action_t: float) -> log.ModelDataV2.Action:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], ModelConstants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
return log.ModelDataV2.Action(desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
cloudlog.warning("modeld init")
|
||||
|
||||
sentry.set_tag("daemon", PROCESS_NAME)
|
||||
cloudlog.bind(daemon=PROCESS_NAME)
|
||||
setproctitle(PROCESS_NAME)
|
||||
config_realtime_process(7, 54)
|
||||
|
||||
cloudlog.warning("setting up CL context")
|
||||
cl_context = CLContext()
|
||||
cloudlog.warning("CL context ready; loading model")
|
||||
model = ModelState(cl_context)
|
||||
cloudlog.warning("models loaded, modeld starting")
|
||||
|
||||
# visionipc clients
|
||||
while True:
|
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False)
|
||||
if available_streams:
|
||||
use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams
|
||||
main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams
|
||||
break
|
||||
time.sleep(.1)
|
||||
|
||||
vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD
|
||||
vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context)
|
||||
vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
|
||||
cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
|
||||
|
||||
while not vipc_client_main.connect(False):
|
||||
time.sleep(0.1)
|
||||
while use_extra_client and not vipc_client_extra.connect(False):
|
||||
time.sleep(0.1)
|
||||
|
||||
cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})")
|
||||
if use_extra_client:
|
||||
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
|
||||
|
||||
# messaging
|
||||
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
|
||||
# setup filter to track dropped frames
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
|
||||
frame_id = 0
|
||||
last_vipc_frame_id = 0
|
||||
run_count = 0
|
||||
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
|
||||
live_calib_seen = False
|
||||
buf_main, buf_extra = None, None
|
||||
meta_main = FrameMeta()
|
||||
meta_extra = FrameMeta()
|
||||
camera_offset_helper = CameraOffsetHelper()
|
||||
|
||||
|
||||
if demo:
|
||||
CP = get_demo_car_params()
|
||||
else:
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
cloudlog.info("modeld got CarParams: %s", CP.brand)
|
||||
|
||||
# Enable lagd support for sunnypilot modeld
|
||||
long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
|
||||
prev_action = log.ModelDataV2.Action()
|
||||
|
||||
DH = DesireHelper()
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
|
||||
buf_main = vipc_client_main.recv()
|
||||
meta_main = FrameMeta(vipc_client_main)
|
||||
if buf_main is None:
|
||||
break
|
||||
|
||||
if buf_main is None:
|
||||
cloudlog.debug("vipc_client_main no frame")
|
||||
continue
|
||||
|
||||
if use_extra_client:
|
||||
# Keep receiving extra frames until frame id matches main camera
|
||||
while True:
|
||||
buf_extra = vipc_client_extra.recv()
|
||||
meta_extra = FrameMeta(vipc_client_extra)
|
||||
if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
|
||||
break
|
||||
|
||||
if buf_extra is None:
|
||||
cloudlog.debug("vipc_client_extra no frame")
|
||||
continue
|
||||
|
||||
if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
|
||||
cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\
|
||||
extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
|
||||
|
||||
else:
|
||||
# Use single camera
|
||||
buf_extra = buf_main
|
||||
meta_extra = meta_main
|
||||
|
||||
sm.update(0)
|
||||
desire = DH.desire
|
||||
v_ego = sm["carState"].vEgo
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["roadCameraState"].frameId
|
||||
if sm.frame % 60 == 0:
|
||||
model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
|
||||
camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
|
||||
lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
|
||||
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
|
||||
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
|
||||
model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
|
||||
live_calib_seen = True
|
||||
|
||||
traffic_convention = np.zeros(2)
|
||||
traffic_convention[int(is_rhd)] = 1
|
||||
|
||||
vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
|
||||
vec_desire[desire] = 1
|
||||
|
||||
# tracked dropped frames
|
||||
vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
|
||||
frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
|
||||
if run_count < 10: # let frame drops warm up
|
||||
frame_dropped_filter.x = 0.
|
||||
frames_dropped = 0.
|
||||
run_count = run_count + 1
|
||||
|
||||
frame_drop_ratio = frames_dropped / (1 + frames_dropped)
|
||||
prepare_only = vipc_dropped_frames > 0
|
||||
if prepare_only:
|
||||
cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
|
||||
|
||||
inputs:dict[str, np.ndarray] = {
|
||||
'desire': vec_desire,
|
||||
'traffic_convention': traffic_convention,
|
||||
}
|
||||
|
||||
if "lateral_control_params" in model.inputs.keys():
|
||||
inputs['lateral_control_params'] = np.array([max(v_ego, 0.), lat_delay], dtype=np.float32)
|
||||
|
||||
if "driving_style" in model.inputs.keys():
|
||||
inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
|
||||
|
||||
if "nav_features" in model.inputs.keys():
|
||||
inputs['nav_features'] = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
|
||||
|
||||
if "nav_instructions" in model.inputs.keys():
|
||||
inputs['nav_instructions'] = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)
|
||||
mt2 = time.perf_counter()
|
||||
model_execution_time = mt2 - mt1
|
||||
|
||||
if model_output is not None:
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
mdv2sp_send = messaging.new_message('modelDataV2SP')
|
||||
action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL)
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
|
||||
v_ego, lat_delay, model.meta)
|
||||
|
||||
desire_state = modelv2_send.modelV2.meta.desireState
|
||||
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
|
||||
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
|
||||
|
||||
fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
|
||||
pm.send('modelV2', modelv2_send)
|
||||
pm.send('drivingModelData', drivingdata_send)
|
||||
pm.send('cameraOdometry', posenet_send)
|
||||
pm.send('modelDataV2SP', mdv2sp_send)
|
||||
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
|
||||
args = parser.parse_args()
|
||||
main(demo=args.demo)
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
|
||||
except Exception:
|
||||
sentry.capture_exception()
|
||||
raise
|
||||
@@ -1,50 +0,0 @@
|
||||
#include "sunnypilot/modeld/models/commonmodel.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
|
||||
input_frames = std::make_unique<float[]>(buf_size);
|
||||
|
||||
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
|
||||
y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_WIDTH * MODEL_HEIGHT, NULL, &err));
|
||||
u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
|
||||
v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
|
||||
net_input_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_FRAME_SIZE * sizeof(float), NULL, &err));
|
||||
|
||||
transform_init(&transform, context, device_id);
|
||||
loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
}
|
||||
|
||||
float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
|
||||
transform_queue(&this->transform, q,
|
||||
yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
|
||||
y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
|
||||
|
||||
if (output == NULL) {
|
||||
loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
|
||||
|
||||
std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
|
||||
CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &input_frames[0];
|
||||
} else {
|
||||
loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
|
||||
// NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
|
||||
clFinish(q);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
ModelFrame::~ModelFrame() {
|
||||
transform_destroy(&transform);
|
||||
loadyuv_destroy(&loadyuv);
|
||||
CL_CHECK(clReleaseMemObject(net_input_cl));
|
||||
CL_CHECK(clReleaseMemObject(v_cl));
|
||||
CL_CHECK(clReleaseMemObject(u_cl));
|
||||
CL_CHECK(clReleaseMemObject(y_cl));
|
||||
CL_CHECK(clReleaseCommandQueue(q));
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cfloat>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
|
||||
#ifdef __APPLE__
|
||||
#include <OpenCL/cl.h>
|
||||
#else
|
||||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include "common/mat.h"
|
||||
#include "sunnypilot/modeld/transforms/loadyuv.h"
|
||||
#include "sunnypilot/modeld/transforms/transform.h"
|
||||
|
||||
class ModelFrame {
|
||||
public:
|
||||
ModelFrame(cl_device_id device_id, cl_context context);
|
||||
~ModelFrame();
|
||||
float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
|
||||
|
||||
const int MODEL_WIDTH = 512;
|
||||
const int MODEL_HEIGHT = 256;
|
||||
const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
|
||||
const int buf_size = MODEL_FRAME_SIZE * 2;
|
||||
|
||||
private:
|
||||
Transform transform;
|
||||
LoadYUVState loadyuv;
|
||||
cl_command_queue q;
|
||||
cl_mem y_cl, u_cl, v_cl, net_input_cl;
|
||||
std::unique_ptr<float[]> input_frames;
|
||||
};
|
||||
@@ -1,18 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
|
||||
|
||||
cdef extern from "common/mat.h":
|
||||
cdef struct mat3:
|
||||
float v[9]
|
||||
|
||||
cdef extern from "common/clutil.h":
|
||||
cdef unsigned long CL_DEVICE_TYPE_DEFAULT
|
||||
cl_device_id cl_get_device_id(unsigned long)
|
||||
cl_context cl_create_context(cl_device_id)
|
||||
|
||||
cdef extern from "sunnypilot/modeld/models/commonmodel.h":
|
||||
cppclass ModelFrame:
|
||||
int buf_size
|
||||
ModelFrame(cl_device_id, cl_context)
|
||||
float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
|
||||
@@ -1,13 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_mem
|
||||
from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
|
||||
|
||||
cdef class CLContext(BaseCLContext):
|
||||
pass
|
||||
|
||||
cdef class CLMem:
|
||||
cdef cl_mem * mem
|
||||
|
||||
@staticmethod
|
||||
cdef create(void*)
|
||||
@@ -1,45 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
import numpy as np
|
||||
cimport numpy as cnp
|
||||
from libc.string cimport memcpy
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_mem
|
||||
from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel cimport mat3, ModelFrame as cppModelFrame
|
||||
|
||||
|
||||
cdef class CLContext(BaseCLContext):
|
||||
def __cinit__(self):
|
||||
self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
|
||||
self.context = cl_create_context(self.device_id)
|
||||
|
||||
cdef class CLMem:
|
||||
@staticmethod
|
||||
cdef create(void * cmem):
|
||||
mem = CLMem()
|
||||
mem.mem = <cl_mem*> cmem
|
||||
return mem
|
||||
|
||||
cdef class ModelFrame:
|
||||
cdef cppModelFrame * frame
|
||||
|
||||
def __cinit__(self, CLContext context):
|
||||
self.frame = new cppModelFrame(context.device_id, context.context)
|
||||
|
||||
def __dealloc__(self):
|
||||
del self.frame
|
||||
|
||||
def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
|
||||
cdef mat3 cprojection
|
||||
memcpy(cprojection.v, &projection[0], 9*sizeof(float))
|
||||
cdef float * data
|
||||
if output is None:
|
||||
data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
|
||||
else:
|
||||
data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
|
||||
if not data:
|
||||
return None
|
||||
return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
|
||||
@@ -1,107 +0,0 @@
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants
|
||||
|
||||
def safe_exp(x, out=None):
|
||||
# -11 is around 10**14, more causes float16 overflow
|
||||
return np.exp(np.clip(x, -np.inf, 11), out=out)
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + safe_exp(-x))
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
safe_exp(x, out=x)
|
||||
else:
|
||||
x = safe_exp(x)
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
|
||||
def check_missing(self, outs, name):
|
||||
if name not in outs and not self.ignore_missing:
|
||||
raise ValueError(f"Missing output {name}")
|
||||
return name not in outs
|
||||
|
||||
def parse_categorical_crossentropy(self, name, outs, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
if out_shape is not None:
|
||||
raw = raw.reshape((raw.shape[0],) + out_shape)
|
||||
outs[name] = softmax(raw, axis=-1)
|
||||
|
||||
def parse_binary_crossentropy(self, name, outs):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
|
||||
if out_N == 1:
|
||||
for fidx in range(weights.shape[0]):
|
||||
idxs = np.argsort(weights[fidx][:,0])[::-1]
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
|
||||
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
for fidx in range(weights.shape[0]):
|
||||
for hidx in range(out_N):
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1:
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
else:
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
|
||||
if 'lat_planner_solution' in outs:
|
||||
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
if 'desired_curvature' in outs:
|
||||
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
|
||||
for k in ['lead_prob', 'lane_lines_prob', 'meta']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
|
||||
return outs
|
||||
@@ -1,22 +0,0 @@
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel_pyx import RunModel, Runtime
|
||||
assert Runtime
|
||||
|
||||
USE_THNEED = int(os.getenv('USE_THNEED', str(int(TICI))))
|
||||
|
||||
class ModelRunner(RunModel):
|
||||
THNEED = 'THNEED'
|
||||
ONNX = 'ONNX'
|
||||
|
||||
def __new__(cls, paths, *args, **kwargs):
|
||||
if ModelRunner.THNEED in paths and USE_THNEED:
|
||||
from openpilot.sunnypilot.modeld.runners.thneedmodel_pyx import ThneedModel as Runner
|
||||
runner_type = ModelRunner.THNEED
|
||||
elif ModelRunner.ONNX in paths:
|
||||
from openpilot.sunnypilot.modeld.runners.onnxmodel import ONNXModel as Runner
|
||||
runner_type = ModelRunner.ONNX
|
||||
else:
|
||||
raise Exception("Couldn't select a model runner, make sure to pass at least one valid model path")
|
||||
|
||||
return Runner(str(paths[runner_type]), *args, **kwargs)
|
||||
@@ -1,98 +0,0 @@
|
||||
import onnx
|
||||
import itertools
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
from typing import Any
|
||||
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel_pyx import RunModel
|
||||
|
||||
ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
|
||||
|
||||
def attributeproto_fp16_to_fp32(attr):
|
||||
float32_list = np.frombuffer(attr.raw_data, dtype=np.float16)
|
||||
attr.data_type = 1
|
||||
attr.raw_data = float32_list.astype(np.float32).tobytes()
|
||||
|
||||
def convert_fp16_to_fp32(onnx_path_or_bytes):
|
||||
if isinstance(onnx_path_or_bytes, bytes):
|
||||
model = onnx.load_from_string(onnx_path_or_bytes)
|
||||
elif isinstance(onnx_path_or_bytes, str):
|
||||
model = onnx.load(onnx_path_or_bytes)
|
||||
|
||||
for i in model.graph.initializer:
|
||||
if i.data_type == 10:
|
||||
attributeproto_fp16_to_fp32(i)
|
||||
for i in itertools.chain(model.graph.input, model.graph.output):
|
||||
if i.type.tensor_type.elem_type == 10:
|
||||
i.type.tensor_type.elem_type = 1
|
||||
for i in model.graph.node:
|
||||
if i.op_type == 'Cast' and i.attribute[0].i == 10:
|
||||
i.attribute[0].i = 1
|
||||
for a in i.attribute:
|
||||
if hasattr(a, 't'):
|
||||
if a.t.data_type == 10:
|
||||
attributeproto_fp16_to_fp32(a.t)
|
||||
return model.SerializeToString()
|
||||
|
||||
def create_ort_session(path, fp16_to_fp32):
|
||||
os.environ["OMP_NUM_THREADS"] = "4"
|
||||
os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
|
||||
|
||||
import onnxruntime as ort
|
||||
print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr)
|
||||
options = ort.SessionOptions()
|
||||
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
|
||||
|
||||
provider: str | tuple[str, dict[Any, Any]]
|
||||
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', {'cudnn_conv_algo_search': 'DEFAULT'})
|
||||
else:
|
||||
options.intra_op_num_threads = 2
|
||||
options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
|
||||
provider = 'CPUExecutionProvider'
|
||||
|
||||
model_data = convert_fp16_to_fp32(path) if fp16_to_fp32 else path
|
||||
print("Onnx selected provider: ", [provider], file=sys.stderr)
|
||||
ort_session = ort.InferenceSession(model_data, options, providers=[provider])
|
||||
print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
|
||||
return ort_session
|
||||
|
||||
|
||||
class ONNXModel(RunModel):
|
||||
def __init__(self, path, output, runtime, use_tf8, cl_context):
|
||||
self.inputs = {}
|
||||
self.output = output
|
||||
|
||||
self.session = create_ort_session(path, fp16_to_fp32=True)
|
||||
self.input_names = [x.name for x in self.session.get_inputs()]
|
||||
self.input_shapes = {x.name: [1, *x.shape[1:]] for x in self.session.get_inputs()}
|
||||
self.input_dtypes = {x.name: ORT_TYPES_TO_NP_TYPES[x.type] for x in self.session.get_inputs()}
|
||||
|
||||
# run once to initialize CUDA provider
|
||||
if "CUDAExecutionProvider" in self.session.get_providers():
|
||||
self.session.run(None, {k: np.zeros(self.input_shapes[k], dtype=self.input_dtypes[k]) for k in self.input_names})
|
||||
print("ready to run onnx model", self.input_shapes, file=sys.stderr)
|
||||
|
||||
def addInput(self, name, buffer):
|
||||
assert name in self.input_names
|
||||
self.inputs[name] = buffer
|
||||
|
||||
def setInputBuffer(self, name, buffer):
|
||||
assert name in self.inputs
|
||||
self.inputs[name] = buffer
|
||||
|
||||
def getCLBuffer(self, name):
|
||||
return None
|
||||
|
||||
def execute(self):
|
||||
inputs = {k: v.view(self.input_dtypes[k]) for k,v in self.inputs.items()}
|
||||
inputs = {k: v.reshape(self.input_shapes[k]).astype(self.input_dtypes[k]) for k,v in inputs.items()}
|
||||
outputs = self.session.run(None, inputs)
|
||||
assert len(outputs) == 1, "Only single model outputs are supported"
|
||||
self.output[:] = outputs[0]
|
||||
return self.output
|
||||
@@ -1,3 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "sunnypilot/modeld/runners/runmodel.h"
|
||||
@@ -1,49 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <cassert>
|
||||
|
||||
#include "common/clutil.h"
|
||||
#include "common/swaglog.h"
|
||||
|
||||
#define USE_CPU_RUNTIME 0
|
||||
#define USE_GPU_RUNTIME 1
|
||||
#define USE_DSP_RUNTIME 2
|
||||
|
||||
struct ModelInput {
|
||||
const std::string name;
|
||||
float *buffer;
|
||||
int size;
|
||||
|
||||
ModelInput(const std::string _name, float *_buffer, int _size) : name(_name), buffer(_buffer), size(_size) {}
|
||||
virtual void setBuffer(float *_buffer, int _size) {
|
||||
assert(size == _size || size == 0);
|
||||
buffer = _buffer;
|
||||
size = _size;
|
||||
}
|
||||
};
|
||||
|
||||
class RunModel {
|
||||
public:
|
||||
std::vector<std::unique_ptr<ModelInput>> inputs;
|
||||
|
||||
virtual ~RunModel() {}
|
||||
virtual void execute() {}
|
||||
virtual void* getCLBuffer(const std::string name) { return nullptr; }
|
||||
|
||||
virtual void addInput(const std::string name, float *buffer, int size) {
|
||||
inputs.push_back(std::unique_ptr<ModelInput>(new ModelInput(name, buffer, size)));
|
||||
}
|
||||
virtual void setInputBuffer(const std::string name, float *buffer, int size) {
|
||||
for (auto &input : inputs) {
|
||||
if (name == input->name) {
|
||||
input->setBuffer(buffer, size);
|
||||
return;
|
||||
}
|
||||
}
|
||||
LOGE("Tried to update input `%s` but no input with this name exists", name.c_str());
|
||||
assert(false);
|
||||
}
|
||||
};
|
||||
@@ -1,14 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from libcpp.string cimport string
|
||||
|
||||
cdef extern from "sunnypilot/modeld/runners/runmodel.h":
|
||||
cdef int USE_CPU_RUNTIME
|
||||
cdef int USE_GPU_RUNTIME
|
||||
cdef int USE_DSP_RUNTIME
|
||||
|
||||
cdef cppclass RunModel:
|
||||
void addInput(string, float*, int)
|
||||
void setInputBuffer(string, float*, int)
|
||||
void * getCLBuffer(string)
|
||||
void execute()
|
||||
@@ -1,6 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from .runmodel cimport RunModel as cppRunModel
|
||||
|
||||
cdef class RunModel:
|
||||
cdef cppRunModel * model
|
||||
@@ -1,37 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from libcpp.string cimport string
|
||||
|
||||
from .runmodel cimport USE_CPU_RUNTIME, USE_GPU_RUNTIME, USE_DSP_RUNTIME
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx cimport CLMem
|
||||
|
||||
class Runtime:
|
||||
CPU = USE_CPU_RUNTIME
|
||||
GPU = USE_GPU_RUNTIME
|
||||
DSP = USE_DSP_RUNTIME
|
||||
|
||||
cdef class RunModel:
|
||||
def __dealloc__(self):
|
||||
del self.model
|
||||
|
||||
def addInput(self, string name, float[:] buffer):
|
||||
if buffer is not None:
|
||||
self.model.addInput(name, &buffer[0], len(buffer))
|
||||
else:
|
||||
self.model.addInput(name, NULL, 0)
|
||||
|
||||
def setInputBuffer(self, string name, float[:] buffer):
|
||||
if buffer is not None:
|
||||
self.model.setInputBuffer(name, &buffer[0], len(buffer))
|
||||
else:
|
||||
self.model.setInputBuffer(name, NULL, 0)
|
||||
|
||||
def getCLBuffer(self, string name):
|
||||
cdef void * cl_buf = self.model.getCLBuffer(name)
|
||||
if not cl_buf:
|
||||
return None
|
||||
return CLMem.create(cl_buf)
|
||||
|
||||
def execute(self):
|
||||
self.model.execute()
|
||||
@@ -1,58 +0,0 @@
|
||||
#include "sunnypilot/modeld/runners/thneedmodel.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
|
||||
ThneedModel::ThneedModel(const std::string path, float *_output, size_t _output_size, int runtime, bool luse_tf8, cl_context context) {
|
||||
thneed = new Thneed(true, context);
|
||||
thneed->load(path.c_str());
|
||||
thneed->clexec();
|
||||
|
||||
recorded = false;
|
||||
output = _output;
|
||||
}
|
||||
|
||||
void* ThneedModel::getCLBuffer(const std::string name) {
|
||||
int index = -1;
|
||||
for (int i = 0; i < inputs.size(); i++) {
|
||||
if (name == inputs[i]->name) {
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (index == -1) {
|
||||
LOGE("Tried to get CL buffer for input `%s` but no input with this name exists", name.c_str());
|
||||
assert(false);
|
||||
}
|
||||
|
||||
if (thneed->input_clmem.size() >= inputs.size()) {
|
||||
return &thneed->input_clmem[inputs.size() - index - 1];
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void ThneedModel::execute() {
|
||||
if (!recorded) {
|
||||
thneed->record = true;
|
||||
float *input_buffers[inputs.size()];
|
||||
for (int i = 0; i < inputs.size(); i++) {
|
||||
input_buffers[inputs.size() - i - 1] = inputs[i]->buffer;
|
||||
}
|
||||
|
||||
thneed->copy_inputs(input_buffers);
|
||||
thneed->clexec();
|
||||
thneed->copy_output(output);
|
||||
thneed->stop();
|
||||
|
||||
recorded = true;
|
||||
} else {
|
||||
float *input_buffers[inputs.size()];
|
||||
for (int i = 0; i < inputs.size(); i++) {
|
||||
input_buffers[inputs.size() - i - 1] = inputs[i]->buffer;
|
||||
}
|
||||
thneed->execute(input_buffers, output);
|
||||
}
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "sunnypilot/modeld/runners/runmodel.h"
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
|
||||
class ThneedModel : public RunModel {
|
||||
public:
|
||||
ThneedModel(const std::string path, float *_output, size_t _output_size, int runtime, bool use_tf8 = false, cl_context context = NULL);
|
||||
void *getCLBuffer(const std::string name);
|
||||
void execute();
|
||||
private:
|
||||
Thneed *thneed = NULL;
|
||||
bool recorded;
|
||||
float *output;
|
||||
};
|
||||
@@ -1,9 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from libcpp.string cimport string
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_context
|
||||
|
||||
cdef extern from "sunnypilot/modeld/runners/thneedmodel.h":
|
||||
cdef cppclass ThneedModel:
|
||||
ThneedModel(string, float*, size_t, int, bool, cl_context)
|
||||
@@ -1,14 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from libcpp cimport bool
|
||||
from libcpp.string cimport string
|
||||
|
||||
from .thneedmodel cimport ThneedModel as cppThneedModel
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx cimport CLContext
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel_pyx cimport RunModel
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel cimport RunModel as cppRunModel
|
||||
|
||||
cdef class ThneedModel(RunModel):
|
||||
def __cinit__(self, string path, float[:] output, int runtime, bool use_tf8, CLContext context):
|
||||
self.model = <cppRunModel *> new cppThneedModel(path, &output[0], len(output), runtime, use_tf8, context.context)
|
||||
@@ -1,8 +0,0 @@
|
||||
thneed is an SNPE accelerator. I know SNPE is already an accelerator, but sometimes things need to go even faster..
|
||||
|
||||
It runs on the local device, and caches a single model run. Then it replays it, but fast.
|
||||
|
||||
thneed slices through abstraction layers like a fish.
|
||||
|
||||
You need a thneed.
|
||||
|
||||
@@ -1,126 +0,0 @@
|
||||
#include "common/clutil.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "sunnypilot/modeld/thneed/clutil_legacy.h"
|
||||
|
||||
void cl_print_build_errors(cl_program program, cl_device_id device) {
|
||||
cl_build_status status;
|
||||
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, NULL);
|
||||
size_t log_size;
|
||||
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
|
||||
std::string log(log_size, '\0');
|
||||
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL);
|
||||
|
||||
LOGE("build failed; status=%d, log: %s", status, log.c_str());
|
||||
}
|
||||
|
||||
cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args) {
|
||||
cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, &binary, NULL, &err));
|
||||
if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
|
||||
cl_print_build_errors(prg, device_id);
|
||||
assert(0);
|
||||
}
|
||||
return prg;
|
||||
}
|
||||
|
||||
// Given a cl code and return a string representation
|
||||
#define CL_ERR_TO_STR(err) case err: return #err
|
||||
const char* cl_get_error_string(int err) {
|
||||
switch (err) {
|
||||
CL_ERR_TO_STR(CL_SUCCESS);
|
||||
CL_ERR_TO_STR(CL_DEVICE_NOT_FOUND);
|
||||
CL_ERR_TO_STR(CL_DEVICE_NOT_AVAILABLE);
|
||||
CL_ERR_TO_STR(CL_COMPILER_NOT_AVAILABLE);
|
||||
CL_ERR_TO_STR(CL_MEM_OBJECT_ALLOCATION_FAILURE);
|
||||
CL_ERR_TO_STR(CL_OUT_OF_RESOURCES);
|
||||
CL_ERR_TO_STR(CL_OUT_OF_HOST_MEMORY);
|
||||
CL_ERR_TO_STR(CL_PROFILING_INFO_NOT_AVAILABLE);
|
||||
CL_ERR_TO_STR(CL_MEM_COPY_OVERLAP);
|
||||
CL_ERR_TO_STR(CL_IMAGE_FORMAT_MISMATCH);
|
||||
CL_ERR_TO_STR(CL_IMAGE_FORMAT_NOT_SUPPORTED);
|
||||
CL_ERR_TO_STR(CL_MAP_FAILURE);
|
||||
CL_ERR_TO_STR(CL_MISALIGNED_SUB_BUFFER_OFFSET);
|
||||
CL_ERR_TO_STR(CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST);
|
||||
CL_ERR_TO_STR(CL_COMPILE_PROGRAM_FAILURE);
|
||||
CL_ERR_TO_STR(CL_LINKER_NOT_AVAILABLE);
|
||||
CL_ERR_TO_STR(CL_LINK_PROGRAM_FAILURE);
|
||||
CL_ERR_TO_STR(CL_DEVICE_PARTITION_FAILED);
|
||||
CL_ERR_TO_STR(CL_KERNEL_ARG_INFO_NOT_AVAILABLE);
|
||||
CL_ERR_TO_STR(CL_INVALID_VALUE);
|
||||
CL_ERR_TO_STR(CL_INVALID_DEVICE_TYPE);
|
||||
CL_ERR_TO_STR(CL_INVALID_PLATFORM);
|
||||
CL_ERR_TO_STR(CL_INVALID_DEVICE);
|
||||
CL_ERR_TO_STR(CL_INVALID_CONTEXT);
|
||||
CL_ERR_TO_STR(CL_INVALID_QUEUE_PROPERTIES);
|
||||
CL_ERR_TO_STR(CL_INVALID_COMMAND_QUEUE);
|
||||
CL_ERR_TO_STR(CL_INVALID_HOST_PTR);
|
||||
CL_ERR_TO_STR(CL_INVALID_MEM_OBJECT);
|
||||
CL_ERR_TO_STR(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);
|
||||
CL_ERR_TO_STR(CL_INVALID_IMAGE_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_SAMPLER);
|
||||
CL_ERR_TO_STR(CL_INVALID_BINARY);
|
||||
CL_ERR_TO_STR(CL_INVALID_BUILD_OPTIONS);
|
||||
CL_ERR_TO_STR(CL_INVALID_PROGRAM);
|
||||
CL_ERR_TO_STR(CL_INVALID_PROGRAM_EXECUTABLE);
|
||||
CL_ERR_TO_STR(CL_INVALID_KERNEL_NAME);
|
||||
CL_ERR_TO_STR(CL_INVALID_KERNEL_DEFINITION);
|
||||
CL_ERR_TO_STR(CL_INVALID_KERNEL);
|
||||
CL_ERR_TO_STR(CL_INVALID_ARG_INDEX);
|
||||
CL_ERR_TO_STR(CL_INVALID_ARG_VALUE);
|
||||
CL_ERR_TO_STR(CL_INVALID_ARG_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_KERNEL_ARGS);
|
||||
CL_ERR_TO_STR(CL_INVALID_WORK_DIMENSION);
|
||||
CL_ERR_TO_STR(CL_INVALID_WORK_GROUP_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_WORK_ITEM_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_GLOBAL_OFFSET);
|
||||
CL_ERR_TO_STR(CL_INVALID_EVENT_WAIT_LIST);
|
||||
CL_ERR_TO_STR(CL_INVALID_EVENT);
|
||||
CL_ERR_TO_STR(CL_INVALID_OPERATION);
|
||||
CL_ERR_TO_STR(CL_INVALID_GL_OBJECT);
|
||||
CL_ERR_TO_STR(CL_INVALID_BUFFER_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_MIP_LEVEL);
|
||||
CL_ERR_TO_STR(CL_INVALID_GLOBAL_WORK_SIZE);
|
||||
CL_ERR_TO_STR(CL_INVALID_PROPERTY);
|
||||
CL_ERR_TO_STR(CL_INVALID_IMAGE_DESCRIPTOR);
|
||||
CL_ERR_TO_STR(CL_INVALID_COMPILER_OPTIONS);
|
||||
CL_ERR_TO_STR(CL_INVALID_LINKER_OPTIONS);
|
||||
CL_ERR_TO_STR(CL_INVALID_DEVICE_PARTITION_COUNT);
|
||||
case -69: return "CL_INVALID_PIPE_SIZE";
|
||||
case -70: return "CL_INVALID_DEVICE_QUEUE";
|
||||
case -71: return "CL_INVALID_SPEC_ID";
|
||||
case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED";
|
||||
case -1002: return "CL_INVALID_D3D10_DEVICE_KHR";
|
||||
case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR";
|
||||
case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR";
|
||||
case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR";
|
||||
case -1006: return "CL_INVALID_D3D11_DEVICE_KHR";
|
||||
case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR";
|
||||
case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR";
|
||||
case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR";
|
||||
case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR";
|
||||
case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR";
|
||||
case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR";
|
||||
case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR";
|
||||
case -1093: return "CL_INVALID_EGL_OBJECT_KHR";
|
||||
case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR";
|
||||
case -1001: return "CL_PLATFORM_NOT_FOUND_KHR";
|
||||
case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT";
|
||||
case -1058: return "CL_INVALID_PARTITION_COUNT_EXT";
|
||||
case -1059: return "CL_INVALID_PARTITION_NAME_EXT";
|
||||
case -1094: return "CL_INVALID_ACCELERATOR_INTEL";
|
||||
case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL";
|
||||
case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL";
|
||||
case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL";
|
||||
case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR";
|
||||
case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL";
|
||||
case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL";
|
||||
case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL";
|
||||
case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL";
|
||||
default: return "CL_UNKNOWN_ERROR";
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/clutil.h"
|
||||
#ifdef __APPLE__
|
||||
#include <OpenCL/cl.h>
|
||||
#else
|
||||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include <string>
|
||||
|
||||
cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
|
||||
const char* cl_get_error_string(int err);
|
||||
@@ -1,155 +0,0 @@
|
||||
#include <cassert>
|
||||
#include <set>
|
||||
|
||||
#include "third_party/json11/json11.hpp"
|
||||
#include "common/util.h"
|
||||
#include "common/clutil.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "sunnypilot/modeld/thneed/clutil_legacy.h"
|
||||
using namespace json11;
|
||||
|
||||
extern map<cl_program, string> g_program_source;
|
||||
|
||||
void Thneed::load(const char *filename) {
|
||||
LOGD("Thneed::load: loading from %s\n", filename);
|
||||
|
||||
string buf = util::read_file(filename);
|
||||
int jsz = *(int *)buf.data();
|
||||
string jsonerr;
|
||||
string jj(buf.data() + sizeof(int), jsz);
|
||||
Json jdat = Json::parse(jj, jsonerr);
|
||||
|
||||
map<cl_mem, cl_mem> real_mem;
|
||||
real_mem[NULL] = NULL;
|
||||
|
||||
int ptr = sizeof(int)+jsz;
|
||||
for (auto &obj : jdat["objects"].array_items()) {
|
||||
auto mobj = obj.object_items();
|
||||
int sz = mobj["size"].int_value();
|
||||
cl_mem clbuf = NULL;
|
||||
|
||||
if (mobj["buffer_id"].string_value().size() > 0) {
|
||||
// image buffer must already be allocated
|
||||
clbuf = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
|
||||
assert(mobj["needs_load"].bool_value() == false);
|
||||
} else {
|
||||
if (mobj["needs_load"].bool_value()) {
|
||||
clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, &buf[ptr], NULL);
|
||||
if (debug >= 1) printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr);
|
||||
ptr += sz;
|
||||
} else {
|
||||
// TODO: is there a faster way to init zeroed out buffers?
|
||||
void *host_zeros = calloc(sz, 1);
|
||||
clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, host_zeros, NULL);
|
||||
free(host_zeros);
|
||||
}
|
||||
}
|
||||
assert(clbuf != NULL);
|
||||
|
||||
if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") {
|
||||
cl_image_desc desc = {0};
|
||||
desc.image_type = (mobj["arg_type"] == "image2d_t") ? CL_MEM_OBJECT_IMAGE2D : CL_MEM_OBJECT_IMAGE1D_BUFFER;
|
||||
desc.image_width = mobj["width"].int_value();
|
||||
desc.image_height = mobj["height"].int_value();
|
||||
desc.image_row_pitch = mobj["row_pitch"].int_value();
|
||||
assert(sz == desc.image_height*desc.image_row_pitch);
|
||||
#ifdef QCOM2
|
||||
desc.buffer = clbuf;
|
||||
#else
|
||||
// TODO: we are creating unused buffers on PC
|
||||
clReleaseMemObject(clbuf);
|
||||
#endif
|
||||
cl_image_format format = {0};
|
||||
format.image_channel_order = CL_RGBA;
|
||||
format.image_channel_data_type = mobj["float32"].bool_value() ? CL_FLOAT : CL_HALF_FLOAT;
|
||||
|
||||
cl_int errcode;
|
||||
|
||||
#ifndef QCOM2
|
||||
if (mobj["needs_load"].bool_value()) {
|
||||
clbuf = clCreateImage(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, &format, &desc, &buf[ptr-sz], &errcode);
|
||||
} else {
|
||||
clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode);
|
||||
}
|
||||
#else
|
||||
clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode);
|
||||
#endif
|
||||
if (clbuf == NULL) {
|
||||
LOGE("clError: %s create image %zux%zu rp %zu with buffer %p\n", cl_get_error_string(errcode),
|
||||
desc.image_width, desc.image_height, desc.image_row_pitch, desc.buffer);
|
||||
}
|
||||
assert(clbuf != NULL);
|
||||
}
|
||||
|
||||
real_mem[*(cl_mem*)(mobj["id"].string_value().data())] = clbuf;
|
||||
}
|
||||
|
||||
map<string, cl_program> g_programs;
|
||||
for (const auto &[name, source] : jdat["programs"].object_items()) {
|
||||
if (debug >= 1) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
|
||||
g_programs[name] = cl_program_from_source(context, device_id, source.string_value());
|
||||
}
|
||||
|
||||
for (auto &obj : jdat["inputs"].array_items()) {
|
||||
auto mobj = obj.object_items();
|
||||
int sz = mobj["size"].int_value();
|
||||
cl_mem aa = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
|
||||
input_clmem.push_back(aa);
|
||||
input_sizes.push_back(sz);
|
||||
LOGD("Thneed::load: adding input %s with size %d\n", mobj["name"].string_value().data(), sz);
|
||||
|
||||
cl_int cl_err;
|
||||
void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &cl_err);
|
||||
if (cl_err != CL_SUCCESS) LOGE("clError: %s map %p %d\n", cl_get_error_string(cl_err), aa, sz);
|
||||
assert(cl_err == CL_SUCCESS);
|
||||
inputs.push_back(ret);
|
||||
}
|
||||
|
||||
for (auto &obj : jdat["outputs"].array_items()) {
|
||||
auto mobj = obj.object_items();
|
||||
int sz = mobj["size"].int_value();
|
||||
LOGD("Thneed::save: adding output with size %d\n", sz);
|
||||
// TODO: support multiple outputs
|
||||
output = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
|
||||
assert(output != NULL);
|
||||
}
|
||||
|
||||
for (auto &obj : jdat["binaries"].array_items()) {
|
||||
string name = obj["name"].string_value();
|
||||
size_t length = obj["length"].int_value();
|
||||
if (debug >= 1) printf("binary %s with size %zu\n", name.c_str(), length);
|
||||
g_programs[name] = cl_program_from_binary(context, device_id, (const uint8_t*)&buf[ptr], length);
|
||||
ptr += length;
|
||||
}
|
||||
|
||||
for (auto &obj : jdat["kernels"].array_items()) {
|
||||
auto gws = obj["global_work_size"];
|
||||
auto lws = obj["local_work_size"];
|
||||
auto kk = shared_ptr<CLQueuedKernel>(new CLQueuedKernel(this));
|
||||
|
||||
kk->name = obj["name"].string_value();
|
||||
kk->program = g_programs[kk->name];
|
||||
kk->work_dim = obj["work_dim"].int_value();
|
||||
for (int i = 0; i < kk->work_dim; i++) {
|
||||
kk->global_work_size[i] = gws[i].int_value();
|
||||
kk->local_work_size[i] = lws[i].int_value();
|
||||
}
|
||||
kk->num_args = obj["num_args"].int_value();
|
||||
for (int i = 0; i < kk->num_args; i++) {
|
||||
string arg = obj["args"].array_items()[i].string_value();
|
||||
int arg_size = obj["args_size"].array_items()[i].int_value();
|
||||
kk->args_size.push_back(arg_size);
|
||||
if (arg_size == 8) {
|
||||
cl_mem val = *(cl_mem*)(arg.data());
|
||||
val = real_mem[val];
|
||||
kk->args.push_back(string((char*)&val, sizeof(val)));
|
||||
} else {
|
||||
kk->args.push_back(arg);
|
||||
}
|
||||
}
|
||||
kq.push_back(kk);
|
||||
}
|
||||
|
||||
clFinish(command_queue);
|
||||
}
|
||||
@@ -1,133 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef __user
|
||||
#define __user __attribute__(())
|
||||
#endif
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <CL/cl.h>
|
||||
|
||||
#include "third_party/linux/include/msm_kgsl.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value);
|
||||
|
||||
namespace json11 {
|
||||
class Json;
|
||||
}
|
||||
class Thneed;
|
||||
|
||||
class GPUMalloc {
|
||||
public:
|
||||
GPUMalloc(int size, int fd);
|
||||
~GPUMalloc();
|
||||
void *alloc(int size);
|
||||
private:
|
||||
uint64_t base;
|
||||
int remaining;
|
||||
};
|
||||
|
||||
class CLQueuedKernel {
|
||||
public:
|
||||
CLQueuedKernel(Thneed *lthneed) { thneed = lthneed; }
|
||||
CLQueuedKernel(Thneed *lthneed,
|
||||
cl_kernel _kernel,
|
||||
cl_uint _work_dim,
|
||||
const size_t *_global_work_size,
|
||||
const size_t *_local_work_size);
|
||||
cl_int exec();
|
||||
void debug_print(bool verbose);
|
||||
int get_arg_num(const char *search_arg_name);
|
||||
cl_program program;
|
||||
string name;
|
||||
cl_uint num_args;
|
||||
vector<string> arg_names;
|
||||
vector<string> arg_types;
|
||||
vector<string> args;
|
||||
vector<int> args_size;
|
||||
cl_kernel kernel = NULL;
|
||||
json11::Json to_json() const;
|
||||
|
||||
cl_uint work_dim;
|
||||
size_t global_work_size[3] = {0};
|
||||
size_t local_work_size[3] = {0};
|
||||
private:
|
||||
Thneed *thneed;
|
||||
};
|
||||
|
||||
class CachedIoctl {
|
||||
public:
|
||||
virtual void exec() {}
|
||||
};
|
||||
|
||||
class CachedSync: public CachedIoctl {
|
||||
public:
|
||||
CachedSync(Thneed *lthneed, string ldata) { thneed = lthneed; data = ldata; }
|
||||
void exec();
|
||||
private:
|
||||
Thneed *thneed;
|
||||
string data;
|
||||
};
|
||||
|
||||
class CachedCommand: public CachedIoctl {
|
||||
public:
|
||||
CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd);
|
||||
void exec();
|
||||
private:
|
||||
void disassemble(int cmd_index);
|
||||
struct kgsl_gpu_command cache;
|
||||
unique_ptr<kgsl_command_object[]> cmds;
|
||||
unique_ptr<kgsl_command_object[]> objs;
|
||||
Thneed *thneed;
|
||||
vector<shared_ptr<CLQueuedKernel> > kq;
|
||||
};
|
||||
|
||||
class Thneed {
|
||||
public:
|
||||
Thneed(bool do_clinit=false, cl_context _context = NULL);
|
||||
void stop();
|
||||
void execute(float **finputs, float *foutput, bool slow=false);
|
||||
void wait();
|
||||
|
||||
vector<cl_mem> input_clmem;
|
||||
vector<void *> inputs;
|
||||
vector<size_t> input_sizes;
|
||||
cl_mem output = NULL;
|
||||
|
||||
cl_context context = NULL;
|
||||
cl_command_queue command_queue;
|
||||
cl_device_id device_id;
|
||||
int context_id;
|
||||
|
||||
// protected?
|
||||
bool record = false;
|
||||
int debug;
|
||||
int timestamp;
|
||||
|
||||
#ifdef QCOM2
|
||||
unique_ptr<GPUMalloc> ram;
|
||||
vector<unique_ptr<CachedIoctl> > cmds;
|
||||
int fd;
|
||||
#endif
|
||||
|
||||
// all CL kernels
|
||||
void copy_inputs(float **finputs, bool internal=false);
|
||||
void copy_output(float *foutput);
|
||||
cl_int clexec();
|
||||
vector<shared_ptr<CLQueuedKernel> > kq;
|
||||
|
||||
// pending CL kernels
|
||||
vector<shared_ptr<CLQueuedKernel> > ckq;
|
||||
|
||||
// loading
|
||||
void load(const char *filename);
|
||||
private:
|
||||
void clinit();
|
||||
};
|
||||
|
||||
@@ -1,216 +0,0 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <map>
|
||||
|
||||
#include "common/clutil.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
map<pair<cl_kernel, int>, string> g_args;
|
||||
map<pair<cl_kernel, int>, int> g_args_size;
|
||||
map<cl_program, string> g_program_source;
|
||||
|
||||
void Thneed::stop() {
|
||||
//printf("Thneed::stop: recorded %lu commands\n", cmds.size());
|
||||
record = false;
|
||||
}
|
||||
|
||||
void Thneed::clinit() {
|
||||
device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
|
||||
if (context == NULL) context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
|
||||
//cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0};
|
||||
cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0};
|
||||
command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err));
|
||||
printf("Thneed::clinit done\n");
|
||||
}
|
||||
|
||||
cl_int Thneed::clexec() {
|
||||
if (debug >= 1) printf("Thneed::clexec: running %lu queued kernels\n", kq.size());
|
||||
for (auto &k : kq) {
|
||||
if (record) ckq.push_back(k);
|
||||
cl_int ret = k->exec();
|
||||
assert(ret == CL_SUCCESS);
|
||||
}
|
||||
return clFinish(command_queue);
|
||||
}
|
||||
|
||||
void Thneed::copy_inputs(float **finputs, bool internal) {
|
||||
for (int idx = 0; idx < inputs.size(); ++idx) {
|
||||
if (debug >= 1) printf("copying %lu -- %p -> %p (cl %p)\n", input_sizes[idx], finputs[idx], inputs[idx], input_clmem[idx]);
|
||||
|
||||
if (internal) {
|
||||
// if it's internal, using memcpy is fine since the buffer sync is cached in the ioctl layer
|
||||
if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
|
||||
} else {
|
||||
if (finputs[idx] != NULL) CL_CHECK(clEnqueueWriteBuffer(command_queue, input_clmem[idx], CL_TRUE, 0, input_sizes[idx], finputs[idx], 0, NULL, NULL));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Thneed::copy_output(float *foutput) {
|
||||
if (output != NULL) {
|
||||
size_t sz;
|
||||
clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
|
||||
if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
|
||||
CL_CHECK(clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL));
|
||||
} else {
|
||||
printf("CAUTION: model output is NULL, does it have no outputs?\n");
|
||||
}
|
||||
}
|
||||
|
||||
// *********** CLQueuedKernel ***********
|
||||
|
||||
CLQueuedKernel::CLQueuedKernel(Thneed *lthneed,
|
||||
cl_kernel _kernel,
|
||||
cl_uint _work_dim,
|
||||
const size_t *_global_work_size,
|
||||
const size_t *_local_work_size) {
|
||||
thneed = lthneed;
|
||||
kernel = _kernel;
|
||||
work_dim = _work_dim;
|
||||
assert(work_dim <= 3);
|
||||
for (int i = 0; i < work_dim; i++) {
|
||||
global_work_size[i] = _global_work_size[i];
|
||||
local_work_size[i] = _local_work_size[i];
|
||||
}
|
||||
|
||||
char _name[0x100];
|
||||
clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(_name), _name, NULL);
|
||||
name = string(_name);
|
||||
clGetKernelInfo(kernel, CL_KERNEL_NUM_ARGS, sizeof(num_args), &num_args, NULL);
|
||||
|
||||
// get args
|
||||
for (int i = 0; i < num_args; i++) {
|
||||
char arg_name[0x100] = {0};
|
||||
clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL);
|
||||
arg_names.push_back(string(arg_name));
|
||||
clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL);
|
||||
arg_types.push_back(string(arg_name));
|
||||
|
||||
args.push_back(g_args[make_pair(kernel, i)]);
|
||||
args_size.push_back(g_args_size[make_pair(kernel, i)]);
|
||||
}
|
||||
|
||||
// get program
|
||||
clGetKernelInfo(kernel, CL_KERNEL_PROGRAM, sizeof(program), &program, NULL);
|
||||
}
|
||||
|
||||
int CLQueuedKernel::get_arg_num(const char *search_arg_name) {
|
||||
for (int i = 0; i < num_args; i++) {
|
||||
if (arg_names[i] == search_arg_name) return i;
|
||||
}
|
||||
printf("failed to find %s in %s\n", search_arg_name, name.c_str());
|
||||
assert(false);
|
||||
}
|
||||
|
||||
cl_int CLQueuedKernel::exec() {
|
||||
if (kernel == NULL) {
|
||||
kernel = clCreateKernel(program, name.c_str(), NULL);
|
||||
arg_names.clear();
|
||||
arg_types.clear();
|
||||
|
||||
for (int j = 0; j < num_args; j++) {
|
||||
char arg_name[0x100] = {0};
|
||||
clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL);
|
||||
arg_names.push_back(string(arg_name));
|
||||
clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL);
|
||||
arg_types.push_back(string(arg_name));
|
||||
|
||||
cl_int ret;
|
||||
if (args[j].size() != 0) {
|
||||
assert(args[j].size() == args_size[j]);
|
||||
ret = thneed_clSetKernelArg(kernel, j, args[j].size(), args[j].data());
|
||||
} else {
|
||||
ret = thneed_clSetKernelArg(kernel, j, args_size[j], NULL);
|
||||
}
|
||||
assert(ret == CL_SUCCESS);
|
||||
}
|
||||
}
|
||||
|
||||
if (thneed->debug >= 1) {
|
||||
debug_print(thneed->debug >= 2);
|
||||
}
|
||||
|
||||
return clEnqueueNDRangeKernel(thneed->command_queue,
|
||||
kernel, work_dim, NULL, global_work_size, local_work_size, 0, NULL, NULL);
|
||||
}
|
||||
|
||||
void CLQueuedKernel::debug_print(bool verbose) {
|
||||
printf("%p %56s -- ", kernel, name.c_str());
|
||||
for (int i = 0; i < work_dim; i++) {
|
||||
printf("%4zu ", global_work_size[i]);
|
||||
}
|
||||
printf(" -- ");
|
||||
for (int i = 0; i < work_dim; i++) {
|
||||
printf("%4zu ", local_work_size[i]);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
if (verbose) {
|
||||
for (int i = 0; i < num_args; i++) {
|
||||
string arg = args[i];
|
||||
printf(" %s %s", arg_types[i].c_str(), arg_names[i].c_str());
|
||||
void *arg_value = (void*)arg.data();
|
||||
int arg_size = arg.size();
|
||||
if (arg_size == 0) {
|
||||
printf(" (size) %d", args_size[i]);
|
||||
} else if (arg_size == 1) {
|
||||
printf(" = %d", *((char*)arg_value));
|
||||
} else if (arg_size == 2) {
|
||||
printf(" = %d", *((short*)arg_value));
|
||||
} else if (arg_size == 4) {
|
||||
if (arg_types[i] == "float") {
|
||||
printf(" = %f", *((float*)arg_value));
|
||||
} else {
|
||||
printf(" = %d", *((int*)arg_value));
|
||||
}
|
||||
} else if (arg_size == 8) {
|
||||
cl_mem val = (cl_mem)(*((uintptr_t*)arg_value));
|
||||
printf(" = %p", val);
|
||||
if (val != NULL) {
|
||||
cl_mem_object_type obj_type;
|
||||
clGetMemObjectInfo(val, CL_MEM_TYPE, sizeof(obj_type), &obj_type, NULL);
|
||||
if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t" || obj_type == CL_MEM_OBJECT_IMAGE2D) {
|
||||
cl_image_format format;
|
||||
size_t width, height, depth, array_size, row_pitch, slice_pitch;
|
||||
cl_mem buf;
|
||||
clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL);
|
||||
assert(format.image_channel_order == CL_RGBA);
|
||||
assert(format.image_channel_data_type == CL_HALF_FLOAT || format.image_channel_data_type == CL_FLOAT);
|
||||
clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL);
|
||||
clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL);
|
||||
clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL);
|
||||
clGetImageInfo(val, CL_IMAGE_DEPTH, sizeof(depth), &depth, NULL);
|
||||
clGetImageInfo(val, CL_IMAGE_ARRAY_SIZE, sizeof(array_size), &array_size, NULL);
|
||||
clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL);
|
||||
assert(depth == 0);
|
||||
assert(array_size == 0);
|
||||
assert(slice_pitch == 0);
|
||||
|
||||
clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL);
|
||||
size_t sz = 0;
|
||||
if (buf != NULL) clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
|
||||
printf(" image %zu x %zu rp %zu @ %p buffer %zu", width, height, row_pitch, buf, sz);
|
||||
} else {
|
||||
size_t sz;
|
||||
clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
|
||||
printf(" buffer %zu", sz);
|
||||
}
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) {
|
||||
g_args_size[make_pair(kernel, arg_index)] = arg_size;
|
||||
if (arg_value != NULL) {
|
||||
g_args[make_pair(kernel, arg_index)] = string((char*)arg_value, arg_size);
|
||||
} else {
|
||||
g_args[make_pair(kernel, arg_index)] = string("");
|
||||
}
|
||||
cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value);
|
||||
return ret;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
@@ -1,258 +0,0 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
|
||||
#include <dlfcn.h>
|
||||
#include <sys/mman.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "common/clutil.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
Thneed *g_thneed = NULL;
|
||||
int g_fd = -1;
|
||||
|
||||
void hexdump(uint8_t *d, int len) {
|
||||
assert((len%4) == 0);
|
||||
printf(" dumping %p len 0x%x\n", d, len);
|
||||
for (int i = 0; i < len/4; i++) {
|
||||
if (i != 0 && (i%0x10) == 0) printf("\n");
|
||||
printf("%8x ", d[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
// *********** ioctl interceptor ***********
|
||||
|
||||
extern "C" {
|
||||
|
||||
int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL;
|
||||
#undef ioctl
|
||||
int ioctl(int filedes, unsigned long request, void *argp) {
|
||||
request &= 0xFFFFFFFF; // needed on QCOM2
|
||||
if (my_ioctl == NULL) my_ioctl = reinterpret_cast<decltype(my_ioctl)>(dlsym(RTLD_NEXT, "ioctl"));
|
||||
Thneed *thneed = g_thneed;
|
||||
|
||||
// save the fd
|
||||
if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes;
|
||||
|
||||
// note that this runs always, even without a thneed object
|
||||
if (request == IOCTL_KGSL_DRAWCTXT_CREATE) {
|
||||
struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp;
|
||||
create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK;
|
||||
create->flags |= 6 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority
|
||||
printf("IOCTL_KGSL_DRAWCTXT_CREATE: creating context with flags 0x%x\n", create->flags);
|
||||
}
|
||||
|
||||
if (thneed != NULL) {
|
||||
if (request == IOCTL_KGSL_GPU_COMMAND) {
|
||||
struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp;
|
||||
if (thneed->record) {
|
||||
thneed->timestamp = cmd->timestamp;
|
||||
thneed->context_id = cmd->context_id;
|
||||
thneed->cmds.push_back(unique_ptr<CachedCommand>(new CachedCommand(thneed, cmd)));
|
||||
}
|
||||
if (thneed->debug >= 1) {
|
||||
printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx context_id: %u timestamp: %u numcmds: %d numobjs: %d\n",
|
||||
thneed->cmds.size(),
|
||||
cmd->flags,
|
||||
cmd->context_id, cmd->timestamp, cmd->numcmds, cmd->numobjs);
|
||||
}
|
||||
} else if (request == IOCTL_KGSL_GPUOBJ_SYNC) {
|
||||
struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp;
|
||||
struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs);
|
||||
|
||||
if (thneed->debug >= 2) {
|
||||
printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count);
|
||||
for (int i = 0; i < cmd->count; i++) {
|
||||
printf(" -- offset:0x%lx len:0x%lx id:%d op:%d ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (thneed->record) {
|
||||
thneed->cmds.push_back(unique_ptr<CachedSync>(new
|
||||
CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count))));
|
||||
}
|
||||
} else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) {
|
||||
struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp;
|
||||
if (thneed->debug >= 1) {
|
||||
printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d timestamp: %d timeout: %d\n",
|
||||
cmd->context_id, cmd->timestamp, cmd->timeout);
|
||||
}
|
||||
} else if (request == IOCTL_KGSL_SETPROPERTY) {
|
||||
if (thneed->debug >= 1) {
|
||||
struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp;
|
||||
printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes);
|
||||
if (thneed->debug >= 2) {
|
||||
hexdump((uint8_t *)prop->value, prop->sizebytes);
|
||||
if (prop->type == KGSL_PROP_PWR_CONSTRAINT) {
|
||||
struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value;
|
||||
hexdump((uint8_t *)constraint->data, constraint->size);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) {
|
||||
// this happens
|
||||
} else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) {
|
||||
// this happens
|
||||
} else {
|
||||
if (thneed->debug >= 1) {
|
||||
printf("other ioctl %lx\n", request);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int ret = my_ioctl(filedes, request, argp);
|
||||
// NOTE: This error message goes into stdout and messes up pyenv
|
||||
// if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// *********** GPUMalloc ***********
|
||||
|
||||
GPUMalloc::GPUMalloc(int size, int fd) {
|
||||
struct kgsl_gpuobj_alloc alloc;
|
||||
memset(&alloc, 0, sizeof(alloc));
|
||||
alloc.size = size;
|
||||
alloc.flags = 0x10000a00;
|
||||
ioctl(fd, IOCTL_KGSL_GPUOBJ_ALLOC, &alloc);
|
||||
void *addr = mmap64(NULL, alloc.mmapsize, 0x3, 0x1, fd, alloc.id*0x1000);
|
||||
assert(addr != MAP_FAILED);
|
||||
|
||||
base = (uint64_t)addr;
|
||||
remaining = size;
|
||||
}
|
||||
|
||||
GPUMalloc::~GPUMalloc() {
|
||||
// TODO: free the GPU malloced area
|
||||
}
|
||||
|
||||
void *GPUMalloc::alloc(int size) {
|
||||
void *ret = (void*)base;
|
||||
size = (size+0xff) & (~0xFF);
|
||||
assert(size <= remaining);
|
||||
remaining -= size;
|
||||
base += size;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// *********** CachedSync, at the ioctl layer ***********
|
||||
|
||||
void CachedSync::exec() {
|
||||
struct kgsl_gpuobj_sync cmd;
|
||||
|
||||
cmd.objs = (uint64_t)data.data();
|
||||
cmd.obj_len = data.length();
|
||||
cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj);
|
||||
|
||||
int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd);
|
||||
assert(ret == 0);
|
||||
}
|
||||
|
||||
// *********** CachedCommand, at the ioctl layer ***********
|
||||
|
||||
CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) {
|
||||
thneed = lthneed;
|
||||
assert(cmd->numsyncs == 0);
|
||||
|
||||
memcpy(&cache, cmd, sizeof(cache));
|
||||
|
||||
if (cmd->numcmds > 0) {
|
||||
cmds = make_unique<struct kgsl_command_object[]>(cmd->numcmds);
|
||||
memcpy(cmds.get(), (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*cmd->numcmds);
|
||||
cache.cmdlist = (uint64_t)cmds.get();
|
||||
for (int i = 0; i < cmd->numcmds; i++) {
|
||||
void *nn = thneed->ram->alloc(cmds[i].size);
|
||||
memcpy(nn, (void*)cmds[i].gpuaddr, cmds[i].size);
|
||||
cmds[i].gpuaddr = (uint64_t)nn;
|
||||
}
|
||||
}
|
||||
|
||||
if (cmd->numobjs > 0) {
|
||||
objs = make_unique<struct kgsl_command_object[]>(cmd->numobjs);
|
||||
memcpy(objs.get(), (void *)cmd->objlist, sizeof(struct kgsl_command_object)*cmd->numobjs);
|
||||
cache.objlist = (uint64_t)objs.get();
|
||||
for (int i = 0; i < cmd->numobjs; i++) {
|
||||
void *nn = thneed->ram->alloc(objs[i].size);
|
||||
memset(nn, 0, objs[i].size);
|
||||
objs[i].gpuaddr = (uint64_t)nn;
|
||||
}
|
||||
}
|
||||
|
||||
kq = thneed->ckq;
|
||||
thneed->ckq.clear();
|
||||
}
|
||||
|
||||
void CachedCommand::exec() {
|
||||
cache.timestamp = ++thneed->timestamp;
|
||||
int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache);
|
||||
|
||||
if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret);
|
||||
|
||||
if (thneed->debug >= 2) {
|
||||
for (auto &it : kq) {
|
||||
it->debug_print(false);
|
||||
}
|
||||
}
|
||||
|
||||
assert(ret == 0);
|
||||
}
|
||||
|
||||
// *********** Thneed ***********
|
||||
|
||||
Thneed::Thneed(bool do_clinit, cl_context _context) {
|
||||
// TODO: QCOM2 actually requires a different context
|
||||
//context = _context;
|
||||
if (do_clinit) clinit();
|
||||
assert(g_fd != -1);
|
||||
fd = g_fd;
|
||||
ram = make_unique<GPUMalloc>(0x80000, fd);
|
||||
timestamp = -1;
|
||||
g_thneed = this;
|
||||
char *thneed_debug_env = getenv("THNEED_DEBUG");
|
||||
debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0;
|
||||
}
|
||||
|
||||
void Thneed::wait() {
|
||||
struct kgsl_device_waittimestamp_ctxtid wait;
|
||||
wait.context_id = context_id;
|
||||
wait.timestamp = timestamp;
|
||||
wait.timeout = -1;
|
||||
|
||||
uint64_t tb = nanos_since_boot();
|
||||
int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait);
|
||||
uint64_t te = nanos_since_boot();
|
||||
|
||||
if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
|
||||
}
|
||||
|
||||
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, true);
|
||||
|
||||
// ****** run commands
|
||||
int i = 0;
|
||||
for (auto &it : cmds) {
|
||||
++i;
|
||||
if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
|
||||
it->exec();
|
||||
if ((i == cmds.size()) || slow) wait();
|
||||
}
|
||||
|
||||
// ****** copy outputs
|
||||
copy_output(foutput);
|
||||
|
||||
if (debug >= 1) {
|
||||
te = nanos_since_boot();
|
||||
printf("model exec in %lu us\n", (te-tb)/1000);
|
||||
}
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
#include "sunnypilot/modeld/transforms/loadyuv.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
|
||||
void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) {
|
||||
memset(s, 0, sizeof(*s));
|
||||
|
||||
s->width = width;
|
||||
s->height = height;
|
||||
|
||||
char args[1024];
|
||||
snprintf(args, sizeof(args),
|
||||
"-cl-fast-relaxed-math -cl-denorms-are-zero "
|
||||
"-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d",
|
||||
width, height);
|
||||
cl_program prg = cl_program_from_file(ctx, device_id, LOADYUV_PATH, args);
|
||||
|
||||
s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
|
||||
s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
|
||||
s->copy_krnl = CL_CHECK_ERR(clCreateKernel(prg, "copy", &err));
|
||||
|
||||
// done with this
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
}
|
||||
|
||||
void loadyuv_destroy(LoadYUVState* s) {
|
||||
CL_CHECK(clReleaseKernel(s->loadys_krnl));
|
||||
CL_CHECK(clReleaseKernel(s->loaduv_krnl));
|
||||
CL_CHECK(clReleaseKernel(s->copy_krnl));
|
||||
}
|
||||
|
||||
void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
|
||||
cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
|
||||
cl_mem out_cl, bool do_shift) {
|
||||
cl_int global_out_off = 0;
|
||||
if (do_shift) {
|
||||
// shift the image in slot 1 to slot 0, then place the new image in slot 1
|
||||
global_out_off += (s->width*s->height) + (s->width/2)*(s->height/2)*2;
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_int), &global_out_off));
|
||||
const size_t copy_work_size = global_out_off/8;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
|
||||
©_work_size, NULL, 0, 0, NULL));
|
||||
}
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
const size_t loadys_work_size = (s->width*s->height)/8;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
|
||||
&loadys_work_size, NULL, 0, 0, NULL));
|
||||
|
||||
const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
|
||||
global_out_off += (s->width*s->height);
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
|
||||
&loaduv_work_size, NULL, 0, 0, NULL));
|
||||
|
||||
global_out_off += (s->width/2)*(s->height/2);
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
|
||||
&loaduv_work_size, NULL, 0, 0, NULL));
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
|
||||
|
||||
__kernel void loadys(__global uchar8 const * const Y,
|
||||
__global float * out,
|
||||
int out_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
const int ois = gid * 8;
|
||||
const int oy = ois / TRANSFORMED_WIDTH;
|
||||
const int ox = ois % TRANSFORMED_WIDTH;
|
||||
|
||||
const uchar8 ys = Y[gid];
|
||||
const float8 ysf = convert_float8(ys);
|
||||
|
||||
// 02
|
||||
// 13
|
||||
|
||||
__global float* outy0;
|
||||
__global float* outy1;
|
||||
if ((oy & 1) == 0) {
|
||||
outy0 = out + out_offset; //y0
|
||||
outy1 = out + out_offset + UV_SIZE*2; //y2
|
||||
} else {
|
||||
outy0 = out + out_offset + UV_SIZE; //y1
|
||||
outy1 = out + out_offset + UV_SIZE*3; //y3
|
||||
}
|
||||
|
||||
vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
|
||||
vstore4(ysf.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
|
||||
}
|
||||
|
||||
__kernel void loaduv(__global uchar8 const * const in,
|
||||
__global float8 * out,
|
||||
int out_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
const uchar8 inv = in[gid];
|
||||
const float8 outv = convert_float8(inv);
|
||||
out[gid + out_offset / 8] = outv;
|
||||
}
|
||||
|
||||
__kernel void copy(__global float8 * inout,
|
||||
int in_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
inout[gid] = inout[gid + in_offset / 8];
|
||||
}
|
||||
@@ -1,16 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
typedef struct {
|
||||
int width, height;
|
||||
cl_kernel loadys_krnl, loaduv_krnl, copy_krnl;
|
||||
} LoadYUVState;
|
||||
|
||||
void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height);
|
||||
|
||||
void loadyuv_destroy(LoadYUVState* s);
|
||||
|
||||
void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
|
||||
cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
|
||||
cl_mem out_cl, bool do_shift = false);
|
||||
@@ -1,97 +0,0 @@
|
||||
#include "sunnypilot/modeld/transforms/transform.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
|
||||
memset(s, 0, sizeof(*s));
|
||||
|
||||
cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, "");
|
||||
s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err));
|
||||
// done with this
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
|
||||
s->m_y_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
|
||||
s->m_uv_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
|
||||
}
|
||||
|
||||
void transform_destroy(Transform* s) {
|
||||
CL_CHECK(clReleaseMemObject(s->m_y_cl));
|
||||
CL_CHECK(clReleaseMemObject(s->m_uv_cl));
|
||||
CL_CHECK(clReleaseKernel(s->krnl));
|
||||
}
|
||||
|
||||
void transform_queue(Transform* s,
|
||||
cl_command_queue q,
|
||||
cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
|
||||
cl_mem out_y, cl_mem out_u, cl_mem out_v,
|
||||
int out_width, int out_height,
|
||||
const mat3& projection) {
|
||||
const int zero = 0;
|
||||
|
||||
// sampled using pixel center origin
|
||||
// (because that's how fastcv and opencv does it)
|
||||
|
||||
mat3 projection_y = projection;
|
||||
|
||||
// in and out uv is half the size of y.
|
||||
mat3 projection_uv = transform_scale_buffer(projection, 0.5);
|
||||
|
||||
CL_CHECK(clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL));
|
||||
CL_CHECK(clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL));
|
||||
|
||||
const int in_y_width = in_width;
|
||||
const int in_y_height = in_height;
|
||||
const int in_y_px_stride = 1;
|
||||
const int in_uv_width = in_width/2;
|
||||
const int in_uv_height = in_height/2;
|
||||
const int in_uv_px_stride = 2;
|
||||
const int in_u_offset = in_uv_offset;
|
||||
const int in_v_offset = in_uv_offset + 1;
|
||||
|
||||
const int out_y_width = out_width;
|
||||
const int out_y_height = out_height;
|
||||
const int out_uv_width = out_width/2;
|
||||
const int out_uv_height = out_height/2;
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); // src
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride)); // src_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride)); // src_px_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero)); // src_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height)); // src_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width)); // src_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y)); // dst
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width)); // dst_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height)); // dst_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width)); // dst_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl)); // M
|
||||
|
||||
const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_y, NULL, 0, 0, NULL));
|
||||
|
||||
const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride)); // src_px_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset)); // src_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height)); // src_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width)); // src_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u)); // dst
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width)); // dst_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height)); // dst_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width)); // dst_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl)); // M
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_uv, NULL, 0, 0, NULL));
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset)); // src_ofset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v)); // dst
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_uv, NULL, 0, 0, NULL));
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
#define INTER_BITS 5
|
||||
#define INTER_TAB_SIZE (1 << INTER_BITS)
|
||||
#define INTER_SCALE 1.f / INTER_TAB_SIZE
|
||||
|
||||
#define INTER_REMAP_COEF_BITS 15
|
||||
#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
|
||||
|
||||
__kernel void warpPerspective(__global const uchar * src,
|
||||
int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols,
|
||||
__global uchar * dst,
|
||||
int dst_row_stride, int dst_offset, int dst_rows, int dst_cols,
|
||||
__constant float * M)
|
||||
{
|
||||
int dx = get_global_id(0);
|
||||
int dy = get_global_id(1);
|
||||
|
||||
if (dx < dst_cols && dy < dst_rows)
|
||||
{
|
||||
float X0 = M[0] * dx + M[1] * dy + M[2];
|
||||
float Y0 = M[3] * dx + M[4] * dy + M[5];
|
||||
float W = M[6] * dx + M[7] * dy + M[8];
|
||||
W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
|
||||
int X = rint(X0 * W), Y = rint(Y0 * W);
|
||||
|
||||
int sx = convert_short_sat(X >> INTER_BITS);
|
||||
int sy = convert_short_sat(Y >> INTER_BITS);
|
||||
|
||||
short sx_clamp = clamp(sx, 0, src_cols - 1);
|
||||
short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1);
|
||||
short sy_clamp = clamp(sy, 0, src_rows - 1);
|
||||
short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1);
|
||||
int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
|
||||
int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
|
||||
int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
|
||||
int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
|
||||
|
||||
short ay = (short)(Y & (INTER_TAB_SIZE - 1));
|
||||
short ax = (short)(X & (INTER_TAB_SIZE - 1));
|
||||
float taby = 1.f/INTER_TAB_SIZE*ay;
|
||||
float tabx = 1.f/INTER_TAB_SIZE*ax;
|
||||
|
||||
int dst_index = mad24(dy, dst_row_stride, dst_offset + dx);
|
||||
|
||||
int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
|
||||
int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
|
||||
int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
|
||||
int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
|
||||
|
||||
int val = v0 * itab0 + v1 * itab1 + v2 * itab2 + v3 * itab3;
|
||||
|
||||
uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
|
||||
dst[dst_index] = pix;
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
|
||||
#ifdef __APPLE__
|
||||
#include <OpenCL/cl.h>
|
||||
#else
|
||||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include "common/mat.h"
|
||||
|
||||
typedef struct {
|
||||
cl_kernel krnl;
|
||||
cl_mem m_y_cl, m_uv_cl;
|
||||
} Transform;
|
||||
|
||||
void transform_init(Transform* s, cl_context ctx, cl_device_id device_id);
|
||||
|
||||
void transform_destroy(Transform* transform);
|
||||
|
||||
void transform_queue(Transform* s, cl_command_queue q,
|
||||
cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
|
||||
cl_mem out_y, cl_mem out_u, cl_mem out_v,
|
||||
int out_width, int out_height,
|
||||
const mat3& projection);
|
||||
@@ -31,7 +31,7 @@ from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
|
||||
|
||||
|
||||
@@ -12,7 +12,7 @@ import numpy as np
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.modeld.constants import Meta, MetaTombRaider, MetaSimPose
|
||||
from openpilot.sunnypilot.models.constants import Meta, MetaTombRaider, MetaSimPose
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
@@ -14,7 +14,7 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
|
||||
|
||||
|
||||
@@ -1,27 +0,0 @@
|
||||
from numpy import clip, interp
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, MAX_LATERAL_JERK
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants
|
||||
|
||||
|
||||
def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures):
|
||||
if len(psis) != CONTROL_N:
|
||||
psis = [0.0]*CONTROL_N
|
||||
curvatures = [0.0]*CONTROL_N
|
||||
v_ego = max(MIN_SPEED, v_ego)
|
||||
|
||||
# MPC can plan to turn the wheel and turn back before t_delay. This means
|
||||
# in high delay cases some corrections never even get commanded. So just use
|
||||
# psi to calculate a simple linearization of desired curvature
|
||||
current_curvature_desired = curvatures[0]
|
||||
psi = interp(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
|
||||
average_curvature_desired = psi / (v_ego * steer_delay)
|
||||
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
|
||||
|
||||
# This is the "desired rate of the setpoint" not an actual desired rate
|
||||
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
||||
safe_desired_curvature = clip(desired_curvature,
|
||||
current_curvature_desired - max_curvature_rate * DT_MDL,
|
||||
current_curvature_desired + max_curvature_rate * DT_MDL)
|
||||
|
||||
return float(safe_desired_curvature)
|
||||
Reference in New Issue
Block a user