mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-17 02:42:10 +08:00
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
This commit is contained in:
Adeeb Shihadeh
2023-02-09 19:27:51 -08:00
committed by GitHub
parent 7978901101
commit 5ab2d35f7b
3 changed files with 33 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+4 -2
View File
@@ -183,6 +183,7 @@ class Controls:
self.steer_limited = False
self.desired_curvature = 0.0
self.desired_curvature_rate = 0.0
self.experimental_mode = False
self.v_cruise_helper = VCruiseHelper(self.CP)
# TODO: no longer necessary, aside from process replay
@@ -544,7 +545,7 @@ class Controls:
else:
self.state = State.enabled
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
self.enabled = self.state in ENABLED_STATES
@@ -787,7 +788,7 @@ class Controls:
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
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()
if self.joystick_mode:
@@ -838,6 +839,7 @@ class Controls:
self.prof.checkpoint("Ratekeeper", ignore=True)
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
CS = self.data_sample()
selfdrive/controls/lib/drive_helpers.py
+15 -11
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,
# model predictions above this speed can be unpredictable
V_CRUISE_MAX = 145 # kph
V_CRUISE_MIN = 8 # kph
V_CRUISE_ENABLE_MIN = 40 # kph
V_CRUISE_INITIAL = 255 # kph
# 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 = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
MIN_SPEED = 1.0
@@ -39,15 +41,15 @@ CRUISE_INTERVAL_SIGN = {
class VCruiseHelper:
def __init__(self, CP):
self.CP = CP
self.v_cruise_kph = V_CRUISE_INITIAL
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
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_INITIAL
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
@@ -62,8 +64,8 @@ class VCruiseHelper:
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_INITIAL
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
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
@@ -125,16 +127,18 @@ class VCruiseHelper:
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):
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, 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
selfdrive/controls/tests/test_cruise_speed.py
+14 -13
View File
@@ -3,7 +3,7 @@ import numpy as np
from parameterized import parameterized_class
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 common.conversions import Conversions as CV
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
@@ -53,16 +53,16 @@ class TestVCruiseHelper(unittest.TestCase):
for _ in range(2):
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
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):
"""
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 pressed in (True, False):
@@ -86,7 +86,7 @@ class TestVCruiseHelper(unittest.TestCase):
CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
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
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.
"""
self.enable(0)
self.enable(0, False)
for standstill 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):
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
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.
"""
for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state()
self.assertFalse(self.v_cruise_helper.v_cruise_initialized)
for experimental_mode in (True, False):
for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state()
self.assertFalse(self.v_cruise_helper.v_cruise_initialized)
self.enable(float(v_ego))
self.assertTrue(V_CRUISE_ENABLE_MIN <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
self.enable(float(v_ego), experimental_mode)
self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
if __name__ == "__main__":