mawei
/
sunnypilot
mirror of https://github.com/sunnypilot/sunnypilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Driving Model Selector v5

This commit is contained in:
Jason Wen 2024-07-07 12:21:03 -04:00
parent d3cb0d24c4
commit b5227c0a06
No known key found for this signature in database
GPG Key ID: EF8EA444C1E7B69C
19 changed files with 154 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cereal/custom.capnp
View File

@ -25,6 +25,15 @@ enum AccelerationPersonality {
stock @3;
}
enum ModelGeneration {
default @0;
one @1;
two @2;
three @3;
four @4;
five @5;
}
struct ControlsStateSP @0x81c2f05a394cf4af {
lateralState @0 :Text;
personality @8 :LongitudinalPersonalitySP;
@ -189,6 +198,9 @@ struct E2eLongStateSP @0xa5cd762cd951a455 {
struct ModelDataV2SP @0xf98d843bfd7004a3 {
laneChangePrev @0 :Bool;
laneChangeEdgeBlock @1 :Bool;
customModel @2 :Bool;
modelGeneration @3 :ModelGeneration;
modelCapabilities @4 :UInt32;
}
struct CustomReserved7 @0xb86e6369214c01c8 {

9
selfdrive/controls/controlsd.py
View File

@ -28,8 +28,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
from openpilot.system.athena.registration import is_registered_device
from openpilot.system.hardware import HARDWARE
@ -182,9 +181,9 @@ class Controls:
self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
self.process_not_running = False
self.custom_model, self.model_gen = get_model_generation(self.params)
model_capabilities = ModelCapabilities.get_by_gen(self.model_gen)
self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution
self.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True)
self.model_use_lateral_planner = self.custom_model_metadata.valid and \
self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution
self.dynamic_personality = self.params.get_bool("DynamicPersonality")

9
selfdrive/controls/lib/desire_helper.py
View File

@ -3,8 +3,7 @@ from openpilot.common.conversions import Conversions as CV
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.drive_helpers import get_road_edge
from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
@ -69,9 +68,9 @@ class DesireHelper:
self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8"))
self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay")
self.custom_model, self.model_gen = get_model_generation(self.param_s)
model_capabilities = ModelCapabilities.get_by_gen(self.model_gen)
self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution
self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True)
self.model_use_lateral_planner = self.custom_model_metadata.valid and \
self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution
def read_param(self):
self.edge_toggle = self.param_s.get_bool("RoadEdge")

8
selfdrive/controls/plannerd.py
View File

@ -8,8 +8,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
import cereal.messaging as messaging
@ -26,9 +25,8 @@ def plannerd_thread():
longitudinal_planner = LongitudinalPlanner(CP)
custom_model, model_gen = get_model_generation(params)
model_capabilities = ModelCapabilities.get_by_gen(model_gen)
model_use_lateral_planner = custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution
custom_model_metadata = CustomModelMetadata(params=params, init_only=True)
model_use_lateral_planner = custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution
lateral_planner = LateralPlanner(CP, debug=debug_mode, model_use_lateral_planner=model_use_lateral_planner)
lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED']

66
selfdrive/modeld/custom_model_metadata.py Normal file
View File

@ -0,0 +1,66 @@
from enum import IntFlag
import os
from cereal import custom
from openpilot.common.params import Params
SIMULATION = "SIMULATION" in os.environ
ModelGeneration = custom.ModelGeneration
class ModelCapabilities(IntFlag):
"""Model capabilities for different generations of models."""
Default = 1
"""Default capability, used for the prebuilt model."""
NoO = 2
"""Navigation on Openpilot capability, used for models support navigation."""
LateralPlannerSolution = 2 ** 2
"""LateralPlannerSolution capability, used for models that support the lateral planner solution."""
DesiredCurvatureV1 = 2 ** 3
"""
DesiredCurvatureV1 capability: This capability is used for models that support the desired curvature.
In this version, 'prev_desired_curvs' is used as the input for the 'desired_curvature' output.
"""
DesiredCurvatureV2 = 2 ** 4
"""
DesiredCurvatureV2 capability: This capability is used for models that support the desired curvature.
In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output.
"""
class CustomModelMetadata:
def __init__(self, params=None, init_only=False) -> None:
# TODO: Handle this with cereal
if not init_only:
raise RuntimeError("cannot be used in a loop, this should only be used on init")
self.params: Params = params
self.generation: ModelGeneration = self.read_model_generation_param()
self.capabilities: int = self.get_model_capabilities()
self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \
self.capabilities != ModelCapabilities.Default
def read_model_generation_param(self) -> ModelGeneration:
return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default)
def get_model_capabilities(self) -> int:
"""Returns the model capabilities for a given generation."""
if self.generation == ModelGeneration.five:
return ModelCapabilities.DesiredCurvatureV2
elif self.generation == ModelGeneration.four:
return ModelCapabilities.DesiredCurvatureV2
elif self.generation == ModelGeneration.three:
return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO
elif self.generation == ModelGeneration.two:
return ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO
elif self.generation == ModelGeneration.one:
return ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO
else:
# Default model is meant to represent the capabilities of the prebuilt model
return ModelCapabilities.Default

