sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

controlsd: higher default set speed in experimental mode (#27279)

Browse Source 
* controlsd: higher default set speed in experimental mode

* update tests

* update tests
inactive-accel-fix
Adeeb Shihadeh 3 years ago committed by GitHub
parent 7978901101
commit 5ab2d35f7b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 33 additions and 26 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      selfdrive/controls/controlsd.py
  2. 26
      selfdrive/controls/lib/drive_helpers.py
  3. 27
      selfdrive/controls/tests/test_cruise_speed.py

6
selfdrive/controls/controlsd.py
Unescape View File

@ -183,6 +183,7 @@ class Controls:
self.steer_limited = False self.steer_limited = False
self.desired_curvature = 0.0 self.desired_curvature = 0.0
self.desired_curvature_rate = 0.0 self.desired_curvature_rate = 0.0
self.experimental_mode = False
self.v_cruise_helper = VCruiseHelper(self.CP) self.v_cruise_helper = VCruiseHelper(self.CP)
# TODO: no longer necessary, aside from process replay # TODO: no longer necessary, aside from process replay
@ -544,7 +545,7 @@ class Controls:
else: else:
self.state = State.enabled self.state = State.enabled
self.current_alert_types.append(ET.ENABLE) self.current_alert_types.append(ET.ENABLE)
self.v_cruise_helper.initialize_v_cruise(CS) self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
# Check if openpilot is engaged and actuators are enabled # Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES self.enabled = self.state in ENABLED_STATES
@ -787,7 +788,7 @@ class Controls:
controlsState.startMonoTime = int(start_time * 1e9) controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel) controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter
controlsState.experimentalMode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl controlsState.experimentalMode = self.experimental_mode
lat_tuning = self.CP.lateralTuning.which() lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode: if self.joystick_mode:
@ -838,6 +839,7 @@ class Controls:
self.prof.checkpoint("Ratekeeper", ignore=True) self.prof.checkpoint("Ratekeeper", ignore=True)
self.is_metric = self.params.get_bool("IsMetric") self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
# Sample data from sockets and get a carState # Sample data from sockets and get a carState
CS = self.data_sample() CS = self.data_sample()

26
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -8,10 +8,12 @@ from selfdrive.modeld.constants import T_IDXS
# WARNING: this value was determined based on the model's training distribution, # WARNING: this value was determined based on the model's training distribution,
# model predictions above this speed can be unpredictable # model predictions above this speed can be unpredictable
V_CRUISE_MAX = 145 # kph # V_CRUISE's are in kph
V_CRUISE_MIN = 8 # kph V_CRUISE_MIN = 8
V_CRUISE_ENABLE_MIN = 40 # kph V_CRUISE_MAX = 145
V_CRUISE_INITIAL = 255 # kph V_CRUISE_UNSET = 255
V_CRUISE_INITIAL = 40
V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
MIN_SPEED = 1.0 MIN_SPEED = 1.0
@ -39,15 +41,15 @@ CRUISE_INTERVAL_SIGN = {
class VCruiseHelper: class VCruiseHelper:
def __init__(self, CP): def __init__(self, CP):
self.CP = CP self.CP = CP
self.v_cruise_kph = V_CRUISE_INITIAL self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_INITIAL self.v_cruise_cluster_kph = V_CRUISE_UNSET
self.v_cruise_kph_last = 0 self.v_cruise_kph_last = 0
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 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} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
@property @property
def v_cruise_initialized(self): def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_INITIAL return self.v_cruise_kph != V_CRUISE_UNSET
def update_v_cruise(self, CS, enabled, is_metric): def update_v_cruise(self, CS, enabled, is_metric):
self.v_cruise_kph_last = self.v_cruise_kph self.v_cruise_kph_last = self.v_cruise_kph
@ -62,8 +64,8 @@ class VCruiseHelper:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
else: else:
self.v_cruise_kph = V_CRUISE_INITIAL self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_INITIAL self.v_cruise_cluster_kph = V_CRUISE_UNSET
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric): 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 # handle button presses. TODO: this should be in state_control, but a decelCruise press
@ -125,16 +127,18 @@ class VCruiseHelper:
self.button_timers[b.type.raw] = 1 if b.pressed else 0 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} self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
def initialize_v_cruise(self, CS): def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
# initializing is handled by the PCM # initializing is handled by the PCM
if self.CP.pcmCruise: if self.CP.pcmCruise:
return 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 # 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: 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 self.v_cruise_kph = self.v_cruise_kph_last
else: else:
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) 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 self.v_cruise_cluster_kph = self.v_cruise_kph

27
selfdrive/controls/tests/test_cruise_speed.py
Unescape View File

@ -3,7 +3,7 @@ import numpy as np
from parameterized import parameterized_class from parameterized import parameterized_class
import unittest import unittest
from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
@ -53,16 +53,16 @@ class TestVCruiseHelper(unittest.TestCase):
for _ in range(2): for _ in range(2):
self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
def enable(self, v_ego): def enable(self, v_ego, experimental_mode):
# Simulates user pressing set with a current speed # Simulates user pressing set with a current speed
self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego)) self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
def test_adjust_speed(self): def test_adjust_speed(self):
""" """
Asserts speed changes on falling edges of buttons. Asserts speed changes on falling edges of buttons.
""" """
self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
for btn in (ButtonType.accelCruise, ButtonType.decelCruise): for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
for pressed in (True, False): for pressed in (True, False):
@ -86,7 +86,7 @@ class TestVCruiseHelper(unittest.TestCase):
CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
if pressed: if pressed:
self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
# Expected diff on enabling. Speed should not change on falling edge of pressed # Expected diff on enabling. Speed should not change on falling edge of pressed
self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
@ -96,7 +96,7 @@ class TestVCruiseHelper(unittest.TestCase):
Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
""" """
self.enable(0) self.enable(0, False)
for standstill in (True, False): for standstill in (True, False):
for pressed in (True, False): for pressed in (True, False):
@ -116,7 +116,7 @@ class TestVCruiseHelper(unittest.TestCase):
for v_ego in np.linspace(0, 100, 101): for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state() self.reset_cruise_speed_state()
self.enable(V_CRUISE_ENABLE_MIN * CV.KPH_TO_MS) self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
# first decrement speed, then perform gas pressed logic # first decrement speed, then perform gas pressed logic
expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@ -137,13 +137,14 @@ class TestVCruiseHelper(unittest.TestCase):
Asserts allowed cruise speeds on enabling with SET. Asserts allowed cruise speeds on enabling with SET.
""" """
for v_ego in np.linspace(0, 100, 101): for experimental_mode in (True, False):
self.reset_cruise_speed_state() for v_ego in np.linspace(0, 100, 101):
self.assertFalse(self.v_cruise_helper.v_cruise_initialized) self.reset_cruise_speed_state()
self.assertFalse(self.v_cruise_helper.v_cruise_initialized)
self.enable(float(v_ego)) self.enable(float(v_ego), experimental_mode)
self.assertTrue(V_CRUISE_ENABLE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX) self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
self.assertTrue(self.v_cruise_helper.v_cruise_initialized) self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
if __name__ == "__main__": if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save