diff --git a/cereal/car.capnp b/cereal/car.capnp index aa0f336ffe..9f72e31637 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -162,10 +162,13 @@ struct CarState { canErrorCounter @48 :UInt32; # car speed - vEgo @1 :Float32; # best estimate of speed - aEgo @16 :Float32; # best estimate of acceleration - vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors - vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI + vEgo @1 :Float32; # best estimate of speed + aEgo @16 :Float32; # best estimate of aCAN cceleration + vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors + vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI + + vCruise @53 :Float32; # actual set speed + vCruiseCluster @54 :Float32; # set speed to display in the UI yawRate @22 :Float32; # best estimate of yaw rate standstill @18 :Bool; diff --git a/cereal/log.capnp b/cereal/log.capnp index 0d047628aa..c7d8ffcd85 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -733,8 +733,6 @@ struct ControlsState @0x97ff69c53601abf1 { longControlState @30 :Car.CarControl.Actuators.LongControlState; vTargetLead @3 :Float32; - vCruise @22 :Float32; # actual set speed - vCruiseCluster @63 :Float32; # set speed to display in the UI upAccelCmd @4 :Float32; uiAccelCmd @5 :Float32; ufAccelCmd @33 :Float32; @@ -882,6 +880,8 @@ struct ControlsState @0x97ff69c53601abf1 { canErrorCounterDEPRECATED @57 :UInt32; vPidDEPRECATED @2 :Float32; alertBlinkingRateDEPRECATED @42 :Float32; + vCruiseDEPRECATED @22 :Float32; # actual set speed + vCruiseClusterDEPRECATED @63 :Float32; # set speed to display in the UI } struct DrivingModelData { diff --git a/common/params.cc b/common/params.cc index 9e30802820..098e1de58e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -180,7 +180,6 @@ std::unordered_map keys = { {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet - {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"RouteCount", PERSISTENT}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"SshEnabled", PERSISTENT}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8b44daa4fc..57b41a5cf5 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import time +import threading import cereal.messaging as messaging @@ -18,9 +19,10 @@ from opendbc.car.fw_versions import ObdCallback from opendbc.car.car_helpers import get_car from opendbc.car.interfaces import CarInterfaceBase from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp +from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState from openpilot.selfdrive.car.helpers import convert_carControl, convert_to_capnp -from openpilot.selfdrive.controls.lib.events import Events +from openpilot.selfdrive.controls.lib.events import Events, ET REPLAY = "REPLAY" in os.environ @@ -139,6 +141,10 @@ class Car: self.car_events = CarSpecificEvents(self.CP) self.mock_carstate = MockCarState() + self.v_cruise_helper = VCruiseHelper(self.CP) + + self.is_metric = self.params.get_bool("IsMetric") + self.experimental_mode = self.params.get_bool("ExperimentalMode") # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -164,6 +170,11 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + # TODO: mirror the carState.cruiseState struct? + self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) + CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) + CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) + return CS def update_events(self, CS: car.CarState): @@ -234,6 +245,9 @@ class Car: self.update_events(CS) + if not self.sm['carControl'].enabled and self.events.contains(ET.ENABLE): + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) + self.state_publish(CS) initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and @@ -244,10 +258,23 @@ class Car: self.initialized_prev = initialized self.CS_prev = CS.as_reader() + def params_thread(self, evt): + while not evt.is_set(): + self.is_metric = self.params.get_bool("IsMetric") + self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + time.sleep(0.1) + def card_thread(self): - while True: - self.step() - self.rk.monitor_time() + e = threading.Event() + t = threading.Thread(target=self.params_thread, args=(e, )) + try: + t.start() + while True: + self.step() + self.rk.monitor_time() + finally: + e.set() + t.join() def main(): diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py new file mode 100644 index 0000000000..e2d83033d6 --- /dev/null +++ b/selfdrive/car/cruise.py @@ -0,0 +1,132 @@ +import math + +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import clip + + +# WARNING: this value was determined based on the model's training distribution, +# model predictions above this speed can be unpredictable +# V_CRUISE's are in kph +V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 +V_CRUISE_INITIAL = 40 +V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 +IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1) # round here to avoid rounding errors incrementing set speed + +ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type +CRUISE_LONG_PRESS = 50 +CRUISE_NEAREST_FUNC = { + ButtonType.accelCruise: math.ceil, + ButtonType.decelCruise: math.floor, +} +CRUISE_INTERVAL_SIGN = { + ButtonType.accelCruise: +1, + ButtonType.decelCruise: -1, +} + + +class VCruiseHelper: + def __init__(self, CP): + self.CP = CP + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET + self.v_cruise_kph_last = 0 + self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} + self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} + + @property + def v_cruise_initialized(self): + return self.v_cruise_kph != V_CRUISE_UNSET + + def update_v_cruise(self, CS, enabled, is_metric): + self.v_cruise_kph_last = self.v_cruise_kph + + if CS.cruiseState.available: + if not self.CP.pcmCruise: + # if stock cruise is completely disabled, then we can use our own set speed logic + self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph + self.update_button_timers(CS, enabled) + else: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + else: + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET + + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): + # handle button presses. TODO: this should be in state_control, but a decelCruise press + # would have the effect of both enabling and changing speed is checked after the state transition + if not enabled: + return + + long_press = False + button_type = None + + v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers and not b.pressed: + if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: + return # end long press + button_type = b.type.raw + break + else: + for k, timer in self.button_timers.items(): + if timer and timer % CRUISE_LONG_PRESS == 0: + button_type = k + long_press = True + break + + if button_type is None: + return + + # Don't adjust speed when pressing resume to exit standstill + cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill + if button_type == ButtonType.accelCruise and cruise_standstill: + return + + # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) + if not self.button_change_states[button_type]["enabled"]: + return + + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval + self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta + else: + self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + + # If set is pressed while overriding, clip cruise speed to minimum of vEgo + if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): + self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) + + self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + + def update_button_timers(self, CS, enabled): + # increment timer for buttons still pressed + for k in self.button_timers: + if self.button_timers[k] > 0: + self.button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers: + # Start/end timer and store current state on change of button pressed + self.button_timers[b.type.raw] = 1 if b.pressed else 0 + self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} + + def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + # initializing is handled by the PCM + if self.CP.pcmCruise: + return + + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + + if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: + self.v_cruise_kph = self.v_cruise_kph_last + else: + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + + self.v_cruise_cluster_kph = self.v_cruise_kph diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py similarity index 97% rename from selfdrive/controls/tests/test_cruise_speed.py rename to selfdrive/car/tests/test_cruise_speed.py index 8f451a49bc..7bda3a24eb 100644 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -4,7 +4,7 @@ import numpy as np from parameterized import parameterized_class from cereal import log -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT +from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6ea4d2096c..9ee2f6309f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,7 @@ from openpilot.common.gps import get_gps_location_service from opendbc.car.car_helpers import get_car_interface from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_startup_event +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_startup_event from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -157,7 +157,6 @@ class Controls: self.desired_curvature = 0.0 self.experimental_mode = False self.personality = self.read_personality_param() - self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False self.can_log_mono_time = 0 @@ -179,13 +178,7 @@ class Controls: self.rk = Ratekeeper(100, print_delay_threshold=None) def set_initial_state(self): - if REPLAY: - controls_state = self.params.get("ReplayControlsState") - if controls_state is not None: - with log.ControlsState.from_bytes(controls_state) as controls_state: - self.v_cruise_helper.v_cruise_kph = controls_state.vCruise - - if any(ps.controlsAllowed for ps in self.sm['pandaStates']): + if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled def update_events(self, CS): @@ -214,7 +207,7 @@ class Controls: # Block resume if cruise never previously enabled resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: + if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: self.events.add(EventName.resumeBlocked) if not self.CP.notCar: @@ -447,8 +440,6 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) - # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state self.soft_disable_timer = max(0, self.soft_disable_timer - 1) @@ -522,7 +513,6 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -578,7 +568,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) # Steering PID loop and lateral MPC @@ -678,7 +668,7 @@ class Controls: CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS) hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -760,8 +750,6 @@ class Controls: controlsState.state = self.state controlsState.engageable = not self.events.contains(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state - controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) - controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 777e0a1f7f..35810c4832 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,23 +1,10 @@ -import math - from cereal import car, log -from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL from openpilot.system.version import get_build_metadata EventName = car.OnroadEvent.EventName -# WARNING: this value was determined based on the model's training distribution, -# model predictions above this speed can be unpredictable -# V_CRUISE's are in kph -V_CRUISE_MIN = 8 -V_CRUISE_MAX = 145 -V_CRUISE_UNSET = 255 -V_CRUISE_INITIAL = 40 -V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 -IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1) # round here to avoid rounding errors incrementing set speed - MIN_SPEED = 1.0 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 @@ -26,124 +13,6 @@ CAR_ROTATION_RADIUS = 0.0 MAX_LATERAL_JERK = 5.0 MAX_VEL_ERR = 5.0 -ButtonEvent = car.CarState.ButtonEvent -ButtonType = car.CarState.ButtonEvent.Type -CRUISE_LONG_PRESS = 50 -CRUISE_NEAREST_FUNC = { - ButtonType.accelCruise: math.ceil, - ButtonType.decelCruise: math.floor, -} -CRUISE_INTERVAL_SIGN = { - ButtonType.accelCruise: +1, - ButtonType.decelCruise: -1, -} - - -class VCruiseHelper: - def __init__(self, CP): - self.CP = CP - self.v_cruise_kph = V_CRUISE_UNSET - self.v_cruise_cluster_kph = V_CRUISE_UNSET - self.v_cruise_kph_last = 0 - self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} - self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} - - @property - def v_cruise_initialized(self): - return self.v_cruise_kph != V_CRUISE_UNSET - - def update_v_cruise(self, CS, enabled, is_metric): - self.v_cruise_kph_last = self.v_cruise_kph - - if CS.cruiseState.available: - if not self.CP.pcmCruise: - # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric) - self.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS, enabled) - else: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - else: - self.v_cruise_kph = V_CRUISE_UNSET - self.v_cruise_cluster_kph = V_CRUISE_UNSET - - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): - # handle button presses. TODO: this should be in state_control, but a decelCruise press - # would have the effect of both enabling and changing speed is checked after the state transition - if not enabled: - return - - long_press = False - button_type = None - - v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT - - for b in CS.buttonEvents: - if b.type.raw in self.button_timers and not b.pressed: - if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS: - return # end long press - button_type = b.type.raw - break - else: - for k, timer in self.button_timers.items(): - if timer and timer % CRUISE_LONG_PRESS == 0: - button_type = k - long_press = True - break - - if button_type is None: - return - - # Don't adjust speed when pressing resume to exit standstill - cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill - if button_type == ButtonType.accelCruise and cruise_standstill: - return - - # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge) - if not self.button_change_states[button_type]["enabled"]: - return - - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) - if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval - self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta - else: - self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] - - # If set is pressed while overriding, clip cruise speed to minimum of vEgo - if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): - self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) - - self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) - - def update_button_timers(self, CS, enabled): - # increment timer for buttons still pressed - for k in self.button_timers: - if self.button_timers[k] > 0: - self.button_timers[k] += 1 - - for b in CS.buttonEvents: - if b.type.raw in self.button_timers: - # Start/end timer and store current state on change of button pressed - self.button_timers[b.type.raw] = 1 if b.pressed else 0 - self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: - # initializing is handled by the PCM - if self.CP.pcmCruise: - return - - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL - - # 250kph or above probably means we never had a set speed - if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: - self.v_cruise_kph = self.v_cruise_kph_last - else: - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) - - self.v_cruise_cluster_kph = self.v_cruise_kph - - def clip_curvature(v_ego, prev_curvature, new_curvature): v_ego = max(MIN_SPEED, v_ego) max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 528ecdec68..2513cc81f5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,7 +12,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s @@ -98,7 +99,7 @@ class LongitudinalPlanner: self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo - v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX) + v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_off = sm['controlsState'].longControlState == LongCtrlState.off diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index a1eedc915b..ef97a009b0 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -111,12 +111,12 @@ class Plant: model.modelV2.acceleration = acceleration control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off - control.controlsState.vCruise = float(v_cruise * 3.6) control.controlsState.experimentalMode = self.e2e control.controlsState.personality = self.personality control.controlsState.forceDecel = self.force_decel car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 + car_state.carState.vCruise = float(v_cruise * 3.6) # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index c1f60959e2..37ca097997 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -15,7 +15,7 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, msgs = migrate_gpsLocation(msgs) msgs = migrate_deviceState(msgs) msgs = migrate_carOutput(msgs) - msgs = migrate_selfdriveState(msgs) + msgs = migrate_controlsState(msgs) if manager_states: msgs = migrate_managerState(msgs) if panda_states: @@ -27,10 +27,13 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, return msgs -def migrate_selfdriveState(lr): +def migrate_controlsState(lr): ret = [] + last_cs = None for msg in lr: if msg.which() == 'controlsState': + last_cs = msg + m = messaging.new_message('selfdriveState') m.valid = msg.valid m.logMonoTime = msg.logMonoTime @@ -40,6 +43,13 @@ def migrate_selfdriveState(lr): "personality"): setattr(ss, field, getattr(msg.controlsState, field)) ret.append(m.as_reader()) + elif msg.which() == 'carState' and last_cs is not None: + if last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruise > 0.1: + msg = msg.as_builder() + msg.carState.vCruise = last_cs.controlsState.vCruiseDEPRECATED + msg.carState.vCruiseCluster = last_cs.controlsState.vCruiseClusterDEPRECATED + msg = msg.as_reader() + ret.append(msg) return ret diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8e1e769e71..5d0c723428 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -453,19 +453,6 @@ class FrequencyBasedRcvCallback: def controlsd_config_callback(params, cfg, lr): - controlsState = None - initialized = False - for msg in lr: - if msg.which() == "controlsState": - controlsState = msg.controlsState - if initialized: - break - elif msg.which() == "onroadEvents": - initialized = car.OnroadEvent.EventName.controlsInitializing not in [e.name for e in msg.onroadEvents] - - assert controlsState is not None and initialized, "controlsState never initialized" - params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) - ublox = params.get_bool("UbloxAvailable") sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5ed9bddaed..a1a343a50c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ab4983de4477d648e5c36447debfd6735dfc466f \ No newline at end of file +073dcca6932c5c66cdadf9aee9b531b623795888 \ No newline at end of file diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 4a95006838..cc9940f275 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -32,7 +32,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { is_metric = s.scene.is_metric; // Handle older routes where vCruiseCluster is not set - float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster(); + float v_cruise = car_state.getVCruiseCluster() == 0.0 ? cs.getVCruiseDEPRECATED() : car_state.getVCruiseCluster(); setSpeed = cs_alive ? v_cruise : SET_SPEED_NA; is_cruise_set = setSpeed > 0 && (int)setSpeed != SET_SPEED_NA; if (is_cruise_set && !is_metric) { diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index e9240fa4f7..1a075b3c44 100644 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -19,7 +19,7 @@ from openpilot.common.prefix import OpenpilotPrefix from openpilot.common.transformations.camera import CameraConfig, DEVICE_CAMERAS from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.selfdrive.test.helpers import with_processes -from openpilot.selfdrive.test.process_replay.migration import migrate_selfdriveState +from openpilot.selfdrive.test.process_replay.migration import migrate_controlsState from openpilot.tools.lib.logreader import LogReader from openpilot.tools.lib.framereader import FrameReader from openpilot.tools.lib.route import Route @@ -224,7 +224,7 @@ def create_screenshots(): segnum = 2 lr = LogReader(route.qlog_paths()[segnum]) DATA['carParams'] = next((event.as_builder() for event in lr if event.which() == 'carParams'), None) - for event in migrate_selfdriveState(lr): + for event in migrate_controlsState(lr): if event.which() in DATA: DATA[event.which()] = event.as_builder()