diff --git a/cereal b/cereal index bf00063015..318da0bb3e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit bf00063015ef0cfc517b762d6f901f178256f520 +Subproject commit 318da0bb3efd4071b4d6402b4b0afe0a6d77daa0 diff --git a/panda b/panda index 92aaeb38b0..7f35e86bf7 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 92aaeb38b08d24912c1433721afcfa1d52a049db +Subproject commit 7f35e86bf7e7c25199a68d602a6303d3af35c949 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2197fd0d21..70724606cd 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -361,7 +361,6 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setIgnitionLine(health.ignition_line_pkt); ps.setIgnitionCan(health.ignition_can_pkt); ps.setControlsAllowed(health.controls_allowed_pkt); - ps.setControlsAllowedLong(health.controls_allowed_long_pkt); ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 905529d154..227ab3301b 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -25,7 +25,7 @@ class CarController: self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) - self.sm = messaging.SubMaster(['longitudinalPlan']) + self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -60,14 +60,14 @@ class CarController: if not self.CP.pcmCruiseSpeed: self.sm.update(0) - if self.sm.updated['longitudinalPlan']: - self.v_tsc_state = self.sm['longitudinalPlan'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlan'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlan'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlan'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlan'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlan'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlan'].turnSpeed + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index e6a3d9dc9a..959688bd5e 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -128,7 +128,7 @@ class CarController: self.last_steer = 0.0 self.last_button_frame = 0 - self.sm = messaging.SubMaster(['longitudinalPlan']) + self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -162,14 +162,14 @@ class CarController: if not self.CP.pcmCruiseSpeed: self.sm.update(0) - if self.sm.updated['longitudinalPlan']: - self.v_tsc_state = self.sm['longitudinalPlan'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlan'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlan'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlan'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlan'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlan'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlan'].turnSpeed + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 07929e3602..e2d1532189 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -63,7 +63,7 @@ class CarController: self.lat_disengage_init = False self.lat_active_last = False - self.sm = messaging.SubMaster(['longitudinalPlan']) + self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -98,14 +98,14 @@ class CarController: if not self.CP.pcmCruiseSpeed: self.sm.update(0) - if self.sm.updated['longitudinalPlan']: - self.v_tsc_state = self.sm['longitudinalPlan'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlan'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlan'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlan'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlan'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlan'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlan'].turnSpeed + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 4aeb12018a..f95baeb336 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -121,6 +121,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6) ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec ret.tireStiffnessFactor = 0.385 + if candidate == CAR.KONA_EV_2022: + ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value + ret.radarUnavailable = False elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV): ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 05d368b817..9c0824b185 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -13,6 +13,8 @@ def get_radar_can_parser(CP): if DBC[CP.carFingerprint]['radar'] is None: if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC: lead_src, bus = "ESCC", 0 + elif CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD: + lead_src, bus = "SCC11", 2 else: return None messages = [(lead_src, 50)] @@ -26,8 +28,11 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) self.enhanced_scc = (CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None + self.camera_scc = CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD self.updated_messages = set() - self.trigger_msg = 0x2AB if self.enhanced_scc else RADAR_START_ADDR + RADAR_MSG_COUNT - 1 + self.trigger_msg = 0x2AB if self.enhanced_scc else \ + 0x420 if self.camera_scc else \ + (RADAR_START_ADDR + RADAR_MSG_COUNT - 1) self.track_id = 0 self.radar_off_can = CP.radarUnavailable @@ -59,8 +64,9 @@ class RadarInterface(RadarInterfaceBase): errors.append("canError") ret.errors = errors - if self.enhanced_scc: - msg = self.rcp.vl["ESCC"] + if self.enhanced_scc or self.camera_scc: + msg_src = "ESCC" if self.enhanced_scc else "SCC11" + msg = self.rcp.vl[msg_src] valid = msg['ACC_ObjStatus'] for ii in range(1): if valid: @@ -70,7 +76,7 @@ class RadarInterface(RadarInterfaceBase): self.track_id += 1 self.pts[ii].measured = True self.pts[ii].dRel = msg['ACC_ObjDist'] - self.pts[ii].yRel = -msg['ACC_ObjLatPos'] + self.pts[ii].yRel = -msg['ACC_ObjLatPos'] if self.enhanced_scc else float('nan') self.pts[ii].vRel = msg['ACC_ObjRelSpd'] self.pts[ii].aRel = float('nan') self.pts[ii].yvRel = float('nan') diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 9c9cc4e0b5..fa0510ac5a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -71,6 +71,7 @@ class HyundaiFlagsSP(IntFlag): SP_ENHANCED_SCC = 1 SP_CAN_LFA_BTN = 2 SP_NAV_MSG = 4 + SP_CAMERA_SCC_LEAD = 8 class CAR: diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index e2b476737f..2efb00e014 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -22,7 +22,7 @@ class CarController: self.brake_counter = 0 self.frame = 0 - self.sm = messaging.SubMaster(['longitudinalPlan']) + self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -56,14 +56,14 @@ class CarController: if not self.CP.pcmCruiseSpeed: self.sm.update(0) - if self.sm.updated['longitudinalPlan']: - self.v_tsc_state = self.sm['longitudinalPlan'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlan'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlan'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlan'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlan'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlan'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlan'].turnSpeed + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 7d802c1fab..a1bdb4dfe8 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -30,7 +30,7 @@ class CarController: self.hca_frame_same_torque = 0 self.last_button_frame = 0 - self.sm = messaging.SubMaster(['longitudinalPlan']) + self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() self.is_metric = self.param_s.get_bool("IsMetric") self.speed_limit_control_enabled = False @@ -68,14 +68,14 @@ class CarController: if not self.CP.pcmCruiseSpeed: self.sm.update(0) - if self.sm.updated['longitudinalPlan']: - self.v_tsc_state = self.sm['longitudinalPlan'].visionTurnControllerState - self.slc_state = self.sm['longitudinalPlan'].speedLimitControlState - self.m_tsc_state = self.sm['longitudinalPlan'].turnSpeedControlState - self.speed_limit = self.sm['longitudinalPlan'].speedLimit - self.speed_limit_offset = self.sm['longitudinalPlan'].speedLimitOffset - self.v_tsc = self.sm['longitudinalPlan'].visionTurnSpeed - self.m_tsc = self.sm['longitudinalPlan'].turnSpeed + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed if self.frame % 200 == 0: self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl") diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8976ae468f..248400dfc4 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -68,7 +68,7 @@ class Controls: self.pm = pm if self.pm is None: self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', - 'carControl', 'carEvents', 'carParams']) + 'carControl', 'carEvents', 'carParams', 'controlsStateSP']) self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] @@ -95,7 +95,8 @@ class Controls: ignore += ['driverMonitoringState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets, + 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick', + 'longitudinalPlanSP', 'lateralPlanSP'] + self.camera_packets, ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick']) if CI is None: @@ -181,7 +182,6 @@ class Controls: self.active = False self.soft_disable_timer = 0 self.mismatch_counter = 0 - self.mismatch_counter_long = 0 self.cruise_mismatch_counter = 0 self.can_rcv_timeout_counter = 0 # conseuctive timeout count self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count @@ -285,7 +285,7 @@ class Controls: if not self.CP.notCar: if not self.d_camera_hardware_missing: self.events.add_from_msg(self.sm['driverMonitoringState'].events) - self.events.add_from_msg(self.sm['longitudinalPlan'].eventsDEPRECATED) + self.events.add_from_msg(self.sm['longitudinalPlanSP'].events) # Add car events, ignore if CAN isn't valid if CS.canValid: @@ -328,11 +328,11 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - if self.sm['lateralPlan'].laneChangeEdgeBlock: + if self.sm['lateralPlanSP'].laneChangeEdgeBlock: self.events.add(EventName.laneChangeRoadEdge) elif self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['lateralPlan'].laneChangeDirection - lc_prev = self.sm['lateralPlan'].laneChangePrev + lc_prev = self.sm['lateralPlanSP'].laneChangePrev if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) @@ -358,8 +358,6 @@ class Controls: if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) - if self.mismatch_counter_long >= 200: - self.events.add(EventName.controlsMismatchLong) if log.PandaState.FaultType.relayMalfunction in pandaState.faults: self.events.add(EventName.relayMalfunction) @@ -511,16 +509,11 @@ class Controls: # Therefore we allow a mismatch for two samples, then we trigger the disengagement. if not self.enabled: self.mismatch_counter = 0 - if not self.enabled_long: - self.mismatch_counter_long = 0 # All pandas not in silent mode must have controlsAllowed when openpilot is enabled if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 - if self.enabled_long and any(not ps.controlsAllowedLong for ps in self.sm['pandaStates'] - if ps.safetyModel not in IGNORED_SAFETY_MODES): - self.mismatch_counter_long += 1 self.distance_traveled += CS.vEgo * DT_CTRL @@ -874,10 +867,16 @@ class Controls: controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': controlsState.lateralControlState.torqueState = lac_log - controlsState.lateralState = lat_tuning self.pm.send('controlsState', dat) + dat_sp = messaging.new_message('controlsStateSP') + controlsStateSP = dat_sp.controlsStateSP + + controlsStateSP.lateralState = lat_tuning + + self.pm.send('controlsStateSP', dat_sp) + # carState car_events = self.events.to_msg() cs_send = messaging.new_message('carState') diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index ee1411972d..16b929b367 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -332,7 +332,7 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, return NormalPermanentAlert("Joystick Mode", vals) def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - speedLimit = sm['longitudinalPlan'].speedLimit + speedLimit = sm['longitudinalPlanSP'].speedLimit speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' return Alert( diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index e4aa6e328d..00d528716a 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -114,7 +114,7 @@ class LateralPlanner: low_speed = v_ego_car < 10 * CV.MPH_TO_MS - if not self.get_dynamic_lane_profile(sm['longitudinalPlan']) and not low_speed: + if not self.get_dynamic_lane_profile(sm['longitudinalPlanSP']) and not low_speed: self.path_xyz = self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz) self.dynamic_lane_profile_status = False else: @@ -161,7 +161,7 @@ class LateralPlanner: else: self.solution_invalid_cnt = 0 - def get_dynamic_lane_profile(self, longitudinal_plan): + def get_dynamic_lane_profile(self, longitudinal_plan_sp): if not self.dynamic_lane_profile_enabled: return True elif self.dynamic_lane_profile == 1: @@ -176,11 +176,11 @@ class LateralPlanner: elif self.DH.lane_change_state == LaneChangeState.off: # laneline probability too low, we switch to laneless mode if (self.LP.lll_prob + self.LP.rll_prob) / 2 < 0.3 \ - or ((longitudinal_plan.visionCurrentLatAcc > 1.0 or longitudinal_plan.visionMaxPredLatAcc > 1.4) + or ((longitudinal_plan_sp.visionCurrentLatAcc > 1.0 or longitudinal_plan_sp.visionMaxPredLatAcc > 1.4) and self.vision_curve_laneless): self.dynamic_lane_profile_status_buffer = True if (self.LP.lll_prob + self.LP.rll_prob) / 2 > 0.5 \ - and ((longitudinal_plan.visionCurrentLatAcc < 0.6 and longitudinal_plan.visionMaxPredLatAcc < 0.7) + and ((longitudinal_plan_sp.visionCurrentLatAcc < 0.6 and longitudinal_plan_sp.visionMaxPredLatAcc < 0.7) or not self.vision_curve_laneless): self.dynamic_lane_profile_status_buffer = False if self.dynamic_lane_profile_status_buffer: # in buffer mode, always laneless @@ -194,15 +194,11 @@ class LateralPlanner: lateralPlan = plan_send.lateralPlan lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] - lateralPlan.laneWidth = float(self.LP.lane_width) lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist() lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] - lateralPlan.lProb = float(self.LP.lll_prob) - lateralPlan.rProb = float(self.LP.rll_prob) - lateralPlan.dProb = float(self.LP.d_prob) lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time @@ -216,19 +212,32 @@ class LateralPlanner: lateralPlan.useLaneLines = self.use_lanelines lateralPlan.laneChangeState = self.DH.lane_change_state lateralPlan.laneChangeDirection = self.DH.lane_change_direction - lateralPlan.laneChangePrev = self.DH.prev_lane_change - lateralPlan.laneChangeEdgeBlock = bool((self.DH.lane_change_state == LaneChangeState.preLaneChange) and self.DH.road_edge) - lateralPlan.dynamicLaneProfile = int(self.dynamic_lane_profile) - lateralPlan.dynamicLaneProfileStatus = bool(self.dynamic_lane_profile_status) + pm.send('lateralPlan', plan_send) - lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]] - lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]] + plan_sp_send = messaging.new_message('lateralPlanSP') + plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) + + lateralPlanSP = plan_sp_send.lateralPlanSP + + lateralPlanSP.laneWidth = float(self.LP.lane_width) + lateralPlanSP.lProb = float(self.LP.lll_prob) + lateralPlanSP.rProb = float(self.LP.rll_prob) + lateralPlanSP.dProb = float(self.LP.d_prob) + + lateralPlanSP.laneChangePrev = self.DH.prev_lane_change + lateralPlanSP.laneChangeEdgeBlock = bool((self.DH.lane_change_state == LaneChangeState.preLaneChange) and self.DH.road_edge) + + lateralPlanSP.dynamicLaneProfile = int(self.dynamic_lane_profile) + lateralPlanSP.dynamicLaneProfileStatus = bool(self.dynamic_lane_profile_status) + + lateralPlanSP.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]] + lateralPlanSP.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]] if self.standstill: self.standstill_elapsed += DT_MDL else: self.standstill_elapsed = 0.0 - lateralPlan.standstillElapsed = int(self.standstill_elapsed) + lateralPlanSP.standstillElapsed = int(self.standstill_elapsed) - pm.send('lateralPlan', plan_send) + pm.send('lateralPlanSP', plan_sp_send) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4465da7dd4..d75289c6ad 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -2,7 +2,7 @@ import os import time import numpy as np -from cereal import log +from cereal import custom from openpilot.common.numpy_fast import clip from openpilot.system.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild @@ -56,27 +56,27 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: +def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): + if personality==custom.LongitudinalPersonalitySP.relaxed: return 1.0 - elif personality==log.LongitudinalPersonality.standard: + elif personality==custom.LongitudinalPersonalitySP.standard: return 1.0 - elif personality==log.LongitudinalPersonality.moderate: + elif personality==custom.LongitudinalPersonalitySP.moderate: return 0.5 - elif personality==log.LongitudinalPersonality.aggressive: + elif personality==custom.LongitudinalPersonalitySP.aggressive: return 0.222 else: raise NotImplementedError("Longitudinal personality not supported") -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: +def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): + if personality==custom.LongitudinalPersonalitySP.relaxed: return 1.75 - elif personality==log.LongitudinalPersonality.standard: + elif personality==custom.LongitudinalPersonalitySP.standard: return 1.45 - elif personality==log.LongitudinalPersonality.moderate: + elif personality==custom.LongitudinalPersonalitySP.moderate: return 1.25 - elif personality==log.LongitudinalPersonality.aggressive: + elif personality==custom.LongitudinalPersonalitySP.aggressive: return 1.0 else: raise NotImplementedError("Longitudinal personality not supported") @@ -277,7 +277,7 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): + def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard): jerk_factor = get_jerk_factor(personality) if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 @@ -336,7 +336,7 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0b25d132c0..ee5766a723 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,7 +3,7 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params -from cereal import car, log +from cereal import car, log, custom import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -30,6 +30,9 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +PERSONALITY_MAPPING = {0: 0, 1: 1, 2: 2, 3: 2} + + EventName = car.CarEvent.EventName @@ -69,7 +72,7 @@ class LongitudinalPlanner: self.params = Params() self.param_read_counter = 0 self.read_param() - self.personality = log.LongitudinalPersonality.standard + self.personality = custom.LongitudinalPersonalitySP.standard self.cruise_source = 'cruise' self.vision_turn_controller = VisionTurnController(CP) @@ -81,7 +84,7 @@ class LongitudinalPlanner: try: self.personality = int(self.params.get('LongitudinalPersonality')) except (ValueError, TypeError): - self.personality = log.LongitudinalPersonality.standard + self.personality = custom.LongitudinalPersonalitySP.standard @staticmethod def parse_model(model_msg, model_error): @@ -185,33 +188,45 @@ class LongitudinalPlanner: longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source if self.mpc.source != 'cruise' else self.cruise_source + longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw longitudinalPlan.solverExecutionTime = self.mpc.solve_time - longitudinalPlan.personality = self.personality - - longitudinalPlan.e2eX = self.mpc.e2e_x.tolist() - - longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state - longitudinalPlan.visionTurnSpeed = float(self.vision_turn_controller.v_turn) - longitudinalPlan.visionCurrentLatAcc = float(self.vision_turn_controller.current_lat_acc) - longitudinalPlan.visionMaxPredLatAcc = float(self.vision_turn_controller.max_pred_lat_acc) - - longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state - longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit) - longitudinalPlan.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset) - longitudinalPlan.distToSpeedLimit = float(self.speed_limit_controller.distance) - longitudinalPlan.isMapSpeedLimit = bool(self.speed_limit_controller.source not in (SpeedLimitResolver.Source.none, SpeedLimitResolver.Source.nav)) - longitudinalPlan.eventsDEPRECATED = self.events.to_msg() - - longitudinalPlan.turnSpeedControlState = self.turn_speed_controller.state - longitudinalPlan.turnSpeed = float(self.turn_speed_controller.speed_limit) - longitudinalPlan.distToTurn = float(self.turn_speed_controller.distance) - longitudinalPlan.turnSign = int(self.turn_speed_controller.turn_sign) + longitudinalPlan.personality = PERSONALITY_MAPPING.get(self.personality, log.LongitudinalPersonality.standard) pm.send('longitudinalPlan', plan_send) + plan_sp_send = messaging.new_message('longitudinalPlanSP') + + plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + + longitudinalPlanSP = plan_sp_send.longitudinalPlanSP + + longitudinalPlanSP.longitudinalPlanSource = self.mpc.source if self.mpc.source != 'cruise' else self.cruise_source + + longitudinalPlanSP.e2eX = self.mpc.e2e_x.tolist() + + longitudinalPlanSP.visionTurnControllerState = self.vision_turn_controller.state + longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_turn) + longitudinalPlanSP.visionCurrentLatAcc = float(self.vision_turn_controller.current_lat_acc) + longitudinalPlanSP.visionMaxPredLatAcc = float(self.vision_turn_controller.max_pred_lat_acc) + + longitudinalPlanSP.speedLimitControlState = self.speed_limit_controller.state + longitudinalPlanSP.speedLimit = float(self.speed_limit_controller.speed_limit) + longitudinalPlanSP.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset) + longitudinalPlanSP.distToSpeedLimit = float(self.speed_limit_controller.distance) + longitudinalPlanSP.isMapSpeedLimit = bool(self.speed_limit_controller.source not in (SpeedLimitResolver.Source.none, SpeedLimitResolver.Source.nav)) + longitudinalPlanSP.events = self.events.to_msg() + + longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state + longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.speed_limit) + longitudinalPlanSP.distToTurn = float(self.turn_speed_controller.distance) + longitudinalPlanSP.turnSign = int(self.turn_speed_controller.turn_sign) + + longitudinalPlanSP.personality = self.personality + + pm.send('longitudinalPlanSP', plan_sp_send) + def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm): # Update controllers self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm) @@ -240,7 +255,7 @@ class LongitudinalPlanner: return source, a_solutions[source], v_solutions[source] def e2e_events(self, sm): - e2e_long_status = sm['e2eLongState'].status + e2e_long_status = sm['e2eLongStateSP'].status if e2e_long_status in (1, 2): self.events.add(EventName.e2eLongStart) diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py index 58b54e8142..14892d8add 100644 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -2,7 +2,7 @@ import numpy as np import time from common.numpy_fast import interp from enum import IntEnum -from cereal import log, car +from cereal import custom, car from common.conversions import Conversions as CV from common.params import Params from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \ @@ -18,7 +18,7 @@ _TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before c _LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h _LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h -SpeedLimitControlState = log.LongitudinalPlan.SpeedLimitControlState +SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState EventName = car.CarEvent.EventName _DEBUG = False @@ -98,7 +98,7 @@ class SpeedLimitResolver(): def _get_from_map_data(self): # Ignore if no live map data - sock = 'liveMapData' + sock = 'liveMapDataSP' if self._sm.logMonoTime[sock] is None: self._limit_solutions[SpeedLimitResolver.Source.map_data] = 0. self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0. diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py index 4c0684bf4a..5612b8720b 100644 --- a/selfdrive/controls/lib/turn_speed_controller.py +++ b/selfdrive/controls/lib/turn_speed_controller.py @@ -1,7 +1,7 @@ import numpy as np import time from common.params import Params -from cereal import log +from cereal import custom from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_SPEED, LIMIT_MAX_MAP_DATA_AGE, \ LIMIT_SPEED_OFFSET_TH, CONTROL_N, LIMIT_MIN_ACC, LIMIT_MAX_ACC from selfdrive.modeld.constants import T_IDXS @@ -13,7 +13,7 @@ _ACTIVE_LIMIT_MAX_ACC = 0.5 # m/s^2 Maximum acelration allowed while active. _DEBUG = False -TurnSpeedControlState = log.LongitudinalPlan.SpeedLimitControlState +TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState def _debug(msg): @@ -98,7 +98,7 @@ class TurnSpeedController(): """Provides the speed limit, distance and turn sign to it for turns based on map data. """ # Ignore if no live map data - sock = 'liveMapData' + sock = 'liveMapDataSP' if sm.logMonoTime[sock] is None: _debug('TS: No map data for turn speed limit') return 0., 0., 0 diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index 0e5d5176a5..e3e4286cc4 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/selfdrive/controls/lib/vision_turn_controller.py @@ -1,7 +1,7 @@ import numpy as np import math import time -from cereal import log +from cereal import custom from common.numpy_fast import interp from common.params import Params from common.conversions import Conversions as CV @@ -51,7 +51,7 @@ def _debug(msg): print(msg) -VisionTurnControllerState = log.LongitudinalPlan.VisionTurnControllerState +VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState def eval_curvature(poly, x_vals): @@ -161,7 +161,7 @@ class VisionTurnController(): # Get path polynomial approximation for curvature estimation from model data. path_poly = None model_data = sm['modelV2'] if sm.valid.get('modelV2', False) else None - lat_planner_data = sm['lateralPlan'] if sm.valid.get('lateralPlan', False) else None + lat_planner_data = sm['lateralPlanSP'] if sm.valid.get('lateralPlanSP', False) else None # 1. When the probability of lanes is good enough, compute polynomial from lanes as they are way more stable # on current mode than drving path. diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index e1ac208692..871bba52ab 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -46,11 +46,13 @@ def plannerd_thread(sm=None, pm=None): lateral_planner = LateralPlanner(CP, debug=debug_mode, use_lanelines=use_lanelines) if sm is None: - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'longitudinalPlan', 'lateralPlan', 'liveMapData', 'navInstruction', 'e2eLongState'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan', + 'longitudinalPlan', 'navInstruction', 'lateralPlanSP', 'longitudinalPlanSP', + 'liveMapDataSP', 'e2eLongStateSP'], poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) if pm is None: - pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan']) + pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan', "longitudinalPlanSP", "lateralPlanSP"]) while True: sm.update() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ec42ae66f2..8be4a14f5d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -9,6 +9,7 @@ from cereal import messaging, log, car from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process +from openpilot.selfdrive.car.hyundai.values import HyundaiFlagsSP from openpilot.system.swaglog import cloudlog from openpilot.common.kalman.simple_kalman import KF1D @@ -91,10 +92,11 @@ class Track: self.aLeadK = aLeadK self.aLeadTau = aLeadTau - def get_RadarState(self, model_prob: float = 0.0): + def get_RadarState(self, CP: car.CarParams = None, lead_msg_y: float = 0.0, model_prob: float = 0.0): + y_rel_vision = False if CP is None else CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD return { "dRel": float(self.dRel), - "yRel": float(self.yRel), + "yRel": float(-lead_msg_y) if y_rel_vision else float(self.yRel), "vRel": float(self.vRel), "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), @@ -167,7 +169,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]: + model_v_ego: float, CP: car.CarParams, low_speed_override: bool = True) -> Dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -176,7 +178,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = track.get_RadarState(CP, lead_msg.y[0], lead_msg.prob) elif (track is None) and ready and (lead_msg.prob > .5): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) @@ -193,7 +195,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn class RadarD: - def __init__(self, radar_ts: float, delay: int = 0): + def __init__(self, radar_ts: float, CP: car.CarParams, delay: int = 0): self.current_time = 0.0 self.tracks: Dict[int, Track] = {} @@ -207,6 +209,8 @@ class RadarD: self.ready = False + self.CP = CP + def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -256,8 +260,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, low_speed_override=False) def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None @@ -305,7 +309,7 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) + RD = RadarD(CP.radarTimeStep, CP, RI.delay) while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 5037208643..6d9ea5808f 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -54,7 +54,8 @@ def cycle_alerts(duration=200, is_metric=False): CP = CarInterface.get_non_essential_params("HONDA CIVIC 2016") sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', - 'managerState'] + cameras) + 'managerState', 'longitudinalPlanSP', 'lateralPlanSP', + 'driverMonitoringStateSP'] + cameras) pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) diff --git a/selfdrive/debug/internal/hands_on_wheel_moniotr.py b/selfdrive/debug/internal/hands_on_wheel_moniotr.py index f58d23bfad..1e6c03a810 100644 --- a/selfdrive/debug/internal/hands_on_wheel_moniotr.py +++ b/selfdrive/debug/internal/hands_on_wheel_moniotr.py @@ -7,11 +7,11 @@ import signal import sys import cereal.messaging as messaging -from cereal import log +from cereal import custom from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus from selfdrive.controls.lib.events import Events -HandsOnWheelState = log.DriverMonitoringState.HandsOnWheelState +HandsOnWheelState = custom.DriverMonitoringStateSP.HandsOnWheelState def sigint_handler(signal, frame): diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 111017e822..2c1e2c79f9 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -7,7 +7,7 @@ import sys import traceback from typing import List, Tuple, Union -from cereal import log +from cereal import custom import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry from openpilot.common.basedir import BASEDIR @@ -47,7 +47,7 @@ def manager_init() -> None: ("HasAcceptedTerms", "0"), ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), - ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), + ("LongitudinalPersonality", str(custom.LongitudinalPersonalitySP.standard)), ("AccMadsCombo", "1"), ("AutoLaneChangeTimer", "0"), diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py index 94b6b1a89a..afa6866601 100644 --- a/selfdrive/mapd/config.py +++ b/selfdrive/mapd/config.py @@ -4,5 +4,5 @@ QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. QUERY_RADIUS_OFFLINE = 2250 # mts. Radius to use on offline OSM data queries. MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query. FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. -LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapData msg. +LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg. LANE_WIDTH = 3.7 # Lane width estimate. Used for detecting departures from way. diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py index 825bba0753..dd931ed95e 100644 --- a/selfdrive/mapd/lib/osm.py +++ b/selfdrive/mapd/lib/osm.py @@ -1,12 +1,12 @@ import overpy import subprocess import numpy as np -from cereal import log +from cereal import custom from common.params import Params from selfdrive.mapd.lib.geo import R from selfdrive.mapd.lib.helpers import is_local_osm_installed, OSM_QUERY -DataType = log.LiveMapData.DataType +DataType = custom.LiveMapDataSP.DataType def create_way(way_id, node_ids, from_way): diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 42bcae50cd..53d404f77b 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -4,7 +4,7 @@ import threading from traceback import print_exception import numpy as np from time import strftime, gmtime -from cereal import log +from cereal import custom import cereal.messaging as messaging from common.params import Params from common.realtime import set_core_affinity, Ratekeeper @@ -14,7 +14,7 @@ from selfdrive.mapd.lib.WayCollection import WayCollection from selfdrive.mapd.config import QUERY_RADIUS, QUERY_RADIUS_OFFLINE, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME from system.swaglog import cloudlog -DataType = log.LiveMapData.DataType +DataType = custom.LiveMapDataSP.DataType _DEBUG = False @@ -177,7 +177,7 @@ class MapD(): # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start # jumping and creating unnecessary losing of the route. Since the route update timestamp has been updated - # a new liveMapData message will be published with the current values (which is desirable) + # a new liveMapDataSP message will be published with the current values (which is desirable) if self.gps_speed < FULL_STOP_MAX_SPEED: _debug('Mapd *****: Route Not updated as car has Stopped ********') return @@ -202,12 +202,12 @@ class MapD(): def publish(self, pm, sm): # Ensure we have a route currently located if self.route is None or not self.route.located: - _debug('Mapd: Skipping liveMapData message as there is no route or is not located.') + _debug('Mapd: Skipping liveMapDataSP message as there is no route or is not located.') return # Ensure we have a route update since last publish if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp: - _debug('Mapd: Skipping liveMapData since there is no new gps fix.') + _debug('Mapd: Skipping liveMapDataSP since there is no new gps fix.') return self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp @@ -219,42 +219,43 @@ class MapD(): next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts) current_road_name = self.route.current_road_name - map_data_msg = messaging.new_message('liveMapData') + map_data_msg = messaging.new_message('liveMapDataSP') map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \ sm.all_valid(service_list=['gpsLocationExternal']) - map_data_msg.liveMapData.lastGpsTimestamp = self.last_gps.unixTimestampMillis - map_data_msg.liveMapData.lastGpsLatitude = float(self.last_gps.latitude) - map_data_msg.liveMapData.lastGpsLongitude = float(self.last_gps.longitude) - map_data_msg.liveMapData.lastGpsSpeed = float(self.last_gps.speed) - map_data_msg.liveMapData.lastGpsBearingDeg = float(self.last_gps.bearingDeg) - map_data_msg.liveMapData.lastGpsAccuracy = float(self.last_gps.accuracy) - map_data_msg.liveMapData.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg) + liveMapDataSP = map_data_msg.liveMapDataSP + liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis + liveMapDataSP.lastGpsLatitude = float(self.last_gps.latitude) + liveMapDataSP.lastGpsLongitude = float(self.last_gps.longitude) + liveMapDataSP.lastGpsSpeed = float(self.last_gps.speed) + liveMapDataSP.lastGpsBearingDeg = float(self.last_gps.bearingDeg) + liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy) + liveMapDataSP.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg) - map_data_msg.liveMapData.speedLimitValid = bool(speed_limit is not None) - map_data_msg.liveMapData.speedLimit = float(speed_limit if speed_limit is not None else 0.0) - map_data_msg.liveMapData.speedLimitAheadValid = bool(next_speed_limit_section is not None) - map_data_msg.liveMapData.speedLimitAhead = float(next_speed_limit_section.value + liveMapDataSP.speedLimitValid = bool(speed_limit is not None) + liveMapDataSP.speedLimit = float(speed_limit if speed_limit is not None else 0.0) + liveMapDataSP.speedLimitAheadValid = bool(next_speed_limit_section is not None) + liveMapDataSP.speedLimitAhead = float(next_speed_limit_section.value if next_speed_limit_section is not None else 0.0) - map_data_msg.liveMapData.speedLimitAheadDistance = float(next_speed_limit_section.start + liveMapDataSP.speedLimitAheadDistance = float(next_speed_limit_section.start if next_speed_limit_section is not None else 0.0) - map_data_msg.liveMapData.turnSpeedLimitValid = bool(turn_speed_limit_section is not None) - map_data_msg.liveMapData.turnSpeedLimit = float(turn_speed_limit_section.value + liveMapDataSP.turnSpeedLimitValid = bool(turn_speed_limit_section is not None) + liveMapDataSP.turnSpeedLimit = float(turn_speed_limit_section.value if turn_speed_limit_section is not None else 0.0) - map_data_msg.liveMapData.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign + liveMapDataSP.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign if turn_speed_limit_section is not None else 0) - map_data_msg.liveMapData.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end + liveMapDataSP.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end if turn_speed_limit_section is not None else 0.0) - map_data_msg.liveMapData.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections] - map_data_msg.liveMapData.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections] - map_data_msg.liveMapData.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections] + liveMapDataSP.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections] + liveMapDataSP.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections] + liveMapDataSP.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections] - map_data_msg.liveMapData.currentRoadName = str(current_road_name if current_road_name is not None else "") + liveMapDataSP.currentRoadName = str(current_road_name if current_road_name is not None else "") - map_data_msg.liveMapData.dataType = self.data_type + map_data_msg.liveMapDataSP.dataType = self.data_type - pm.send('liveMapData', map_data_msg) + pm.send('liveMapDataSP', map_data_msg) _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False) @@ -271,7 +272,7 @@ def mapd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['gpsLocationExternal', 'carControl']) if pm is None: - pm = messaging.PubMaster(['liveMapData']) + pm = messaging.PubMaster(['liveMapDataSP']) while True: sm.update() diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index f1c4bc0079..d528f32fc3 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -16,7 +16,7 @@ def dmonitoringd_thread(sm=None, pm=None): set_realtime_priority(2) if pm is None: - pm = messaging.PubMaster(['driverMonitoringState']) + pm = messaging.PubMaster(['driverMonitoringState', 'driverMonitoringStateSP']) if sm is None: sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2']) @@ -90,10 +90,15 @@ def dmonitoringd_thread(sm=None, pm=None): "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, "isRHD": driver_status.wheel_on_right, - "handsOnWheelState": hands_on_wheel_status.hands_on_wheel_state, } pm.send('driverMonitoringState', dat) + sp_dat = messaging.new_message('driverMonitoringStateSP') + sp_dat.driverMonitoringStateSP = { + "handsOnWheelState": hands_on_wheel_status.hands_on_wheel_state, + } + pm.send('driverMonitoringStateSP', sp_dat) + # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and diff --git a/selfdrive/monitoring/hands_on_wheel_monitor.py b/selfdrive/monitoring/hands_on_wheel_monitor.py index b1fed173cd..713d3a8d89 100644 --- a/selfdrive/monitoring/hands_on_wheel_monitor.py +++ b/selfdrive/monitoring/hands_on_wheel_monitor.py @@ -1,8 +1,8 @@ -from cereal import log, car +from cereal import custom, car from common.conversions import Conversions as CV EventName = car.CarEvent.EventName -HandsOnWheelState = log.DriverMonitoringState.HandsOnWheelState +HandsOnWheelState = custom.DriverMonitoringStateSP.HandsOnWheelState _PRE_ALERT_THRESHOLD = 150 # 15s _PROMPT_ALERT_THRESHOLD = 300 # 30s diff --git a/selfdrive/monitoring/test_hands_monitoring.py b/selfdrive/monitoring/test_hands_monitoring.py index 39b65a2611..05284bc077 100644 --- a/selfdrive/monitoring/test_hands_monitoring.py +++ b/selfdrive/monitoring/test_hands_monitoring.py @@ -2,7 +2,7 @@ import unittest import numpy as np -from cereal import car, log +from cereal import car, custom from common.realtime import DT_DMON from selfdrive.controls.lib.events import Events from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus, _PRE_ALERT_THRESHOLD, \ @@ -10,7 +10,7 @@ from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus, _PRE _MIN_MONITORING_SPEED EventName = car.CarEvent.EventName -HandsOnWheelState = log.DriverMonitoringState.HandsOnWheelState +HandsOnWheelState = custom.DriverMonitoringStateSP.HandsOnWheelState _TEST_TIMESPAN = 120 # seconds diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 2810ad19d9..439385a360 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -470,6 +470,10 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { sidebar_layout->setMargin(0); panel_widget = new QStackedWidget(); + // setup layout for close button + QVBoxLayout *close_btn_layout = new QVBoxLayout; + close_btn_layout->setContentsMargins(0, 0, 0, 20); + // close button QPushButton *close_btn = new QPushButton(tr("×")); close_btn->setStyleSheet(R"( @@ -477,7 +481,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { font-size: 140px; padding-bottom: 20px; border 1px grey solid; - border-radius: 30px; + border-radius: 76px; background-color: #292929; font-weight: 400; } @@ -485,12 +489,16 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { background-color: #3B3B3B; } )"); - close_btn->setFixedSize(300, 110); - sidebar_layout->addSpacing(10); - sidebar_layout->addWidget(close_btn, 0, Qt::AlignRight); - sidebar_layout->addSpacing(10); + close_btn->setFixedSize(152, 152); + close_btn_layout->addWidget(close_btn, 0, Qt::AlignLeft); QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindow::closeSettings); + // setup buttons widget + QWidget *buttons_widget = new QWidget; + QVBoxLayout *buttons_layout = new QVBoxLayout(buttons_widget); + buttons_layout->setMargin(0); + buttons_layout->addSpacing(10); + // setup panels DevicePanel *device = new DevicePanel(this); QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); @@ -500,22 +508,24 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QObject::connect(this, &SettingsWindow::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription); QList> panels = { - {tr("Device"), device}, - {tr("Network"), new Networking(this)}, - {tr("Toggles"), toggles}, - {tr("Software"), new SoftwarePanel(this)}, + {tr(" Device"), device}, + {tr(" Network"), new Networking(this)}, + {tr(" Toggles"), toggles}, + {tr(" Software"), new SoftwarePanel(this)}, }; - panels.push_back({tr("SP - General"), new SPGeneralPanel(this)}); - panels.push_back({tr("SP - Controls"), new SPControlsPanel(this)}); - panels.push_back({tr("SP - Vehicles"), new SPVehiclesPanel(this)}); - panels.push_back({tr("SP - Visuals"), new SPVisualsPanel(this)}); + panels.push_back({tr(" SP - General"), new SPGeneralPanel(this)}); + panels.push_back({tr(" SP - Controls"), new SPControlsPanel(this)}); + panels.push_back({tr(" SP - Vehicles"), new SPVehiclesPanel(this)}); + panels.push_back({tr(" SP - Visuals"), new SPVisualsPanel(this)}); nav_btns = new QButtonGroup(this); for (auto &[name, panel] : panels) { QPushButton *btn = new QPushButton(name); btn->setCheckable(true); btn->setChecked(nav_btns->buttons().size() == 0); + btn->setIcon(QIcon(QPixmap("../assets/offroad/icon_blank.png"))); + btn->setIconSize(QSize(45, 45)); btn->setStyleSheet(R"( QPushButton { color: grey; @@ -533,7 +543,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { )"); btn->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); nav_btns->addButton(btn); - sidebar_layout->addWidget(btn, 0, Qt::AlignRight); + buttons_layout->addWidget(btn, 0, Qt::AlignLeft | Qt::AlignBottom); + buttons_layout->addSpacing(25); const int lr_margin = name != tr("Network") ? 50 : 0; // Network panel handles its own margins panel->setContentsMargins(lr_margin, 25, lr_margin, 25); @@ -546,11 +557,18 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { panel_widget->setCurrentWidget(w); }); } - sidebar_layout->setContentsMargins(50, 50, 100, 50); + sidebar_layout->setContentsMargins(50, 50, 50, 50); // main settings layout, sidebar + main panel QHBoxLayout *main_layout = new QHBoxLayout(this); + // add layout for close button + sidebar_layout->addLayout(close_btn_layout); + + // add layout for buttons scrolling + ScrollView *buttons_scrollview = new ScrollView(buttons_widget, this); + sidebar_layout->addWidget(buttons_scrollview); + sidebar_widget->setFixedWidth(500); main_layout->addWidget(sidebar_widget); main_layout->addWidget(panel_widget); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index f05e9d89cf..0452656811 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -125,8 +125,8 @@ void OnroadWindow::updateState(const UIState &s) { void issue_debug_snapshot(SubMaster &sm) { - auto longitudinal_plan = sm["longitudinalPlan"].getLongitudinalPlan(); - auto live_map_data = sm["liveMapData"].getLiveMapData(); + auto longitudinal_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); + auto live_map_data_sp = sm["liveMapDataSP"].getLiveMapDataSP(); auto car_state = sm["carState"].getCarState(); auto t = std::time(nullptr); @@ -139,30 +139,30 @@ void issue_debug_snapshot(SubMaster &sm) { os.precision(2); os << "Datetime: " << param_name_os.str() << ", vEgo: " << car_state.getVEgo() * 3.6 << "\n\n"; os.precision(6); - os << "Location: (" << live_map_data.getLastGpsLatitude() << ", " << live_map_data.getLastGpsLongitude() << ")\n"; + os << "Location: (" << live_map_data_sp.getLastGpsLatitude() << ", " << live_map_data_sp.getLastGpsLongitude() << ")\n"; os.precision(2); - os << "Bearing: " << live_map_data.getLastGpsBearingDeg() << "; "; - os << "GPSSpeed: " << live_map_data.getLastGpsSpeed() * 3.6 << "\n\n"; + os << "Bearing: " << live_map_data_sp.getLastGpsBearingDeg() << "; "; + os << "GPSSpeed: " << live_map_data_sp.getLastGpsSpeed() * 3.6 << "\n\n"; os.precision(1); - os << "Speed Limit: " << live_map_data.getSpeedLimit() * 3.6 << ", "; - os << "Valid: " << live_map_data.getSpeedLimitValid() << "\n"; - os << "Speed Limit Ahead: " << live_map_data.getSpeedLimitAhead() * 3.6 << ", "; - os << "Valid: " << live_map_data.getSpeedLimitAheadValid() << ", "; - os << "Distance: " << live_map_data.getSpeedLimitAheadDistance() << "\n"; - os << "Turn Speed Limit: " << live_map_data.getTurnSpeedLimit() * 3.6 << ", "; - os << "Valid: " << live_map_data.getTurnSpeedLimitValid() << ", "; - os << "End Distance: " << live_map_data.getTurnSpeedLimitEndDistance() << ", "; - os << "Sign: " << live_map_data.getTurnSpeedLimitSign() << "\n\n"; + os << "Speed Limit: " << live_map_data_sp.getSpeedLimit() * 3.6 << ", "; + os << "Valid: " << live_map_data_sp.getSpeedLimitValid() << "\n"; + os << "Speed Limit Ahead: " << live_map_data_sp.getSpeedLimitAhead() * 3.6 << ", "; + os << "Valid: " << live_map_data_sp.getSpeedLimitAheadValid() << ", "; + os << "Distance: " << live_map_data_sp.getSpeedLimitAheadDistance() << "\n"; + os << "Turn Speed Limit: " << live_map_data_sp.getTurnSpeedLimit() * 3.6 << ", "; + os << "Valid: " << live_map_data_sp.getTurnSpeedLimitValid() << ", "; + os << "End Distance: " << live_map_data_sp.getTurnSpeedLimitEndDistance() << ", "; + os << "Sign: " << live_map_data_sp.getTurnSpeedLimitSign() << "\n\n"; - const auto turn_speeds = live_map_data.getTurnSpeedLimitsAhead(); + const auto turn_speeds = live_map_data_sp.getTurnSpeedLimitsAhead(); os << "Turn Speed Limits Ahead:\n"; os << "VALUE\tDIST\tSIGN\n"; if (turn_speeds.size() == 0) { os << "-\t-\t-" << "\n\n"; } else { - const auto distances = live_map_data.getTurnSpeedLimitsAheadDistances(); - const auto signs = live_map_data.getTurnSpeedLimitsAheadSigns(); + const auto distances = live_map_data_sp.getTurnSpeedLimitsAheadDistances(); + const auto signs = live_map_data_sp.getTurnSpeedLimitsAheadSigns(); for(int i = 0; i < turn_speeds.size(); i++) { os << turn_speeds[i] * 3.6 << "\t" << distances[i] << "\t" << signs[i] << "\n"; } @@ -170,17 +170,17 @@ void issue_debug_snapshot(SubMaster &sm) { } os << "SPEED LIMIT CONTROLLER:\n"; - os << "sl: " << longitudinal_plan.getSpeedLimit() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan.getSpeedLimitControlState()) << ", "; - os << "isMap: " << longitudinal_plan.getIsMapSpeedLimit() << "\n\n"; + os << "sl: " << longitudinal_plan_sp.getSpeedLimit() * 3.6 << ", "; + os << "state: " << int(longitudinal_plan_sp.getSpeedLimitControlState()) << ", "; + os << "isMap: " << longitudinal_plan_sp.getIsMapSpeedLimit() << "\n\n"; os << "TURN SPEED CONTROLLER:\n"; - os << "speed: " << longitudinal_plan.getTurnSpeed() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan.getTurnSpeedControlState()) << "\n\n"; + os << "speed: " << longitudinal_plan_sp.getTurnSpeed() * 3.6 << ", "; + os << "state: " << int(longitudinal_plan_sp.getTurnSpeedControlState()) << "\n\n"; os << "VISION TURN CONTROLLER:\n"; - os << "speed: " << longitudinal_plan.getVisionTurnSpeed() * 3.6 << ", "; - os << "state: " << int(longitudinal_plan.getVisionTurnControllerState()); + os << "speed: " << longitudinal_plan_sp.getVisionTurnSpeed() * 3.6 << ", "; + os << "state: " << int(longitudinal_plan_sp.getVisionTurnControllerState()); Params().put(param_name_os.str().c_str(), os.str().c_str(), os.str().length()); uiState()->scene.display_debug_alert_frame = sm.frame; @@ -193,12 +193,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { UIState *s = uiState(); UIScene &scene = s->scene; SubMaster &sm = *(uiState()->sm); - auto longitudinal_plan = sm["longitudinalPlan"].getLongitudinalPlan(); + auto longitudinal_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); QRect debug_tap_rect = QRect(rect().center().x() - 200, rect().center().y() - 200, 400, 400); QRect speed_limit_touch_rect = speed_sgn_rc.adjusted(-50, -50, 50, 50); - if (longitudinal_plan.getSpeedLimit() > 0.0 && speed_limit_touch_rect.contains(e->x(), e->y())) { + if (longitudinal_plan_sp.getSpeedLimit() > 0.0 && speed_limit_touch_rect.contains(e->x(), e->y())) { // If touching the speed limit sign area when visible scene.last_speed_limit_sign_tap = seconds_since_boot(); params.putBool("LastSpeedLimitSignTap", true); @@ -373,8 +373,8 @@ void ExperimentalButton::changeMode() { void ExperimentalButton::updateState(const UIState &s) { const auto cs = (*s.sm)["controlsState"].getControlsState(); - const auto lp = (*s.sm)["longitudinalPlan"].getLongitudinalPlan(); - bool eng = (cs.getEngageable() || cs.getEnabled()) && !(lp.getVisionTurnControllerState() > cereal::LongitudinalPlan::VisionTurnControllerState::DISABLED); + const auto lp_sp = (*s.sm)["longitudinalPlanSP"].getLongitudinalPlanSP(); + bool eng = (cs.getEngageable() || cs.getEnabled()) && !(lp_sp.getVisionTurnControllerState() > cereal::LongitudinalPlanSP::VisionTurnControllerState::DISABLED); if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { engageable = eng; experimental_mode = cs.getExperimentalMode(); @@ -437,7 +437,7 @@ void OnroadSettingsButton::updateState(const UIState &s) { // Window that shows camera view and variety of info drawn on top AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { pm = std::make_unique>({"uiDebug"}); - e2e_state = std::make_unique>({"e2eLongState"}); + e2e_state = std::make_unique>({"e2eLongStateSP"}); main_layout = new QVBoxLayout(this); main_layout->setMargin(UI_BORDER_SIZE); @@ -515,13 +515,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { const bool cs_alive = sm.alive("controlsState"); const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid(); const auto cs = sm["controlsState"].getControlsState(); + const auto cs_sp = sm["controlsStateSP"].getControlsStateSP(); const auto car_state = sm["carState"].getCarState(); const auto nav_instruction = sm["navInstruction"].getNavInstruction(); const auto car_control = sm["carControl"].getCarControl(); const auto radar_state = sm["radarState"].getRadarState(); const auto gpsLocationExternal = sm["gpsLocationExternal"].getGpsLocationExternal(); const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); - const auto lateral_plan = sm["lateralPlan"].getLateralPlan(); + const auto lateral_plan_sp = sm["lateralPlanSP"].getLateralPlanSP(); // Handle older routes where vCruiseCluster is not set float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster(); @@ -562,7 +563,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { standStillTimer = s.scene.stand_still_timer; standStill = car_state.getStandstill(); - standstillElapsedTime = lateral_plan.getStandstillElapsed(); + standstillElapsedTime = lateral_plan_sp.getStandstillElapsed(); hideVEgoUi = s.scene.hide_vego_ui; @@ -572,7 +573,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { lead_d_rel = radar_state.getLeadOne().getDRel(); lead_v_rel = radar_state.getLeadOne().getVRel(); lead_status = radar_state.getLeadOne().getStatus(); - lateralState = QString::fromStdString(cs.getLateralState()); + lateralState = QString::fromStdString(cs_sp.getLateralState()); angleSteers = car_state.getSteeringAngleDeg(); steerAngleDesired = cs.getLateralControlState().getPidState().getSteeringAngleDesiredDeg(); curvature = cs.getCurvature(); @@ -587,7 +588,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { steeringTorqueEps = car_state.getSteeringTorqueEps(); bearingAccuracyDeg = gpsLocationExternal.getBearingAccuracyDeg(); bearingDeg = gpsLocationExternal.getBearingDeg(); - torquedUseParams = ltp.getUseParams() || s.scene.live_torque_toggle; + torquedUseParams = (ltp.getUseParams() || s.scene.live_torque_toggle) && !s.scene.torqued_override; latAccelFactorFiltered = ltp.getLatAccelFactorFiltered(); frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered(); liveValid = ltp.getLiveValid(); @@ -597,7 +598,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { left_blinker = car_state.getLeftBlinker(); right_blinker = car_state.getRightBlinker(); - lane_change_edge_block = lateral_plan.getLaneChangeEdgeBlock(); + lane_change_edge_block = lateral_plan_sp.getLaneChangeEdgeBlock(); // update engageability/experimental mode button experimental_btn->updateState(s); @@ -627,37 +628,37 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { main_layout->setAlignment(onroad_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom); } - const auto lp = sm["longitudinalPlan"].getLongitudinalPlan(); - slcState = lp.getSpeedLimitControlState(); + const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); + slcState = lp_sp.getSpeedLimitControlState(); if (sm.frame % (UI_FREQ / 2) == 0) { - const auto vtcState = lp.getVisionTurnControllerState(); - const float vtc_speed = lp.getVisionTurnSpeed() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); - const auto lpSoruce = lp.getLongitudinalPlanSource(); + const auto vtcState = lp_sp.getVisionTurnControllerState(); + const float vtc_speed = lp_sp.getVisionTurnSpeed() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + const auto lpSoruce = lp_sp.getLongitudinalPlanSource(); QColor vtc_color = tcs_colors[int(vtcState)]; - vtc_color.setAlpha(lpSoruce == cereal::LongitudinalPlan::LongitudinalPlanSource::TURN ? 255 : 100); + vtc_color.setAlpha(lpSoruce == cereal::LongitudinalPlanSP::LongitudinalPlanSource::TURN ? 255 : 100); - showVTC = vtcState > cereal::LongitudinalPlan::VisionTurnControllerState::DISABLED; + showVTC = vtcState > cereal::LongitudinalPlanSP::VisionTurnControllerState::DISABLED; vtcSpeed = QString::number(std::nearbyint(vtc_speed)); vtcColor = vtc_color; showDebugUI = s.scene.show_debug_ui; - const auto lmd = sm["liveMapData"].getLiveMapData(); + const auto lmd_sp = sm["liveMapDataSP"].getLiveMapDataSP(); - const auto data_type = int(lmd.getDataType()); + const auto data_type = int(lmd_sp.getDataType()); const QString data_type_draw(data_type == 2 ? "🌐 " : ""); - roadName = QString::fromStdString(lmd.getCurrentRoadName()); + roadName = QString::fromStdString(lmd_sp.getCurrentRoadName()); roadName = !roadName.isEmpty() ? data_type_draw + roadName : ""; - float speed_limit_slc = lp.getSpeedLimit() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); - const float speed_limit_offset = lp.getSpeedLimitOffset() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + float speed_limit_slc = lp_sp.getSpeedLimit() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + const float speed_limit_offset = lp_sp.getSpeedLimitOffset() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); const bool sl_force_active = s.scene.speed_limit_control_enabled && seconds_since_boot() < s.scene.last_speed_limit_sign_tap + 2.0; const bool sl_inactive = !sl_force_active && (!s.scene.speed_limit_control_enabled || - slcState == cereal::LongitudinalPlan::SpeedLimitControlState::INACTIVE); + slcState == cereal::LongitudinalPlanSP::SpeedLimitControlState::INACTIVE); const bool sl_temp_inactive = !sl_force_active && (s.scene.speed_limit_control_enabled && - slcState == cereal::LongitudinalPlan::SpeedLimitControlState::TEMP_INACTIVE); - const int sl_distance = int(lp.getDistToSpeedLimit() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10; + slcState == cereal::LongitudinalPlanSP::SpeedLimitControlState::TEMP_INACTIVE); + const int sl_distance = int(lp_sp.getDistToSpeedLimit() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10; const QString sl_distance_str(QString::number(sl_distance) + (s.scene.is_metric ? "m" : "f")); const QString sl_offset_str(speed_limit_offset > 0.0 ? speed_limit_offset < 0.0 ? "-" + QString::number(std::nearbyint(std::abs(speed_limit_offset))) : @@ -670,21 +671,21 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { speedLimitSLC = speed_limit_slc; slcSubText = sl_substring; slcSubTextSize = sl_inactive || sl_temp_inactive || sl_distance > 0 ? 25.0 : 27.0; - mapSourcedSpeedLimit = lp.getIsMapSpeedLimit(); + mapSourcedSpeedLimit = lp_sp.getIsMapSpeedLimit(); slcActive = !sl_inactive && !sl_temp_inactive; overSpeedLimit = (((speed_limit_slc + speed_limit_offset) < speed) && !sl_inactive && !sl_temp_inactive) || ((speed_limit_slc < speed) && (speed_limit_slc > 0.0) && (sl_inactive || sl_temp_inactive)); - const float tsc_speed = lp.getTurnSpeed() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); - const auto tscState = lp.getTurnSpeedControlState(); - const int t_distance = int(lp.getDistToTurn() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10; + const float tsc_speed = lp_sp.getTurnSpeed() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + const auto tscState = lp_sp.getTurnSpeedControlState(); + const int t_distance = int(lp_sp.getDistToTurn() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10; const QString t_distance_str(QString::number(t_distance) + (s.scene.is_metric ? "m" : "f")); showTurnSpeedLimit = tsc_speed > 0.0 && (tsc_speed < speed || s.scene.show_debug_ui); turnSpeedLimit = QString::number(std::nearbyint(tsc_speed)); tscSubText = t_distance > 0 ? t_distance_str : QString(""); - tscActive = tscState > cereal::LongitudinalPlan::SpeedLimitControlState::TEMP_INACTIVE; - curveSign = lp.getTurnSign(); + tscActive = tscState > cereal::LongitudinalPlanSP::SpeedLimitControlState::TEMP_INACTIVE; + curveSign = lp_sp.getTurnSign(); } longitudinalPersonality = s.scene.longitudinal_personality; @@ -1992,9 +1993,9 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { pm->send("uiDebug", msg); MessageBuilder e2e_long_msg; - auto e2eLongStatus = e2e_long_msg.initEvent().initE2eLongState(); + auto e2eLongStatus = e2e_long_msg.initEvent().initE2eLongStateSP(); e2eLongStatus.setStatus(e2eStatus); - e2e_state->send("e2eLongState", e2e_long_msg); + e2e_state->send("e2eLongStateSP", e2e_long_msg); } void AnnotatedCameraWidget::showEvent(QShowEvent *event) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index d679a74efe..e5ad14dd0d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -227,7 +227,7 @@ private: int blinker_frame; int blinker_state = 0; - cereal::LongitudinalPlan::SpeedLimitControlState slcState; + cereal::LongitudinalPlanSP::SpeedLimitControlState slcState; int longitudinalPersonality; int dynamicLaneProfile; diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 44d7f49300..2b209c1b63 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -220,7 +220,7 @@ public: background-color: #4a4a4a; } QPushButton:checked:enabled { - background-color: #33Ab4C; + background-color: #1e79e8; } QPushButton:disabled { color: #33E4E4E4; diff --git a/selfdrive/ui/qt/widgets/toggle.cc b/selfdrive/ui/qt/widgets/toggle.cc index 82302ad5bc..c8f12bc272 100644 --- a/selfdrive/ui/qt/widgets/toggle.cc +++ b/selfdrive/ui/qt/widgets/toggle.cc @@ -75,9 +75,9 @@ void Toggle::setEnabled(bool value) { enabled = value; if (value) { circleColor.setRgb(0xfafafa); - green.setRgb(0x33ab4c); + green.setRgb(0x1e79e8); } else { circleColor.setRgb(0x888888); - green.setRgb(0x227722); + green.setRgb(0x125db8); } } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 62bd20f090..9f58ede777 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -214,15 +214,15 @@ static void update_state(UIState *s) { scene.light_sensor = std::max(100.0f - scale * cam_state.getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; - if (sm.updated("lateralPlan")) { - scene.dynamic_lane_profile_status = sm["lateralPlan"].getLateralPlan().getDynamicLaneProfileStatus(); + if (sm.updated("lateralPlanSP")) { + scene.dynamic_lane_profile_status = sm["lateralPlanSP"].getLateralPlanSP().getDynamicLaneProfileStatus(); } if (sm.updated("controlsState")) { scene.controlsState = sm["controlsState"].getControlsState(); } - if (sm.updated("longitudinalPlan")) { + if (sm.updated("longitudinalPlanSP")) { for (int i = 0; i < std::size(scene.e2eX); i++) { - scene.e2eX[i] = sm["longitudinalPlan"].getLongitudinalPlan().getE2eX()[i]; + scene.e2eX[i] = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getE2eX()[i]; } } } @@ -254,6 +254,7 @@ void ui_update_params(UIState *s) { s->scene.e2e_long_alert_ui = params.getBool("EndToEndLongAlertUI"); s->scene.map_3d_buildings = params.getBool("Map3DBuildings"); s->scene.live_torque_toggle = params.getBool("LiveTorque"); + s->scene.torqued_override = params.getBool("TorquedOverride"); // Handle Onroad Screen Off params if (s->scene.onroadScreenOff > 0) { @@ -374,8 +375,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "longitudinalPlan", "liveMapData", - "carControl", "lateralPlan", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "longitudinalPlanSP", "liveMapDataSP", + "carControl", "lateralPlanSP", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", "controlsStateSP" }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 76911b6211..741dd82186 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -142,10 +142,10 @@ static std::map alert_colors = { }; const QColor tcs_colors [] = { - [int(cereal::LongitudinalPlan::VisionTurnControllerState::DISABLED)] = QColor(0x0, 0x0, 0x0, 0xff), - [int(cereal::LongitudinalPlan::VisionTurnControllerState::ENTERING)] = QColor(0xC9, 0x22, 0x31, 0xf1), - [int(cereal::LongitudinalPlan::VisionTurnControllerState::TURNING)] = QColor(0xDA, 0x6F, 0x25, 0xf1), - [int(cereal::LongitudinalPlan::VisionTurnControllerState::LEAVING)] = QColor(0x17, 0x86, 0x44, 0xf1), + [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::DISABLED)] = QColor(0x0, 0x0, 0x0, 0xff), + [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::ENTERING)] = QColor(0xC9, 0x22, 0x31, 0xf1), + [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::TURNING)] = QColor(0xDA, 0x6F, 0x25, 0xf1), + [int(cereal::LongitudinalPlanSP::VisionTurnControllerState::LEAVING)] = QColor(0x17, 0x86, 0x44, 0xf1), }; typedef struct UIScene { @@ -238,6 +238,8 @@ typedef struct UIScene { bool onroad_settings_visible; bool map_3d_buildings; + + bool torqued_override; } UIScene; class UIState : public QObject {