mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 00:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Test and fix cruise speed all personalities (#28658)

Browse Source 
* Fix cruise speed in non-standard mode

* Test all personalities

* Test only critical speeds

* 35ms max
old-commit-hash: ae3681f2bb
This commit is contained in:
Harald Schäfer
2023-06-23 17:14:41 -07:00
committed by GitHub
parent 826706dd30
commit 075dc34240
3 changed files with 33 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -357,7 +357,7 @@ class LongitudinalMpc:
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_lower,
v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW())
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW(personality))
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]

22
selfdrive/controls/tests/test_cruise_speed.py
View File

@@ -1,8 +1,10 @@
#!/usr/bin/env python3
import numpy as np
from parameterized import parameterized_class
import unittest
from parameterized import parameterized_class
from cereal import log
from common.params import Params
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
@@ -31,13 +33,19 @@ def run_cruise_simulation(cruise, e2e, t_end=20.):
class TestCruiseSpeed(unittest.TestCase):
def test_cruise_speed(self):
for e2e in [False, True]:
for speed in np.arange(5, 40, 5):
print(f'Testing {speed} m/s')
cruise_speed = float(speed)
params = Params()
personalities = [log.LongitudinalPersonality.relaxed,
log.LongitudinalPersonality.standard,
log.LongitudinalPersonality.aggressive]
for personality in personalities:
params.put("LongitudinalPersonality", str(personality))
for e2e in [False, True]:
for speed in [5,35]:
print(f'Testing {speed} m/s')
cruise_speed = float(speed)
simulation_steady_state = run_cruise_simulation(cruise_speed, e2e)
self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
simulation_steady_state = run_cruise_simulation(cruise_speed, e2e)
self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
# TODO: test pcmCruise

27
selfdrive/controls/tests/test_following_distance.py
View File

@@ -1,8 +1,9 @@
#!/usr/bin/env python3
import unittest
import numpy as np
from common.params import Params
from cereal import log
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
@@ -24,14 +25,20 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
class TestFollowingDistance(unittest.TestCase):
def test_following_distance(self):
for e2e in [False, True]:
for speed in np.arange(0, 40, 5):
print(f'Testing {speed} m/s')
v_lead = float(speed)
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e)
correct_steady_state = desired_follow_distance(v_lead, v_lead)
err_ratio = 0.2 if e2e else 0.1
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
params = Params()
personalities = [log.LongitudinalPersonality.relaxed,
log.LongitudinalPersonality.standard,
log.LongitudinalPersonality.aggressive]
for personality in personalities:
params.put("LongitudinalPersonality", str(personality))
for e2e in [False, True]:
for speed in [0,10,35]:
print(f'Testing {speed} m/s')
v_lead = float(speed)
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e)
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(personality))
err_ratio = 0.2 if e2e else 0.1
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
if __name__ == "__main__":