6
selfdrive/modeld/fill_model_msg.py
View File

@ -3,6 +3,7 @@ import capnp
import numpy as np
from cereal import log
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
from openpilot.selfdrive.modeld.custom_model_metadata import ModelCapabilities
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@ -54,7 +55,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
nav_enabled: bool, valid: bool,
model_use_lateral_planner: bool, model_use_nav: bool) -> None:
custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> 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
@ -75,6 +76,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
modelV2.frameAge = frame_age
modelV2.frameDropPerc = frame_drop_perc
modelV2.timestampEof = timestamp_eof
model_use_nav = custom_model_valid and custom_model_capabilities & ModelCapabilities.NoO
if model_use_nav:
modelV2.locationMonoTimeDEPRECATED = timestamp_llk
modelV2.modelExecutionTime = model_execution_time
@ -98,7 +100,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
# lateral planning
if model_use_lateral_planner:
if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution:
solution = modelV2.lateralPlannerSolutionDEPRECATED
solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]

41
selfdrive/modeld/model_capabilities.py
View File

@ -1,41 +0,0 @@
from enum import IntFlag, auto
class ModelCapabilities(IntFlag):
"""Model capabilities for different generations of models."""
Default = auto()
"""Default capability, used for the prebuilt model."""
NoO = auto()
"""Navigation on Openpilot capability, used for models support navigation."""
LateralPlannerSolution = auto()
"""LateralPlannerSolution capability, used for models that support the lateral planner solution."""
DesiredCurvatureV1 = auto()
"""
DesiredCurvatureV1 capability: This capability is used for models that support the desired curvature.
In this version, 'prev_desired_curvs' is used as the input for the 'desired_curvature' output.
"""
DesiredCurvatureV2 = auto()
"""
DesiredCurvatureV2 capability: This capability is used for models that support the desired curvature.
In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output.
"""
@staticmethod
def get_by_gen(gen):
"""Returns the model capabilities for a given generation."""
if gen == 1:
return ModelCapabilities.Default | ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO
elif gen == 2:
return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO
elif gen == 3:
return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO
elif gen == 4:
return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2
else:
# Default model is meant to represent the capabilities of the prebuilt model.
return ModelCapabilities.Default

62
selfdrive/modeld/modeld.py
View File

