diff --git a/cereal/log.capnp b/cereal/log.capnp index 82963e6..1bc9e05 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1218,8 +1218,6 @@ struct ModelDataV2 { desiredCurvature @0 :Float32; desiredAcceleration @1 :Float32; shouldStop @2 :Bool; - desiredVelocity @3 :Float32; - desiredJerk @4 :Float32; } } @@ -1294,9 +1292,9 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { xState @40: Int32; trafficState @41: Int32; events @42:List(OnroadEvent); - vTarget @43: Float32; - xTarget @44: Float32; - jTarget @45: Float32; + vTargetNotUsed @43: Float32; + cruiseTarget @44: Float32; + jTargetNotUsed @45: Float32; tFollow @46: Float32; desiredDistance @47: Float32; myDrivingMode @48: Int32; diff --git a/cereal/services.py b/cereal/services.py index fb9af62..a44576d 100644 --- a/cereal/services.py +++ b/cereal/services.py @@ -36,7 +36,7 @@ _services: dict[str, tuple] = { "errorLogMessage": (True, 0., 1), "liveCalibration": (True, 4., 4), "liveTorqueParameters": (True, 4., 1), - "liveDelay": (True, 4., 1), + #"liveDelay": (True, 4., 1), "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), diff --git a/common/params_keys.h b/common/params_keys.h index f7f7ec5..be7f941 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -184,7 +184,7 @@ inline static std::unordered_map keys = { {"AutoRoadSpeedAdjust", PERSISTENT}, {"StopDistanceCarrot", PERSISTENT}, {"ComfortBrake", PERSISTENT}, - {"JLeadFactor", PERSISTENT}, + {"JLeadFactor2", PERSISTENT}, {"CruiseButtonMode", PERSISTENT}, {"CruiseButtonTest1", PERSISTENT}, {"CruiseButtonTest2", PERSISTENT}, @@ -206,6 +206,7 @@ inline static std::unordered_map keys = { {"LongTuningKiV", PERSISTENT}, {"LongTuningKf", PERSISTENT}, {"LongActuatorDelay", PERSISTENT }, + {"LongVelocityControl", PERSISTENT}, {"VEgoStopping", PERSISTENT }, {"RadarReactionFactor", PERSISTENT}, {"EnableRadarTracks", PERSISTENT}, diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index b5646c2..ef09bce 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -95,10 +95,10 @@ function launch { # events.py 한글로 변경 및 파일이 교체된 상태인지 확인 if [ "${LANG}" = "main_ko" ] && [[ ! "${EVENTSTAT}" == *"modified: selfdrive/controls/lib/events.py"* ]]; then - cp -f $BASEDIR/selfdrive/selfdrived/events.py $BASEDIR/scripts/add/events_en.py - cp -f $BASEDIR/scripts/add/events_ko.py $BASEDIR/selfdrive/selfdrived/events.py + cp -f $DIR/selfdrive/selfdrived/events.py $DIR/scripts/add/events_en.py + cp -f $DIR/scripts/add/events_ko.py $DIR/selfdrive/selfdrived/events.py elif [ "${LANG}" = "main_en" ] && [[ "${EVENTSTAT}" == *"modified: selfdrive/controls/lib/events.py"* ]]; then - cp -f $BASEDIR/scripts/add/events_en.py $BASEDIR/selfdrive/selfdrived/events.py + cp -f $DIR/scripts/add/events_en.py $DIR/selfdrive/selfdrived/events.py fi # start manager diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 0f32f9f..ae28e85 100644 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -120,7 +120,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0.] if candidate in (CAMERA_ACC_CAR | SDGM_CAR): - ret.experimentalLongitudinalAvailable = candidate not in (CC_ONLY_CAR | SDGM_CAR) + ret.alphaLongitudinalAvailable = candidate not in (CC_ONLY_CAR | SDGM_CAR) ret.networkLocation = NetworkLocation.fwdCamera ret.radarUnavailable = True # no radar ret.pcmCruise = True @@ -367,11 +367,11 @@ class CarInterface(CarInterfaceBase): elif candidate in CC_ONLY_CAR: ret.flags |= GMFlags.CC_LONG.value ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.CC_LONG.value - if experimental_long: + if alpha_long: ret.openpilotLongitudinalControl = True ret.flags |= GMFlags.CC_LONG.value ret.radarUnavailable = True - ret.experimentalLongitudinalAvailable = True + ret.alphaLongitudinalAvailable = True ret.minEnableSpeed = 24 * CV.MPH_TO_MS ret.pcmCruise = True diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index f89a72f..1a6dc3b 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -485,6 +485,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["DAW_ICON"] = 0 values["SOUNDS_2"] = 0 # 2: STEER중지 경고후에도 사운드가 나옴. + values["SOUNDS_4"] = 0 # 차선변경알림? 에이 그냥0으로.. if values["ALERTS_3"] in [3, 4, 13, 17, 19, 26, 7, 8, 9, 10]: values["ALERTS_3"] = 0 diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 54bf5d0..13b6a61 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -660,6 +660,8 @@ class VCruiseCarrot: #self.events.append(EventName.stopStop) if self.desiredSpeed < self.v_ego_kph_set: self._cruise_control(1, -1, "Cruise on (desired speed)") + if self._cruise_ready and self.xState in [3]: + self._cruise_control(1, -1, "Cruise on (traffic sign)") if self._gas_pressed_count > self._gas_tok_timer: if CS.aEgo < -0.5: diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index e27df55..3334239 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -169,7 +169,7 @@ class CarrotPlanner: elif self.params_count == 40: self.stop_distance = self.params.get_float("StopDistanceCarrot") / 100. self.comfortBrake = self.params.get_float("ComfortBrake") / 100. - self.j_lead_factor = self.params.get_float("JLeadFactor") / 100. + self.j_lead_factor = self.params.get_float("JLeadFactor2") / 100. self.eco_over_speed = self.params.get_int("CruiseEcoControl") elif self.params_count >= 100: @@ -215,8 +215,9 @@ class CarrotPlanner: if self.dynamicTFollow > 0.0: gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) t_follow += gap_dist_adjust - if gap_dist_adjust < 0: - self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게. + #if gap_dist_adjust < 0: + # self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게. + self.jerk_factor_apply = np.interp(abs(lead.jLead), [0, 2], [self.jerk_factor, self.jerk_factor * self.j_lead_factor]) return t_follow diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 05b915f..ab097de 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -380,16 +380,16 @@ }, { "group": "주행튜닝", - "name": "JLeadFactor", + "name": "JLeadFactor2", "title": "JerkLeadFactor(0)", - "descr": "x0.01, 값이 크면 전방차량의 Jerk에 민감하게 반응", + "descr": "값이 작으면 전방차량의 Jerk에 민감하게 반응", "egroup": "LONG", "etitle": "JerkLeadFactor(0)", "edescr": "", - "min": 0, + "min": 10, "max": 100, - "default": 0, - "unit": 5 + "default": 100, + "unit": 10 }, { "group": "주행튜닝", @@ -443,6 +443,19 @@ "default": 20, "unit": 5 }, + { + "group": "주행튜닝", + "name": "LongVelocityControl", + "title": "LongVelocityControl(0)", + "descr": "0: Accel PID, 1: Velocity PID", + "egroup": "LONG", + "etitle": "LongVelocityControl(0)", + "edescr": "0: Accel PID, 1: Velocity PID", + "min": 0, + "max": 1, + "default": 0, + "unit": 1 + }, { "group": "주행튜닝", "name": "VEgoStopping", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 26c0960..b5b4a6c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -132,7 +132,7 @@ class Controls: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL - accel, aTargetNow, jerk = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) + accel, aTargetNow, jerk = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan, self.sm['radarState']) actuators.accel = float(accel) actuators.aTargetNow = float(aTargetNow) actuators.jerk = float(jerk) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e2a6215..0cbd606 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -88,24 +88,17 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: return 0.0 -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05, jerks=None): +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): if len(speeds) == len(t_idxs): v_now = speeds[0] a_now = accels[0] - v_target = np.interp(action_t, t_idxs, speeds) - if jerks is not None: - j_target = np.interp(action_t, t_idxs, jerks) - else: - j_target = 0.0 a_target = 2 * (v_target - v_now) / (action_t) - a_now v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) else: v_target = 0.0 - j_target = 0.0 v_target_1sec = 0.0 a_target = 0.0 should_stop = (v_target < vEgoStopping and v_target_1sec < vEgoStopping) - return a_target, should_stop, v_target, j_target - + return a_target, should_stop diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f9a5bfb..e9a8ff8 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -66,19 +66,29 @@ class LongControl: def reset(self): self.pid.reset() - def update(self, active, CS, long_plan, accel_limits, t_since_plan): + def update(self, active, CS, long_plan, accel_limits, t_since_plan, radarState): soft_hold_active = CS.softHoldActive > 0 a_target = long_plan.aTarget - v_target = long_plan.vTarget - j_target = long_plan.jTarget should_stop = long_plan.shouldStop - + velocity_pid = self.params.get_float("LongVelocityControl") + long_delay = self.params.get_float("LongActuatorDelay")*0.01 + t_since_plan + + j_lead_factor = (1 - self.params.get_float("JLeadFactor2") * 0.01) * 2.0 # 최대 2 sec 미래를 봄.. # JLeadFactor2가 0에 가까우면 더 민감함. 100이면 안함. + if j_lead_factor > 0.0 and radarState.leadOne.status and velocity_pid == 0: + j_lead = np.clip(radarState.leadOne.jLead, -2.0, 2.0) + plan_alpha = np.interp(abs(j_lead), [0.0, 2.0], [0.0, j_lead_factor]) + else: + j_lead = 0.0 + plan_alpha = 0.0 + speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target_now = np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds) - a_target_now = np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) - j_target_now = long_plan.jerks[0] #np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks) + j_target_now = long_plan.jerks[0] #np.interp(long_delay, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks) + if j_target_now < 0.2 and j_lead > 0.0: + plan_alpha = 0.0 + v_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds) + a_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) else: v_target_now = a_target_now = j_target_now = 0.0 @@ -126,10 +136,12 @@ class LongControl: self.reset() else: # LongCtrlState.pid - #error = a_target_now - CS.aEgo - error = v_target_now - CS.vEgo + if velocity_pid == 0: + error = a_target_now - CS.aEgo + else: + error = v_target_now - CS.vEgo output_accel = self.pid.update(error, speed=CS.vEgo, - feedforward=a_target) + feedforward=a_target_now) self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel, a_target_now, j_target_now diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2bb5001..6c66189 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -318,48 +318,18 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - @staticmethod - def extrapolate_lead_with_j(x_lead, v_lead, a_lead, j_lead, a_lead_tau): - a_lead_traj = np.zeros_like(T_IDXS) - a_lead_traj[0] = a_lead - - """ - for i in range(1, len(T_IDXS)): - dt = T_IDXS[i] - T_IDXS[i - 1] - a_lead_traj[i] = ( - a_lead_traj[i - 1] * np.exp(-a_lead_tau * dt) # `a_lead`만 감쇄 - + j_lead * dt # `j_lead` 감쇄 없이 그대로 적용 - ) - """ - # `j_lead`도 감쇄하고 싶다면 아래 코드 사용 - j_lead_tau = 0.4 - for i in range(1, len(T_IDXS)): - dt = T_IDXS[i] - T_IDXS[i - 1] - j_lead_decayed = j_lead * np.exp(-j_lead_tau * dt) - a_lead_traj[i] = ( - a_lead_traj[i - 1] * np.exp(-a_lead_tau * dt) - + j_lead_decayed * dt - ) - - v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) - x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) - lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) - return lead_xv - - def process_lead(self, lead, carrot): + def process_lead(self, lead): v_ego = self.x0[1] if lead is not None and lead.status: x_lead = lead.dRel v_lead = lead.vLead a_lead = lead.aLeadK - j_lead = lead.jLead a_lead_tau = lead.aLeadTau else: # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 v_lead = v_ego + 10.0 a_lead = 0.0 - j_lead = 0.0 a_lead_tau = _LEAD_ACCEL_TAU # MPC will not converge if immediate crash is expected @@ -368,19 +338,7 @@ class LongitudinalMpc: x_lead = np.clip(x_lead, min_x_lead, 1e8) v_lead = np.clip(v_lead, 0.0, 1e8) a_lead = np.clip(a_lead, -10., 5.) - j_lead = np.clip(j_lead, -2., 2.) - - j_lead *= carrot.j_lead_factor - #if j_lead > 0 and a_lead < 0 and (v_lead - v_ego) > 0 and x_lead > self.desired_distance: - if j_lead > 0 and a_lead < 0 and x_lead > self.desired_distance: - a_lead += min(j_lead, 0.5) - a_lead = min(a_lead, 0.0) - - if j_lead < 0 and a_lead < -0.5: - a_lead -= min(abs(j_lead)*1.5, 0.8) - lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) - #lead_xv = self.extrapolate_lead_with_j(x_lead, v_lead, a_lead, j_lead, a_lead_tau) return lead_xv, v_lead def set_accel_limits(self, min_a, max_a): @@ -395,8 +353,8 @@ class LongitudinalMpc: a_ego = self.x0[2] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne, carrot) - lead_xv_1, _ = self.process_lead(radarstate.leadTwo, carrot) + lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne) + lead_xv_1, _ = self.process_lead(radarstate.leadTwo) mode = self.mode comfort_brake = carrot.comfort_brake diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 672c81d..d4d5f5b 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -65,8 +65,6 @@ class LongitudinalPlanner: self.v_model_error = 0.0 self.output_a_target = 0.0 self.output_should_stop = False - self.output_v_target = 0.0 - self.output_j_target = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -105,7 +103,7 @@ class LongitudinalPlanner: else: accel_coast = ACCEL_MAX - v_ego = sm['modelV2'].velocity.x[0] + v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) self.v_cruise_kph = carrot.update(sm, v_cruise_kph) @@ -189,24 +187,18 @@ class LongitudinalPlanner: vEgoStopping = Params().get_float("VEgoStopping") * 0.01 action_t = longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc, v_target, j_target = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=vEgoStopping, jerks=self.j_desired_trajectory) + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, + action_t=action_t, vEgoStopping=vEgoStopping) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_v_target_e2e = sm['modelV2'].action.desiredVelocity - output_j_target_e2e = sm['modelV2'].action.desiredJerk output_should_stop_e2e = sm['modelV2'].action.shouldStop if self.mpc.mode == 'acc': self.output_a_target = output_a_target_mpc self.output_should_stop = output_should_stop_mpc - self.output_v_target = v_target - self.output_j_target = j_target else: self.output_a_target = min(output_a_target_mpc, output_a_target_e2e) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - self.output_v_target = output_v_target_e2e - self.output_j_target = output_j_target_e2e #for idx in range(2): # accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) @@ -236,11 +228,9 @@ class LongitudinalPlanner: longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) - longitudinalPlan.vTarget = float(self.output_v_target) - longitudinalPlan.jTarget = float(self.output_j_target) longitudinalPlan.xState = carrot.xState.value longitudinalPlan.trafficState = carrot.trafficState.value - longitudinalPlan.xTarget = self.v_cruise_kph + longitudinalPlan.cruiseTarget = self.v_cruise_kph longitudinalPlan.tFollow = float(self.mpc.t_follow) longitudinalPlan.desiredDistance = float(self.mpc.desired_distance) longitudinalPlan.events = carrot.events.to_msg() diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 488add3..2f2edca 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -43,7 +43,7 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' -LAT_SMOOTH_SECONDS = 0.3 +LAT_SMOOTH_SECONDS = 0.13 LONG_SMOOTH_SECONDS = 0.3 def smooth_value(val, prev_val, tau): @@ -54,21 +54,17 @@ def smooth_value(val, prev_val, tau): def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: plan = model_output['plan'][0] - desired_accel, should_stop, desired_velocity, desired_jerk = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], + desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], plan[:,Plan.ACCELERATION][:,0], ModelConstants.T_IDXS, action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - desired_velocity = smooth_value(desired_velocity, prev_action.desiredVelocity, LONG_SMOOTH_SECONDS) - desired_jerk = smooth_value(desired_jerk, prev_action.desiredJerk, LONG_SMOOTH_SECONDS) desired_curvature = model_output['desired_curvature'][0, 0] desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), - desiredVelocity=float(desired_velocity), - desiredJerk=float(desired_jerk), shouldStop=bool(should_stop)) class FrameMeta: diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 03d2cb2..53634fc 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -82,7 +82,7 @@ class SelfdriveD: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'carrotMan', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index dbfe3a9..5d4a298 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -1864,7 +1864,7 @@ public: int trafficState = 0; int trafficState_carrot = 0; int active_carrot = 0; - float xTarget = 0.0; + float cruiseTarget = 0.0; int myDrivingMode = 1; QString szPosRoadName = ""; @@ -1967,7 +1967,7 @@ public: xState = lp.getXState(); trafficState = lp.getTrafficState(); - xTarget = lp.getXTarget(); + cruiseTarget = lp.getCruiseTarget(); myDrivingMode = lp.getMyDrivingMode(); s->max_distance = std::clamp(*(model_position.getX().end() - 1), @@ -2180,8 +2180,8 @@ public: ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK); ui_draw_text(s, apply_x, apply_y - 50, apply_source.toStdString().c_str(), 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK); } - else if(abs(xTarget - v_cruise) > 0.5) { - sprintf(apply_speed_str, "%.0f", xTarget); + else if(abs(cruiseTarget - v_cruise) > 0.5) { + sprintf(apply_speed_str, "%.0f", cruiseTarget); ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK); ui_draw_text(s, apply_x, apply_y - 50, "eco", 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK); } @@ -2206,6 +2206,17 @@ public: if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD); strcpy(driving_mode_str_last, driving_mode_str); + if(sm.updated(s->gps_location_socket)) { + if (!strcmp(s->gps_location_socket, "gpsLocationExternal")) { + auto gpsLocation = sm[s->gps_location_socket].getGpsLocationExternal(); + ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD); + } + else { + auto gpsLocation = sm[s->gps_location_socket].getGpsLocation(); + ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD); + + } + } /* auto locationd = sm["liveLocationKalman"].getLiveLocationKalman(); bool is_gps_valid = locationd.getGpsOK(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 52a9da3..484feb5 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -457,8 +457,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { {tr("Device"), device}, {tr("Network"), networking}, {tr("Toggles"), toggles}, - {tr("Software"), new SoftwarePanel(this)}, - {tr("Firehose"), new FirehosePanel(this)}, + //{tr("Software"), new SoftwarePanel(this)}, + //{tr("Firehose"), new FirehosePanel(this)}, {tr("Carrot"), new CarrotPanel(this)}, {tr("Developer"), new DeveloperPanel(this)}, }; @@ -655,6 +655,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(200)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5)); latLongToggles->addItem(new CValueControl("LongTuningKf", "LONG: FF Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); latLongToggles->addItem(new CValueControl("LongActuatorDelay", "LONG: ActuatorDelay(20)", "", "../assets/offroad/icon_logic.png", 0, 200, 5)); + latLongToggles->addItem(new CValueControl("LongVelocityControl", "LONG: VelocityControl(0)", "", "../assets/offroad/icon_logic.png", 0, 1, 1)); latLongToggles->addItem(new CValueControl("VEgoStopping", "LONG: VEgoStopping(50)", "Stopping factor", "../assets/offroad/icon_logic.png", 1, 100, 5)); latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(10)", "", "../assets/offroad/icon_logic.png", 0, 200, 10)); //latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10)); @@ -664,7 +665,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10)); //latLongToggles->addItem(new CValueControl("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10)); latLongToggles->addItem(new CValueControl("ComfortBrake", "LONG: Comfort Brake (240)", "x0.01", "../assets/offroad/icon_logic.png", 200, 300, 1)); - latLongToggles->addItem(new CValueControl("JerkLeadFactor", "LONG: Jerk Lead Factor (0)", "x0.01", "../assets/offroad/icon_logic.png", 0, 100, 5)); + latLongToggles->addItem(new CValueControl("JLeadFactor2", "LONG: Jerk Lead Factor (0)", "x0.01", "../assets/offroad/icon_logic.png", 10, 100, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals1", "ACCEL:0km/h(160)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals2", "ACCEL:40km/h(120)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); latLongToggles->addItem(new CValueControl("CruiseMaxVals3", "ACCEL:60km/h(100)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5)); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 773839d..d9ff4bd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -98,13 +98,20 @@ void UIState::updateStatus() { } UIState::UIState(QObject *parent) : QObject(parent) { + Params params; + if (params.getBool("UbloxAvailable")) { + strcpy(gps_location_socket, "gpsLocationExternal"); + } + else { + strcpy(gps_location_socket, "gpsLocation"); + } sm = std::make_unique(std::vector{ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "longitudinalPlan", "carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters", - "navRoute", "navInstruction", "navInstructionCarrot", + "navRoute", "navInstruction", "navInstructionCarrot", gps_location_socket, }); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 884e6bd..b944da7 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -98,6 +98,8 @@ public: float show_brightness_ratio = 1.0; int show_brightness_timer = 20; + char gps_location_socket[32] = "gpsLocationExternal"; + signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); diff --git a/system/logmessaged.py b/system/logmessaged.py index c095c26..cd00850 100755 --- a/system/logmessaged.py +++ b/system/logmessaged.py @@ -6,6 +6,7 @@ import cereal.messaging as messaging from openpilot.common.logging_extra import SwagLogFileFormatter from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import get_file_handler +from openpilot.common.params import Params def main() -> NoReturn: @@ -25,7 +26,16 @@ def main() -> NoReturn: while True: dat = b''.join(sock.recv_multipart()) level = dat[0] - record = dat[1:].decode("utf-8") + raw_bytes = dat[1:] + + try: + record = dat[1:].decode("utf-8", errors="replace") + except Exception as e: + print(f"decode error: {e}, skipping log") + print(f"Raw bytes (hex): {raw_bytes.hex()[:200]}...") # 앞부분만 출력 + Params().put_bool("CarrotException", True) + continue + if level >= log_level: log_handler.emit(record) diff --git a/system/manager/manager.py b/system/manager/manager.py index 908cf50..40b0119 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -84,7 +84,7 @@ def get_default_params(): ("StoppingAccel", "0"), ("StopDistanceCarrot", "550"), ("ComfortBrake", "240"), - ("JLeadFactor", "0"), + ("JLeadFactor2", "100"), ("CruiseButtonMode", "2"), ("CruiseButtonTest1", "8"), ("CruiseButtonTest2", "30"), @@ -104,6 +104,7 @@ def get_default_params(): ("LongTuningKiV", "0"), ("LongTuningKf", "100"), ("LongActuatorDelay", "20"), + ("LongVelocityControl", "0"), ("VEgoStopping", "50"), ("RadarReactionFactor", "10"), ("EnableRadarTracks", "0"), diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 81f1dca..25dce84 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -108,7 +108,7 @@ procs = [ PythonProcess("navd", "selfdrive.navd.navd", only_onroad), PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), - PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), + #PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),