diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 157585eca3..0226fff405 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -496,11 +496,11 @@ struct CustomReserved15 @0xbd443b539493bc68 { struct CustomReserved16 @0xfc6241ed8877b611 { } -struct CustomReserved17 @0xa30662f84033036c { +struct MapdExtendedOut @0xa30662f84033036c { } -struct CustomReserved18 @0xc86a3d38d13eb3ef { +struct MapdIn @0xc86a3d38d13eb3ef { } -struct CustomReserved19 @0xa4f1eb3323f5f582 { +struct MapdOut @0xa4f1eb3323f5f582 { } diff --git a/cereal/log.capnp b/cereal/log.capnp index e07ee31f8f..ebeeb4d40b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2639,9 +2639,9 @@ struct Event { customReserved14 @140 :Custom.CustomReserved14; customReserved15 @141 :Custom.CustomReserved15; customReserved16 @142 :Custom.CustomReserved16; - customReserved17 @143 :Custom.CustomReserved17; - customReserved18 @144 :Custom.CustomReserved18; - customReserved19 @145 :Custom.CustomReserved19; + mapdExtendedOut @143 :Custom.MapdExtendedOut; + mapdIn @144 :Custom.MapdIn; + mapdOut @145 :Custom.MapdOut; # *********** legacy + deprecated *********** model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated diff --git a/cereal/services.py b/cereal/services.py index b6871f0e0f..cd7a1eedf2 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -91,6 +91,7 @@ _services: dict[str, tuple] = { "modelDataV2SP": (True, 20.), "navigationd": (True, 3.), "liveLocationKalman": (True, 20.), + "mapdOut": (True, 20., 0.1), # debug "uiDebug": (True, 0., 1), diff --git a/common/params_keys.h b/common/params_keys.h index 640b79c983..780cf11c8f 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -239,6 +239,9 @@ inline static std::unordered_map keys = { {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, + + // mapd v020 + {"MapdSettings", {PERSISTENT | BACKUP, JSON}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, {"MapdVersion", {PERSISTENT, STRING}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 566204cd4a..3bc74d1886 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -155,6 +155,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise) + if sm.valid['mapdOut']: + if sm['mapdOut'].suggestedSpeed > 0 and v_cruise > sm['mapdOut'].suggestedSpeed: + v_cruise = sm['mapdOut'].suggestedSpeed + if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index f7d3370f90..5dbe8fcbb1 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -27,12 +27,12 @@ def main(): longitudinal_planner = LongitudinalPlanner(CP, CP_SP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', - 'liveMapDataSP', 'carStateSP', gps_location_service], + 'liveMapDataSP', 'carStateSP', 'mapdOut', gps_location_service], poll='carState') while True: sm.update() - longitudinal_planner.sla.update_car_state(sm['carState']) + #longitudinal_planner.sla.update_car_state(sm['carState']) if sm.updated['modelV2']: longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) diff --git a/selfdrive/mapd b/selfdrive/mapd new file mode 100755 index 0000000000..2c1ae3146d Binary files /dev/null and b/selfdrive/mapd differ diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 08f7274186..cf4778b799 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -25,28 +25,27 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc): self.events_sp = EventsSP() - self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.accel_controller = AccelPersonalityController() - self.scc = SmartCruiseControl() - self.resolver = SpeedLimitResolver() - self.sla = SpeedLimitAssist(CP, CP_SP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() + # Disabled controllers + self.scc = None + self.sla = None + self.resolver = None + self.output_v_target = 0. self.output_a_target = 0. @property def mlsim(self) -> bool: - # If we don't have a generation set, we assume it's default model. Which as of today are mlsim. return bool(self.generation is None or self.generation >= 11) def get_mpc_mode(self) -> str | None: if not self.dec.active(): return None - return self.dec.mode() def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: @@ -54,29 +53,10 @@ class LongitudinalPlannerSP: v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS - long_enabled = sm['carControl'].enabled - long_override = sm['carControl'].cruiseControl.override - - # Smart Cruise Control - self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) - - # Speed Limit Resolver - self.resolver.update(v_ego, sm) - - # Speed Limit Assist - has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid - self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit, - self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp) - - targets = { - LongitudinalPlanSource.cruise: (v_cruise, a_ego), - LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), - LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target), - LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target), - } - - self.source = min(targets, key=lambda k: targets[k][0]) - self.output_v_target, self.output_a_target = targets[self.source] + # Skip SCC and Speed Limit logic + self.source = LongitudinalPlanSource.cruise + self.output_v_target = v_cruise_cluster + self.output_a_target = a_ego return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: @@ -87,7 +67,6 @@ class LongitudinalPlannerSP: def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') - plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlanSP = plan_sp_send.longitudinalPlanSP @@ -102,43 +81,10 @@ class LongitudinalPlannerSP: dec.enabled = self.dec.enabled() dec.active = self.dec.active() - # Smart Cruise Control - smartCruiseControl = longitudinalPlanSP.smartCruiseControl - # Vision Control - sccVision = smartCruiseControl.vision - sccVision.state = self.scc.vision.state - sccVision.vTarget = float(self.scc.vision.output_v_target) - sccVision.aTarget = float(self.scc.vision.output_a_target) - sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc) - sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc) - sccVision.enabled = self.scc.vision.is_enabled - sccVision.active = self.scc.vision.is_active - # Map Control - sccMap = smartCruiseControl.map - sccMap.state = self.scc.map.state - sccMap.vTarget = float(self.scc.map.output_v_target) - sccMap.aTarget = float(self.scc.map.output_a_target) - sccMap.enabled = self.scc.map.is_enabled - sccMap.active = self.scc.map.is_active - - # Speed Limit - speedLimit = longitudinalPlanSP.speedLimit - resolver = speedLimit.resolver - resolver.speedLimit = float(self.resolver.speed_limit) - resolver.speedLimitLast = float(self.resolver.speed_limit_last) - resolver.speedLimitFinal = float(self.resolver.speed_limit_final) - resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last) - resolver.speedLimitValid = self.resolver.speed_limit_valid - resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid - resolver.speedLimitOffset = float(self.resolver.speed_limit_offset) - resolver.distToSpeedLimit = float(self.resolver.distance) - resolver.source = self.resolver.source - assist = speedLimit.assist - assist.state = self.sla.state - assist.enabled = self.sla.is_enabled - assist.active = self.sla.is_active - assist.vTarget = float(self.sla.output_v_target) - assist.aTarget = float(self.sla.output_a_target) + # Skip SCC and Speed Limit fields (leave zeroed) + longitudinalPlanSP.smartCruiseControl.vision.enabled = False + longitudinalPlanSP.smartCruiseControl.map.enabled = False + longitudinalPlanSP.speedLimit.assist.enabled = False # E2E Alerts e2eAlerts = longitudinalPlanSP.e2eAlerts diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 2ecb305a3d..40dfa04d48 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -178,8 +178,9 @@ procs += [ PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)), # mapd - NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), - PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), + #NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), + #PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), + NativeProcess("mapd", "selfdrive", ["./mapd"], always_run), # navigationd PythonProcess("navigationd", "sunnypilot.navd.navigationd", only_onroad),