@ -20,13 +20,12 @@ from openpilot.common.transformations.model import get_warp_matrix
from openpilot.system import sentry
from openpilot.selfdrive.car.car_helpers import get_demo_car_params
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.system.hardware.hw import Paths
THREAD_NAME = "selfdrive.modeld.modeld"
@ -62,8 +61,7 @@ class ModelState:
def __init__(self, context: CLContext):
self.param_s = Params()
self.custom_model, self.model_gen = get_model_generation(self.param_s)
self.model_capabilities = ModelCapabilities.get_by_gen(self.model_gen)
self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True)
self.frame = ModelFrame(context)
self.wide_frame = ModelFrame(context)
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
@ -73,17 +71,17 @@ class ModelState:
'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
}
_inputs_2 = {}
if self.custom_model and self.model_capabilities != ModelCapabilities.Default:
if self.model_capabilities & ModelCapabilities.LateralPlannerSolution:
if self.custom_model_metadata.valid:
if self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution:
_inputs = {
'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32),
}
if self.model_capabilities & ModelCapabilities.DesiredCurvatureV1:
if self.custom_model_metadata.capabilities & ModelCapabilities.DesiredCurvatureV1:
_inputs = {
'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32),
}
if self.model_capabilities & ModelCapabilities.NoO:
if self.custom_model_metadata.capabilities & ModelCapabilities.NoO:
_inputs_2 = {
'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
@ -97,7 +95,7 @@ class ModelState:
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
}
if self.custom_model and self.model_capabilities != ModelCapabilities.Default:
if self.custom_model_metadata.valid:
_model_name = self.param_s.get("DrivingModelText", encoding="utf8")
_model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"}
_metadata_name = self.param_s.get("DrivingModelMetadataText", encoding="utf8")
@ -135,9 +133,9 @@ class ModelState:
self.prev_desire[:] = inputs['desire']
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
if not (self.custom_model and self.model_capabilities & ModelCapabilities.LateralPlannerSolution):
if not (self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
if self.custom_model and self.model_capabilities & ModelCapabilities.NoO:
if self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.NoO:
self.inputs['nav_features'][:] = inputs['nav_features']
self.inputs['nav_instructions'][:] = inputs['nav_instructions']
@ -154,11 +152,11 @@ class ModelState:
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 self.custom_model and self.model_capabilities != ModelCapabilities.Default:
if self.model_capabilities & ModelCapabilities.LateralPlannerSolution:
if self.custom_model_metadata.valid:
if self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution:
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])
elif self.model_capabilities & ModelCapabilities.DesiredCurvatureV1:
elif self.custom_model_metadata.capabilities & ModelCapabilities.DesiredCurvatureV1:
self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:]
self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0]
else: # Default model, and as of time of writing, this model uses DesiredCurvatureV2
@ -205,12 +203,11 @@ def main(demo=False):
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
params = Params()
custom_model, model_gen = get_model_generation(params)
model_capabilities = ModelCapabilities.get_by_gen(model_gen)
custom_model_metadata = CustomModelMetadata(params=params, init_only=True)
# messaging
extended_svs = ["lateralPlanDEPRECATED", "lateralPlanSPDEPRECATED"]
if custom_model and model_capabilities & ModelCapabilities.NoO:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO:
extended_svs += ["navModelDEPRECATED", "navInstruction"]
pm = PubMaster(["modelV2", "modelV2SP", "drivingModelData", "cameraOdometry"])
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"] + extended_svs)
@ -226,9 +223,9 @@ def main(demo=False):
model_transform_main = np.zeros((3, 3), dtype=np.float32)
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
live_calib_seen = False
if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution:
driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
if custom_model and model_capabilities & ModelCapabilities.NoO:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO:
nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
buf_main, buf_extra = None, None
@ -282,10 +279,13 @@ def main(demo=False):
meta_extra = meta_main
sm.update(0)
desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution else DH.desire
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution:
desire = sm["lateralPlanDEPRECATED"].desire.raw
else:
desire = DH.desire
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution):
if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
@ -303,7 +303,7 @@ def main(demo=False):
timestamp_llk = 0
nav_enabled = False
if custom_model and model_capabilities & ModelCapabilities.NoO:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO:
# Enable/disable nav features
timestamp_llk = sm["navModelDEPRECATED"].locationMonoTime
nav_valid = sm.valid["navModelDEPRECATED"] # and (nanos_since_boot() - timestamp_llk < 1e9)
@ -341,7 +341,7 @@ def main(demo=False):
if prepare_only:
cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution:
_inputs = {
'driving_style': driving_style
}
@ -349,7 +349,7 @@ def main(demo=False):
_inputs = {
'lateral_control_params': lateral_control_params
}
if custom_model and model_capabilities & ModelCapabilities.NoO:
if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO:
_inputs_2 = {
'nav_features': nav_features,
'nav_instructions': nav_instructions
@ -377,9 +377,9 @@ def main(demo=False):
posenet_send = messaging.new_message('cameraOdometry')
fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen,
custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution, custom_model and model_capabilities & ModelCapabilities.NoO)
custom_model_metadata.valid, custom_model_metadata.capabilities)
if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution):
if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
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]
@ -398,9 +398,13 @@ def main(demo=False):
modelv2_sp_send = messaging.new_message('modelV2SP')
modelv2_sp_send.valid = True
if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution):
modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change
modelv2_sp_send.modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED
modelV2SP = modelv2_sp_send.modelV2SP
if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
modelV2SP.laneChangePrev = DH.prev_lane_change
modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED
modelV2SP.customModel = custom_model_metadata.valid
modelV2SP.modelGeneration = custom_model_metadata.generation
modelV2SP.modelCapabilities = int(custom_model_metadata.capabilities)
pm.send('modelV2SP', modelv2_sp_send)
last_vipc_frame_id = meta_main.frame_id

