diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3b36532ac9..8825cf6f07 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -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 { diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 685d9655e4..360f7046ad 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -1,18 +1,12 @@ -from enum import IntFlag, IntEnum +from enum import IntFlag import os +from cereal import custom from openpilot.common.params import Params SIMULATION = "SIMULATION" in os.environ - -class ModelGeneration(IntEnum): - DEFAULT = 0 - ONE = 1 - TWO = 2 - THREE = 3 - FOUR = 4 - FIVE = 5 +ModelGeneration = custom.ModelGeneration class ModelCapabilities(IntFlag): @@ -53,19 +47,19 @@ class CustomModelMetadata: self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: - return int(self.params.get('DrivingModelGeneration') or ModelGeneration.DEFAULT) + 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: + if self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.FOUR: + elif self.generation == ModelGeneration.four: return ModelCapabilities.DesiredCurvatureV2 - elif self.generation == ModelGeneration.THREE: + elif self.generation == ModelGeneration.three: return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.TWO: + elif self.generation == ModelGeneration.two: return ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO - elif self.generation == ModelGeneration.ONE: + elif self.generation == ModelGeneration.one: return ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO else: # Default model is meant to represent the capabilities of the prebuilt model diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 95099d27c3..39a161ed89 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -55,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, - custom_model_valid: bool, custom_model_capabilities: int) -> 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 @@ -68,9 +68,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc action = driving_model_data.action - model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution - if not model_use_lateral_planner: - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -102,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)] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index b6880d4d8d..ff3d9ec633 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -402,6 +402,9 @@ def main(demo=False): 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 diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 7e27449dc5..009ff76138 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -375,7 +375,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { featureStatusToggle = s.scene.feature_status_toggle; experimental_btn->setVisible(!(showDebugUI && showVTC)); - customModelValid = s.scene.custom_driving_model_valid; drivingModelGen = s.scene.driving_model_generation; } @@ -1210,7 +1209,7 @@ void AnnotatedCameraWidget::drawFeatureStatusText(QPainter &p, int x, int y) { } // Dynamic Lane Profile - if (customModelValid && drivingModelGen == 1) { // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + if (drivingModelGen == cereal::ModelGeneration::ONE) { drawFeatureStatusElement(dynamicLaneProfile, feature_text.dlp_list_text, feature_color.dlp_list_color, true, "OFF", "DLP"); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 5c3697aaf3..29b9c4833b 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -203,8 +203,7 @@ private: bool featureStatusToggle; - bool customModelValid; - int drivingModelGen; + cereal::ModelGeneration drivingModelGen; protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 57de6c12ee..fbca42da7b 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -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.custom_driving_model_valid && s.scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + 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); diff --git a/selfdrive/ui/qt/onroad_settings.cc b/selfdrive/ui/qt/onroad_settings.cc index d39d908662..00810294ba 100644 --- a/selfdrive/ui/qt/onroad_settings.cc +++ b/selfdrive/ui/qt/onroad_settings.cc @@ -119,7 +119,7 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent) void OnroadSettings::changeDynamicLaneProfile() { UIScene &scene = uiState()->scene; - bool can_change = scene.custom_driving_model_valid && scene.driving_model_generation == 1; // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + 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.custom_driving_model_valid && scene.driving_model_generation == 1); // ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + dlp_widget->setVisible(scene.driving_model_generation == cereal::ModelGeneration::ONE); // Gap Adjust Cruise gac_widget->updateGapAdjustCruise("LongitudinalPersonality"); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b0ef6d4431..126be0d6b1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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) { @@ -263,8 +269,6 @@ void ui_update_params(UIState *s) { 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"); - s->scene.custom_driving_model_valid = params.getBool("CustomDrivingModel"); - s->scene.driving_model_generation = std::atoi(params.get("DrivingModelGeneration").c_str()); // Handle Onroad Screen Off params if (s->scene.onroadScreenOff > 0) { @@ -389,7 +393,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks", "longitudinalPlanSP", "liveMapDataSP", "carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", - "controlsStateSP" + "controlsStateSP", "modelV2SP" }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 49a38a3e14..a9fb6f0671 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -202,7 +202,8 @@ typedef struct UIScene { int speed_limit_warning_value_offset; bool custom_driving_model_valid; - int driving_model_generation; + cereal::ModelGeneration driving_model_generation; + uint32_t driving_model_capabilities; bool feature_status_toggle; bool onroad_settings_toggle;