mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-24 14:22:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dev-priv/master' into subaru-gen2-torque-increase

Browse Source 
# Conflicts:
#	panda
This commit is contained in:
Jason Wen
2023-10-01 03:21:51 -04:00
parent 705bd1a42a e6d11a46b4
commit 95ef2c09bb
37 changed files with 336 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: bf00063015...318da0bb3e
panda
+1 -1
Submodule panda updated: 92aaeb38b0...7f35e86bf7
selfdrive/boardd/boardd.cc
-1
View File
@@ -361,7 +361,6 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
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);
selfdrive/car/chrysler/carcontroller.py
+9 -9
View File
@@ -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")
selfdrive/car/honda/carcontroller.py
+9 -9
View File
@@ -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")
selfdrive/car/hyundai/carcontroller.py
+9 -9
View File
@@ -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")
selfdrive/car/hyundai/interface.py
+3
View File
@@ -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
selfdrive/car/hyundai/radar_interface.py
+10 -4
View File
@@ -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')
selfdrive/car/hyundai/values.py
+1
View File
@@ -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:
selfdrive/car/mazda/carcontroller.py
+9 -9
View File
@@ -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")
selfdrive/car/volkswagen/carcontroller.py
+9 -9
View File
@@ -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")
selfdrive/controls/controlsd.py
+13 -14
View File
@@ -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')
selfdrive/controls/lib/events.py
+1 -1
View File
@@ -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(
selfdrive/controls/lib/lateral_planner.py
+25 -16
View File
@@ -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)
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+13 -13
View File
@@ -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
selfdrive/controls/lib/longitudinal_planner.py
+40 -25
View File
@@ -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)
selfdrive/controls/lib/speed_limit_controller.py
+3 -3
View File
@@ -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.
selfdrive/controls/lib/turn_speed_controller.py
+3 -3
View File
@@ -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
selfdrive/controls/lib/vision_turn_controller.py
+3 -3
View File
@@ -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.
selfdrive/controls/plannerd.py
+4 -2
View File
@@ -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()
selfdrive/controls/radard.py
+12 -8
View File
@@ -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)
selfdrive/debug/cycle_alerts.py
+2 -1
View File
@@ -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'])
selfdrive/debug/internal/hands_on_wheel_moniotr.py
+2 -2
View File
@@ -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):
selfdrive/manager/manager.py
+2 -2
View File
@@ -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"),
selfdrive/mapd/config.py
+1 -1
View File
@@ -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.
selfdrive/mapd/lib/osm.py
+2 -2
View File
@@ -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):
selfdrive/mapd/mapd.py
+30 -29
View File
@@ -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()
selfdrive/monitoring/dmonitoringd.py
+7 -2
View File
@@ -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
selfdrive/monitoring/hands_on_wheel_monitor.py
+2 -2
View File
@@ -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
selfdrive/monitoring/test_hands_monitoring.py
+2 -2
View File
@@ -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
selfdrive/ui/qt/offroad/settings.cc
+33 -15
View File
@@ -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<QPair<QString, QWidget *>> 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);
selfdrive/ui/qt/onroad.cc
+58 -57
View File
@@ -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<PubMaster, const std::initializer_list<const char *>>({"uiDebug"});
e2e_state = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"e2eLongState"});
e2e_state = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"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) {
selfdrive/ui/qt/onroad.h
+1 -1
View File
@@ -227,7 +227,7 @@ private:
int blinker_frame;
int blinker_state = 0;
cereal::LongitudinalPlan::SpeedLimitControlState slcState;
cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
int longitudinalPersonality;
int dynamicLaneProfile;
selfdrive/ui/qt/widgets/controls.h
+1 -1
View File
@@ -220,7 +220,7 @@ public:
background-color: #4a4a4a;
}
QPushButton:checked:enabled {
background-color: #33Ab4C;
background-color: #1e79e8;
}
QPushButton:disabled {
color: #33E4E4E4;
selfdrive/ui/qt/widgets/toggle.cc
+2 -2
View File
@@ -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);
}
}
selfdrive/ui/ui.cc
+7 -6
View File
@@ -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<SubMaster, const std::initializer_list<const char *>>({
"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;
selfdrive/ui/ui.h
+6 -4
View File
@@ -142,10 +142,10 @@ static std::map<cereal::ControlsState::AlertStatus, QColor> 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 {