6
selfdrive/modeld/navmodeld.py
View File

@ -13,8 +13,8 @@ from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.common.realtime import set_realtime_priority
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.system.hardware.hw import Paths
NAV_INPUT_SIZE = 256*256
@ -50,8 +50,8 @@ class ModelState:
assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32)
self.param_s = Params()
self.custom_model, self.model_gen = get_model_generation(self.param_s)
if self.custom_model and self.model_gen not in (0, 4):
self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True)
if self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.NoO:
_model_name = self.param_s.get("NavModelText", encoding="utf8")
_model_paths = {
ModelRunner.SNPE: f"{CUSTOM_MODEL_PATH}/navmodel_q_{_model_name}.dlc"}

10
selfdrive/sunnypilot/__init__.py
View File

@ -1,10 +0,0 @@
import os
SIMULATION = "SIMULATION" in os.environ
def get_model_generation(params):
custom_model = params.get_bool("CustomDrivingModel") and not SIMULATION
gen = int(params.get("DrivingModelGeneration", encoding="utf8"))
return custom_model, gen

2
selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.cc
View File

@ -150,5 +150,5 @@ std::vector<Model> ModelsFetcher::getModelsFromURL(const QString&url) {
}
std::vector<Model> ModelsFetcher::getModelsFromURL() {
return getModelsFromURL("https://docs.sunnypilot.ai/models_v4.json");
return getModelsFromURL("https://docs.sunnypilot.ai/models_v5.json");
}

8
selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc
View File

@ -437,10 +437,10 @@ void SunnypilotPanel::updateToggles() {
toggles["VisionCurveLaneless"]->setEnabled(dynamic_lane_profile_param == "2");
toggles["VisionCurveLaneless"]->refresh();
bool custom_driving_model = params.getBool("CustomDrivingModel");
auto driving_model_gen = QString::fromStdString(params.get("DrivingModelGeneration"));
bool model_use_lateral_planner = custom_driving_model && driving_model_gen == "1";
auto driving_model_name = custom_driving_model && driving_model_gen != "0" ? QString::fromStdString(params.get("DrivingModelName")) : CURRENT_MODEL;
bool custom_driving_model_valid = params.getBool("CustomDrivingModel");
auto driving_model_generation = QString::fromStdString(params.get("DrivingModelGeneration"));
bool model_use_lateral_planner = custom_driving_model_valid && driving_model_generation == "1";
auto driving_model_name = custom_driving_model_valid && driving_model_generation != "0" ? QString::fromStdString(params.get("DrivingModelName")) : CURRENT_MODEL;
QString driving_model_text = QString("<font color='yellow'>" + driving_model_name + "</font>");
dlp_settings->setEnabled(model_use_lateral_planner);
toggles["VisionCurveLaneless"]->setVisible(model_use_lateral_planner);

4
selfdrive/ui/qt/onroad/annotated_camera.cc
View File

@ -375,7 +375,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
featureStatusToggle = s.scene.feature_status_toggle;
experimental_btn->setVisible(!(showDebugUI && showVTC));
drivingModelGen = s.scene.driving_model_gen;
drivingModelGen = s.scene.driving_model_generation;
}
void AnnotatedCameraWidget::drawHud(QPainter &p) {
@ -1209,7 +1209,7 @@ void AnnotatedCameraWidget::drawFeatureStatusText(QPainter &p, int x, int y) {
}
// Dynamic Lane Profile
if (drivingModelGen == 1) {
if (drivingModelGen == cereal::ModelGeneration::ONE) {
drawFeatureStatusElement(dynamicLaneProfile, feature_text.dlp_list_text, feature_color.dlp_list_color, true, "OFF", "DLP");
}

2
selfdrive/ui/qt/onroad/annotated_camera.h
View File

@ -203,7 +203,7 @@ private:
bool featureStatusToggle;
int drivingModelGen;
cereal::ModelGeneration drivingModelGen;
protected:
void paintGL() override;

2
selfdrive/ui/qt/onroad/buttons.cc
View File

@ -99,7 +99,7 @@ void OnroadSettingsButton::paintEvent(QPaintEvent *event) {
void OnroadSettingsButton::updateState(const UIState &s) {
const auto cp = (*s.sm)["carParams"].getCarParams();
auto dlp_enabled = s.scene.driving_model_gen == 1;
auto dlp_enabled = s.scene.driving_model_generation == cereal::ModelGeneration::ONE;
bool allow_btn = s.scene.onroad_settings_toggle && (dlp_enabled || hasLongitudinalControl(cp) || !cp.getPcmCruiseSpeed());
setVisible(allow_btn);

4
selfdrive/ui/qt/onroad_settings.cc
View File

@ -119,7 +119,7 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent)
void OnroadSettings::changeDynamicLaneProfile() {
UIScene &scene = uiState()->scene;
bool can_change = scene.driving_model_gen == 1;
bool can_change = scene.driving_model_generation == cereal::ModelGeneration::ONE;
if (can_change) {
scene.dynamic_lane_profile++;
scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile;
@ -229,7 +229,7 @@ void OnroadSettings::refresh() {
// Dynamic Lane Profile
dlp_widget->updateDynamicLaneProfile("DynamicLaneProfile");
dlp_widget->setVisible(scene.driving_model_gen == 1);
dlp_widget->setVisible(scene.driving_model_generation == cereal::ModelGeneration::ONE);
// Gap Adjust Cruise
gac_widget->updateGapAdjustCruise("LongitudinalPersonality");

11
selfdrive/ui/ui.cc
View File

@ -230,6 +230,12 @@ static void update_state(UIState *s) {
scene.e2eX[i] = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getE2eX()[i];
}
}
if (sm.updated("modelV2SP")) {
auto model_v2_sp = sm["modelV2SP"].getModelV2SP();
scene.custom_driving_model_valid = model_v2_sp.getCustomModel();
scene.driving_model_generation = model_v2_sp.getModelGeneration();
scene.driving_model_capabilities = model_v2_sp.getModelCapabilities();
}
}
void ui_update_params(UIState *s) {
@ -260,8 +266,6 @@ void ui_update_params(UIState *s) {
s->scene.speed_limit_warning_flash = params.getBool("SpeedLimitWarningFlash");
s->scene.speed_limit_warning_type = std::atoi(params.get("SpeedLimitWarningType").c_str());
s->scene.speed_limit_warning_value_offset = std::atoi(params.get("SpeedLimitWarningValueOffset").c_str());
s->scene.custom_driving_model = params.getBool("CustomDrivingModel");
s->scene.driving_model_gen = std::atoi(params.get("DrivingModelGeneration").c_str());
s->scene.speed_limit_control_enabled = params.getBool("EnableSlc");
s->scene.feature_status_toggle = params.getBool("FeatureStatus");
s->scene.onroad_settings_toggle = params.getBool("OnroadSettings");
@ -388,7 +392,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
"wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks", "longitudinalPlanSP", "liveMapDataSP",
"carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", "controlsStateSP"
"carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters",
"controlsStateSP", "modelV2SP"
});
Params params;

5
selfdrive/ui/ui.h
View File

@ -201,8 +201,9 @@ typedef struct UIScene {
int speed_limit_warning_type;
int speed_limit_warning_value_offset;
bool custom_driving_model;
int driving_model_gen;
bool custom_driving_model_valid;
cereal::ModelGeneration driving_model_generation;
uint32_t driving_model_capabilities;
bool feature_status_toggle;
bool onroad_settings_toggle;

6
system/manager/process_config.py
View File

@ -3,7 +3,7 @@ import os
from cereal import car
from openpilot.common.params import Params
from openpilot.system.hardware import PC, TICI
from openpilot.selfdrive.sunnypilot import get_model_generation
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
from openpilot.system.mapd_manager import MAPD_PATH, COMMON_DIR
from openpilot.system.manager.sunnylink import sunnylink_need_register, sunnylink_ready
@ -45,8 +45,8 @@ def only_offroad(started, params, CP: car.CarParams) -> bool:
return not started
def model_use_nav(started, params, CP: car.CarParams) -> bool:
custom_model, model_gen = get_model_generation(params)
return started and custom_model and model_gen not in (0, 4)
custom_model_metadata = CustomModelMetadata(params=params, init_only=True)
return started and custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO
def sunnylink_ready_shim(started, params, CP: car.CarParams) -> bool:
"""Shim for sunnylink_ready to match the process manager signature."""