mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-12 07:23:45 +08:00
Fix Ioniq Force Stop placement and smooth final braking
This commit is contained in:
firestar5683
Binary file not shown.
Binary file not shown.
@@ -60,7 +60,11 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa
|
||||
elif starting or state.launch_active or (state.long_control_state_last == LongState.starting and control_state == LongState.pid):
|
||||
state.launch_active = True
|
||||
|
||||
hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE if state.stopping else 0.0
|
||||
upper_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (2.0, 3.0, 2.0))) * RESPONSE if control_state == LongState.pid else MIN_JERK
|
||||
if state.stopping and accel > state.accel_last:
|
||||
# Follow an already gentler planner request without weakening stronger braking.
|
||||
upper_limit = hold_jerk
|
||||
lower_limit = float(np.interp(speed, (0.0, 5.0, 20.0), (5.0, 3.5, 3.0))) * RESPONSE
|
||||
horizon = float(np.interp(speed, LOOKAHEAD_SPEEDS, LOOKAHEAD_TIMES))
|
||||
desired_jerk = float(np.clip((accel - state.accel_last) / horizon, -MAX_JERK, MAX_JERK))
|
||||
@@ -70,10 +74,10 @@ def update_calibration(state: Ioniq6LongitudinalState, accel: float, speed: floa
|
||||
state.jerk_lower = min(lower_jerk, lower_limit)
|
||||
|
||||
if state.stopping:
|
||||
if speed <= 2.0:
|
||||
# A saturated braking request must survive the comfort soft-stop cap.
|
||||
if speed <= 2.0 and (speed <= .01 or accel > CarControllerParams.ACCEL_MIN):
|
||||
brake_cap = float(np.interp(speed, (0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0), (-0.15, -0.16, -0.22, -0.42, -0.78, -1.15, -1.40)))
|
||||
state.desired_accel = min(0.0, max(accel, brake_cap))
|
||||
hold_jerk = float(np.interp(speed, (0.0, 0.15, 0.6, 1.2, 2.0, 3.0), (0.35, 0.40, 0.48, 0.65, 0.85, 1.10))) * RESPONSE
|
||||
state.jerk_upper = min(state.jerk_upper, hold_jerk)
|
||||
else:
|
||||
state.desired_accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, 0.0))
|
||||
|
||||
@@ -61,6 +61,36 @@ class TestIoniq6Longitudinal(unittest.TestCase):
|
||||
for value, previous in zip(actual, expected, strict=True):
|
||||
self.assertAlmostEqual(value, previous, places=12)
|
||||
|
||||
def test_saturated_stopping_brake_survives_low_speed_comfort_cap(self):
|
||||
for speed in (.1, .5, 1., 2.):
|
||||
with self.subTest(speed=speed):
|
||||
policy = Ioniq6LongitudinalPolicy()
|
||||
last_sent = 0.
|
||||
for frame in range(600):
|
||||
result = policy.update(frame, active=True, override=False, accel=-3.5, speed=speed,
|
||||
measured_accel=last_sent, control_state=LongState.stopping,
|
||||
last_sent_accel=last_sent)
|
||||
self.assertTrue(result.stopping)
|
||||
self.assertGreaterEqual(result.accel, -3.5)
|
||||
if frame % 2 == 0:
|
||||
self.assertLessEqual(result.accel, last_sent)
|
||||
self.assertGreaterEqual(result.accel + 1e-12, last_sent - .36)
|
||||
last_sent = result.accel
|
||||
self.assertAlmostEqual(last_sent, -3.5)
|
||||
refused = policy.update(601, active=True, override=True, accel=-3.5, speed=speed,
|
||||
measured_accel=last_sent, control_state=LongState.stopping,
|
||||
last_sent_accel=last_sent)
|
||||
self.assertEqual(refused.accel, 0.)
|
||||
self.assertFalse(refused.stopping)
|
||||
self.assertEqual(policy.state, Ioniq6LongitudinalState())
|
||||
|
||||
def test_saturated_request_returns_to_soft_hold_at_standstill(self):
|
||||
state = Ioniq6LongitudinalState(accel_last=-3.5)
|
||||
update_calibration(state, -3.5, 0., 0., LongState.stopping)
|
||||
self.assertAlmostEqual(state.desired_accel, -.15)
|
||||
self.assertTrue(state.stopping)
|
||||
self.assertGreater(state.actual_accel, -3.5)
|
||||
|
||||
def test_soft_target_revokes_launch_on_offcycle_and_scheduled_ticks(self):
|
||||
for drop_frame in (31, 35):
|
||||
with self.subTest(drop_frame=drop_frame):
|
||||
@@ -323,6 +353,20 @@ class TestIoniq6Longitudinal(unittest.TestCase):
|
||||
counters.append(frame[1][2])
|
||||
self.assertEqual(counters, [counter & 0xff for counter in range(260)])
|
||||
|
||||
def test_already_gentler_request_uses_bounded_stop_release(self):
|
||||
state = Ioniq6LongitudinalState(accel_last=-1.5)
|
||||
update_calibration(state, -.5, 3., -1.5, LongState.stopping)
|
||||
self.assertGreater(state.actual_accel, -1.47)
|
||||
self.assertLessEqual(state.actual_accel, -.5)
|
||||
self.assertLessEqual(state.jerk_upper, 1.32)
|
||||
self.assertTrue(state.stopping)
|
||||
|
||||
def test_stop_release_does_not_weaken_independent_braking_above_soft_cap(self):
|
||||
state = Ioniq6LongitudinalState(accel_last=-3.2)
|
||||
update_calibration(state, -3.2, 2.5, -3.2, LongState.stopping)
|
||||
self.assertEqual(state.desired_accel, -3.2)
|
||||
self.assertEqual(state.actual_accel, -3.2)
|
||||
|
||||
def test_stock_ioniq_and_other_cars_do_not_install_this_policy(self):
|
||||
for car in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5, CAR.KIA_EV6):
|
||||
cp = CarInterface.get_non_essential_params(car)
|
||||
|
||||
@@ -277,6 +277,8 @@ struct StarPilotLongitudinalPlan @0xda96579883444c35 {
|
||||
modelMonoTime @2 :UInt64;
|
||||
driveStartMonoTime @3 :UInt64;
|
||||
forceStopHolding @4 :Bool;
|
||||
forceStopBraking @5 :Bool;
|
||||
forceStopEpisodeMonoTime @6 :UInt64;
|
||||
}
|
||||
|
||||
struct SlcState @0xa1680744031fdb2d {
|
||||
@@ -1577,3 +1579,4 @@ struct LiveTrackingResponseWitness @0xdaa1a9f0d8968e31 {
|
||||
windowStartMonoTime @4 :UInt64; independentPoses @5 :UInt16;
|
||||
residualRmse @6 :Float64; validUntilMonoTime @7 :UInt64;
|
||||
}
|
||||
|
||||
|
||||
@@ -3737,17 +3737,17 @@ const ::capnp::_::RawSchema s_de92e382e2400293 = {
|
||||
0, 17, i_de92e382e2400293, nullptr, nullptr, { &s_de92e382e2400293, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<101> b_da96579883444c35 = {
|
||||
static const ::capnp::_::AlignedData<136> b_da96579883444c35 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
53, 76, 68, 131, 152, 87, 150, 218,
|
||||
13, 0, 0, 0, 1, 0, 4, 0,
|
||||
13, 0, 0, 0, 1, 0, 5, 0,
|
||||
89, 10, 85, 29, 102, 186, 38, 181,
|
||||
0, 0, 7, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 58, 1, 0, 0,
|
||||
37, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
33, 0, 0, 0, 31, 1, 0, 0,
|
||||
33, 0, 0, 0, 143, 1, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
@@ -3756,42 +3756,56 @@ static const ::capnp::_::AlignedData<101> b_da96579883444c35 = {
|
||||
110, 103, 105, 116, 117, 100, 105, 110,
|
||||
97, 108, 80, 108, 97, 110, 0, 0,
|
||||
0, 0, 0, 0, 1, 0, 1, 0,
|
||||
20, 0, 0, 0, 3, 0, 4, 0,
|
||||
28, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
125, 0, 0, 0, 66, 0, 0, 0,
|
||||
181, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
120, 0, 0, 0, 3, 0, 1, 0,
|
||||
132, 0, 0, 0, 2, 0, 1, 0,
|
||||
176, 0, 0, 0, 3, 0, 1, 0,
|
||||
188, 0, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
129, 0, 0, 0, 154, 0, 0, 0,
|
||||
185, 0, 0, 0, 154, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
132, 0, 0, 0, 3, 0, 1, 0,
|
||||
144, 0, 0, 0, 2, 0, 1, 0,
|
||||
188, 0, 0, 0, 3, 0, 1, 0,
|
||||
200, 0, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
141, 0, 0, 0, 114, 0, 0, 0,
|
||||
197, 0, 0, 0, 114, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
140, 0, 0, 0, 3, 0, 1, 0,
|
||||
152, 0, 0, 0, 2, 0, 1, 0,
|
||||
196, 0, 0, 0, 3, 0, 1, 0,
|
||||
208, 0, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
149, 0, 0, 0, 154, 0, 0, 0,
|
||||
205, 0, 0, 0, 154, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
152, 0, 0, 0, 3, 0, 1, 0,
|
||||
164, 0, 0, 0, 2, 0, 1, 0,
|
||||
208, 0, 0, 0, 3, 0, 1, 0,
|
||||
220, 0, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 16, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
161, 0, 0, 0, 138, 0, 0, 0,
|
||||
217, 0, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
164, 0, 0, 0, 3, 0, 1, 0,
|
||||
176, 0, 0, 0, 2, 0, 1, 0,
|
||||
220, 0, 0, 0, 3, 0, 1, 0,
|
||||
232, 0, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 17, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
229, 0, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
232, 0, 0, 0, 3, 0, 1, 0,
|
||||
244, 0, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
241, 0, 0, 0, 202, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
248, 0, 0, 0, 3, 0, 1, 0,
|
||||
4, 1, 0, 0, 2, 0, 1, 0,
|
||||
118, 101, 114, 115, 105, 111, 110, 0,
|
||||
7, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -3838,15 +3852,36 @@ static const ::capnp::_::AlignedData<101> b_da96579883444c35 = {
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
102, 111, 114, 99, 101, 83, 116, 111,
|
||||
112, 66, 114, 97, 107, 105, 110, 103,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
102, 111, 114, 99, 101, 83, 116, 111,
|
||||
112, 69, 112, 105, 115, 111, 100, 101,
|
||||
77, 111, 110, 111, 84, 105, 109, 101,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, }
|
||||
};
|
||||
::capnp::word const* const bp_da96579883444c35 = b_da96579883444c35.words;
|
||||
#if !CAPNP_LITE
|
||||
static const uint16_t m_da96579883444c35[] = {3, 4, 2, 1, 0};
|
||||
static const uint16_t i_da96579883444c35[] = {0, 1, 2, 3, 4};
|
||||
static const uint16_t m_da96579883444c35[] = {3, 5, 6, 4, 2, 1, 0};
|
||||
static const uint16_t i_da96579883444c35[] = {0, 1, 2, 3, 4, 5, 6};
|
||||
const ::capnp::_::RawSchema s_da96579883444c35 = {
|
||||
0xda96579883444c35, b_da96579883444c35.words, 101, nullptr, m_da96579883444c35,
|
||||
0, 5, i_da96579883444c35, nullptr, nullptr, { &s_da96579883444c35, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
0xda96579883444c35, b_da96579883444c35.words, 136, nullptr, m_da96579883444c35,
|
||||
0, 7, i_da96579883444c35, nullptr, nullptr, { &s_da96579883444c35, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<1081> b_a1680744031fdb2d = {
|
||||
|
||||
@@ -729,7 +729,7 @@ struct StarPilotLongitudinalPlan {
|
||||
class Pipeline;
|
||||
|
||||
struct _capnpPrivate {
|
||||
CAPNP_DECLARE_STRUCT_HEADER(da96579883444c35, 4, 0)
|
||||
CAPNP_DECLARE_STRUCT_HEADER(da96579883444c35, 5, 0)
|
||||
#if !CAPNP_LITE
|
||||
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
|
||||
#endif // !CAPNP_LITE
|
||||
@@ -4298,6 +4298,10 @@ public:
|
||||
|
||||
inline bool getForceStopHolding() const;
|
||||
|
||||
inline bool getForceStopBraking() const;
|
||||
|
||||
inline ::uint64_t getForceStopEpisodeMonoTime() const;
|
||||
|
||||
private:
|
||||
::capnp::_::StructReader _reader;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -4341,6 +4345,12 @@ public:
|
||||
inline bool getForceStopHolding();
|
||||
inline void setForceStopHolding(bool value);
|
||||
|
||||
inline bool getForceStopBraking();
|
||||
inline void setForceStopBraking(bool value);
|
||||
|
||||
inline ::uint64_t getForceStopEpisodeMonoTime();
|
||||
inline void setForceStopEpisodeMonoTime( ::uint64_t value);
|
||||
|
||||
private:
|
||||
::capnp::_::StructBuilder _builder;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -18958,6 +18968,34 @@ inline void StarPilotLongitudinalPlan::Builder::setForceStopHolding(bool value)
|
||||
::capnp::bounded<16>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool StarPilotLongitudinalPlan::Reader::getForceStopBraking() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<17>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool StarPilotLongitudinalPlan::Builder::getForceStopBraking() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<17>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void StarPilotLongitudinalPlan::Builder::setForceStopBraking(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<17>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline ::uint64_t StarPilotLongitudinalPlan::Reader::getForceStopEpisodeMonoTime() const {
|
||||
return _reader.getDataField< ::uint64_t>(
|
||||
::capnp::bounded<4>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline ::uint64_t StarPilotLongitudinalPlan::Builder::getForceStopEpisodeMonoTime() {
|
||||
return _builder.getDataField< ::uint64_t>(
|
||||
::capnp::bounded<4>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void StarPilotLongitudinalPlan::Builder::setForceStopEpisodeMonoTime( ::uint64_t value) {
|
||||
_builder.setDataField< ::uint64_t>(
|
||||
::capnp::bounded<4>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool SlcState::Reader::hasSlotId() const {
|
||||
return !_reader.getPointerField(
|
||||
::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,4 +1,5 @@
|
||||
import numpy as np
|
||||
import math
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
@@ -96,6 +97,9 @@ class LongControl:
|
||||
output_accel -= self.stopping_decel_rate * DT_CTRL # m/s^2/s while trying to stop
|
||||
if self.extension is not None:
|
||||
output_accel = self.extension.stopping_output(output_accel, a_target, should_stop, CS)
|
||||
if (context.force_stop_braking and self.extension is not None and self.extension.ioniq6_start is not None and
|
||||
should_stop and not CS.standstill and CS.vEgo > .3 and math.isfinite(a_target)):
|
||||
output_accel = min(a_target, 0.0)
|
||||
self.reset(reset_start=False)
|
||||
|
||||
elif self.long_control_state == LongCtrlState.starting and self.extension is not None and self.extension.starting:
|
||||
@@ -119,3 +123,4 @@ class LongControl:
|
||||
output_accel = min(output_accel, planner_target)
|
||||
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
return self.last_output_accel
|
||||
|
||||
|
||||
+263
-263
File diff suppressed because one or more lines are too long
@@ -32,6 +32,7 @@ from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_lon
|
||||
from openpilot.starpilot.longitudinal.lane_change_gap import LaneChangeGap, project as project_lane_gap
|
||||
from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff, ExperimentalRelease, project as project_release
|
||||
from openpilot.starpilot.longitudinal.force_stop import StopPlan
|
||||
from openpilot.starpilot.longitudinal.force_stop_braking import distance_target
|
||||
from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk
|
||||
from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff
|
||||
from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior
|
||||
@@ -409,6 +410,9 @@ class LongitudinalPlanner:
|
||||
|
||||
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
|
||||
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
|
||||
if self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and self.force_stop_plan.obstacle_m is not None:
|
||||
stop_target = distance_target(v_ego, sm['carState'].aEgo, self.force_stop_plan.obstacle_m, action_t)
|
||||
candidates.append((stop_target, LongitudinalPlanSource.cruise, False))
|
||||
if sm['selfdriveState'].experimentalMode:
|
||||
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
|
||||
|
||||
@@ -510,5 +514,9 @@ class LongitudinalPlanner:
|
||||
hold.modelMonoTime = longitudinalPlan.modelMonoTime
|
||||
hold.driveStartMonoTime = self.plan_drive_id
|
||||
hold.forceStopHolding = bool(self.force_stop_plan.manual_hold)
|
||||
hold.forceStopBraking = bool(self.ioniq6_throttle_gate_enabled and self.force_stop_plan.forcing and
|
||||
self.force_stop_plan.should_stop)
|
||||
hold.forceStopEpisodeMonoTime = self.force_stop_plan.episode_ns
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
pm.send('starpilotLongitudinalPlan', companion)
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -24,6 +24,7 @@ class LongitudinalContext:
|
||||
custom_acceleration: bool | None = None
|
||||
profile_max_accel: float | None = None
|
||||
faster_takeoffs: bool = False
|
||||
force_stop_braking: bool = False
|
||||
|
||||
|
||||
class LongitudinalExtension:
|
||||
@@ -164,3 +165,4 @@ def create_extension(cp, startup_preferences=None):
|
||||
not extension.ignore_cruise_standstill):
|
||||
return None
|
||||
return extension
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@ from dataclasses import dataclass
|
||||
import math
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.starpilot.longitudinal.force_stop_braking import STOP_RESERVE_M
|
||||
|
||||
STANDSTILL_HOLD_NS = 2_000_000_000
|
||||
STANDSTILL_CLEAR_NS = 750_000_000
|
||||
@@ -34,9 +35,12 @@ class StopTune:
|
||||
jerk_scale: float = 0.32
|
||||
reanchor_speed_tolerance: float | None = None
|
||||
low_speed_hold_mps: float | None = None
|
||||
distance_control: bool = False
|
||||
|
||||
|
||||
def vehicle_tune(fingerprint: str) -> StopTune:
|
||||
if fingerprint == 'HYUNDAI_IONIQ_6':
|
||||
return StopTune(distance_control=True)
|
||||
if fingerprint == 'TOYOTA_CAMRY_TSS2':
|
||||
return StopTune(handoff_m=4.5, distance_bias_m=6.0)
|
||||
if fingerprint == 'HYUNDAI_ELANTRA_2021':
|
||||
@@ -70,6 +74,7 @@ class StopFrame:
|
||||
sign: bool = False
|
||||
dashboard_sign: bool = False
|
||||
vision_distance_m: float | None = None
|
||||
moving_preview: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -86,6 +91,7 @@ class StopPlan:
|
||||
takeoff_light_observed: bool = False
|
||||
takeoff_light: bool = False
|
||||
resume_override: bool = False
|
||||
episode_ns: int = 0
|
||||
|
||||
|
||||
class ForceStop:
|
||||
@@ -110,6 +116,9 @@ class ForceStop:
|
||||
self.previously_enabled = False
|
||||
self.distance = 0.0
|
||||
self.distance_cap = 0.0
|
||||
self.episode_ns = 0
|
||||
self.stop_requested = False
|
||||
self.preview_tracking = False
|
||||
self.queue_sign_ns = 0
|
||||
self.queue_line = None
|
||||
self._clear_sign_estimate()
|
||||
@@ -276,16 +285,21 @@ class ForceStop:
|
||||
offset = max(-20, min(20, offset_ft)) * .3048
|
||||
approach = 0.0
|
||||
ceiling = None
|
||||
native_stop = False
|
||||
if force and not overridden:
|
||||
if self.entry_speed is None and not frame.standstill:
|
||||
self.entry_speed = speed
|
||||
self.preview_tracking = tune.distance_control and frame.moving_preview
|
||||
self.forcing |= not frame.standstill or held
|
||||
if held:
|
||||
self.distance = 0
|
||||
ceiling = 0.0
|
||||
native_stop = True
|
||||
else:
|
||||
self.distance = max(0.0, self.distance - speed * dt)
|
||||
if (available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
|
||||
self.distance -= speed * dt
|
||||
if not tune.distance_control:
|
||||
self.distance = max(0.0,self.distance)
|
||||
if (not tune.distance_control and available and not frame.sign and self.distance > max(tune.handoff_m, 40) and not frame.model_should_stop and
|
||||
horizon > self.distance + 3 and (tune.reanchor_speed_tolerance is None or self.entry_speed is None or
|
||||
speed >= self.entry_speed - tune.reanchor_speed_tolerance)):
|
||||
self.distance = horizon
|
||||
@@ -294,16 +308,29 @@ class ForceStop:
|
||||
self.distance_cap = max(0.0, self.distance_cap - speed * dt)
|
||||
cap = self.distance_cap + 15 * min(self.distance_cap / 60, 1)
|
||||
self.distance = min(self.distance, cap)
|
||||
if available and (not frame.moving_preview or frame.model_should_stop or self.sign_confirmed):
|
||||
self.preview_tracking = False
|
||||
if (self.preview_tracking and available and frame.moving_preview and not self.stop_requested and
|
||||
not self.sign_confirmed and not frame.model_should_stop):
|
||||
# A moving model preview is not a fixed world endpoint to count down.
|
||||
self.distance = self.distance_cap = horizon
|
||||
terminal_distance = self.distance
|
||||
if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M):
|
||||
self.distance = min(sign_estimate, cap)
|
||||
if self.queue_line is not None and self.sign_confirmed:
|
||||
self.distance = self.queue_line
|
||||
if tune.distance_control and self.stop_requested:
|
||||
self.distance = min(self.distance, terminal_distance)
|
||||
effective = self.distance + offset + tune.distance_bias_m
|
||||
decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
|
||||
ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
|
||||
native_stop = ceiling <= .5
|
||||
if tune.distance_control:
|
||||
ceiling = math.sqrt(2 * decel * max(0, effective - STOP_RESERVE_M))
|
||||
else:
|
||||
self.forcing = False
|
||||
self.entry_speed = None
|
||||
self.preview_tracking = False
|
||||
self.sign_confirmed = False
|
||||
self._clear_standstill()
|
||||
self.distance = self.distance_cap = horizon if available else 0.0
|
||||
@@ -313,13 +340,22 @@ class ForceStop:
|
||||
if effective > tune.handoff_m:
|
||||
ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m))
|
||||
self.previously_enabled = frame.enabled
|
||||
self.stop_requested = bool(self.forcing and (self.stop_requested or native_stop))
|
||||
if self.forcing and not self.episode_ns:
|
||||
self.episode_ns = now
|
||||
elif not self.forcing:
|
||||
self.episode_ns = 0
|
||||
stop_length = self.distance if self.forcing else approach
|
||||
# The MPC obstacle includes its standing-distance term; adding it at the
|
||||
# native boundary avoids moving the actual stop line by that buffer.
|
||||
# The offset moves the handoff with the ceiling; otherwise a positive offset creeps with no stop target.
|
||||
obstacle = (stop_length * .93 + offset + tune.distance_bias_m
|
||||
if stop_length > 0 and stop_length + offset > tune.handoff_m else None)
|
||||
return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5),
|
||||
self.distance if self.forcing else 0.0, approach,
|
||||
if stop_length > 0 and stop_length + max(0.0, offset) > tune.handoff_m else None)
|
||||
if tune.distance_control and self.forcing:
|
||||
obstacle = max(0.0,min(self.distance * .93 + offset + tune.distance_bias_m,
|
||||
(self.distance + offset + tune.distance_bias_m) * .93))
|
||||
should_stop = self.stop_requested if tune.distance_control else bool(self.forcing and ceiling is not None and ceiling <= .5)
|
||||
return StopPlan(now, ceiling, obstacle, self.forcing, should_stop,
|
||||
max(0.0,self.distance) if self.forcing else 0.0, approach,
|
||||
tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden),
|
||||
resume_override=now < self.resume_until_ns)
|
||||
resume_override=now < self.resume_until_ns, episode_ns=self.episode_ns)
|
||||
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
"""Ioniq 6 distance braking under the existing Force Stop stop/hold intent."""
|
||||
|
||||
import math
|
||||
|
||||
MAX_AGE_NS = 150_000_000
|
||||
STOP_RESERVE_M = .5
|
||||
MIN_ACCEL = -3.5
|
||||
|
||||
|
||||
def distance_target(speed, acceleration, distance, delay):
|
||||
if (not all(math.isfinite(x) for x in (speed, acceleration, distance, delay)) or
|
||||
speed < 0 or distance < 0 or not 0 <= delay <= 2):
|
||||
return MIN_ACCEL
|
||||
remaining = distance - speed * delay - .5 * max(acceleration, 0.) * delay**2 - STOP_RESERVE_M
|
||||
future_speed = speed + max(acceleration, 0.) * delay
|
||||
return max(MIN_ACCEL, -future_speed**2 / (2 * max(remaining, .01)))
|
||||
|
||||
|
||||
def current_braking(sm, now_ns):
|
||||
try:
|
||||
device, plan, stop = (sm[name] for name in ('deviceState', 'longitudinalPlan', 'starpilotLongitudinalPlan'))
|
||||
drive = int(device.startedMonoTime)
|
||||
if not device.started or not 0 < drive < now_ns or not plan.shouldStop:
|
||||
return False
|
||||
for name in ('carState', 'deviceState', 'longitudinalPlan', 'starpilotLongitudinalPlan'):
|
||||
stamp, receipt = int(sm.logMonoTime[name]), int(sm.recv_time[name] * 1e9)
|
||||
maximum = 1_250_000_000 if name == 'deviceState' else MAX_AGE_NS
|
||||
if (not sm.seen[name] or not sm.valid[name] or not sm.alive[name] or
|
||||
not drive < stamp <= receipt <= now_ns or now_ns - stamp > maximum):
|
||||
return False
|
||||
return bool(stop.version == 1 and stop.forceStopBraking and stop.driveStartMonoTime == drive and
|
||||
drive < stop.forceStopEpisodeMonoTime <= stop.modelMonoTime <= stop.sourcePlanMonoTime <= now_ns and
|
||||
stop.sourcePlanMonoTime == sm.logMonoTime['longitudinalPlan'] and
|
||||
stop.sourcePlanMonoTime == sm.logMonoTime['starpilotLongitudinalPlan'] and
|
||||
stop.modelMonoTime == plan.modelMonoTime and now_ns - stop.modelMonoTime <= MAX_AGE_NS and
|
||||
sm['carState'].canValid and not sm['carState'].canTimeout and
|
||||
not sm['carState'].brakePressed and not sm['carState'].gasPressed)
|
||||
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
|
||||
return False
|
||||
@@ -1,6 +1,7 @@
|
||||
"""Current-drive Force Stop owner with bounded preference reads and live scene inputs."""
|
||||
|
||||
from dataclasses import replace
|
||||
import math
|
||||
|
||||
from openpilot.starpilot.conditional_mode.curve import frozen_raw_curve
|
||||
from openpilot.starpilot.conditional_mode.preferences import CEMOptions, decode_preferences, PreferenceError
|
||||
@@ -13,6 +14,7 @@ from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, St
|
||||
from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_horizon, standstill_release_speed
|
||||
from openpilot.starpilot.saved_source import read_saved
|
||||
from openpilot.starpilot.longitudinal.stop_resume import StopResume
|
||||
from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
|
||||
from openpilot.starpilot.speed_limits.vision.stop_observation import CONTROL_STOP_MAX_AGE_NS, StopConfirmation, read_stop_evidence
|
||||
|
||||
|
||||
@@ -198,8 +200,12 @@ class ForceStopRuntime:
|
||||
raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None,
|
||||
bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested,
|
||||
perception_available=perception_available, sign=dashboard_sign or vision_fresh,
|
||||
dashboard_sign=dashboard_sign, vision_distance_m=vision_distance)
|
||||
self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
|
||||
dashboard_sign=dashboard_sign, vision_distance_m=vision_distance,
|
||||
moving_preview=bool(perception_available and not model.action.shouldStop and
|
||||
len(model.velocity.x) == len(model.position.x) and
|
||||
math.isfinite(model.velocity.x[-1]) and model.velocity.x[-1] > .3))
|
||||
tune = replace(vehicle_tune(str(cp.carFingerprint)), distance_control=ioniq6_long_eligible(cp))
|
||||
self.plan = self.policy.step(frame, tune, self.offset)
|
||||
if takeoff_enabled:
|
||||
self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)
|
||||
return self.plan
|
||||
|
||||
@@ -131,6 +131,8 @@ class LongitudinalInputs:
|
||||
|
||||
@property
|
||||
def optional_services(self):
|
||||
if self.ioniq6_start_enabled:
|
||||
return ['radarState', 'deviceState', 'starpilotLongitudinalPlan']
|
||||
if (self.toyota_sienna_replay or self.toyota_sienna_production or self.toyota_corolla_stop or
|
||||
self.ioniq6_start_enabled or self.gm_start_enabled or self.gm_volt_enabled or self.gm_euv_enabled or
|
||||
self.gm_cc_enabled or self.gm_cc_stop_enabled or self.gm_ascm_enabled or self.gm_suburban_enabled):
|
||||
@@ -151,6 +153,9 @@ class LongitudinalInputs:
|
||||
|
||||
def context(self, active):
|
||||
context = self._vehicle_context(active)
|
||||
if active and self.ioniq6_start_enabled:
|
||||
from openpilot.starpilot.longitudinal.force_stop_braking import current_braking
|
||||
context = replace(context, force_stop_braking=current_braking(self.sm, time.monotonic_ns()))
|
||||
self.faster_takeoffs = bool(self.takeoff_preferences is not None and
|
||||
self.takeoff_preferences.sample(time.monotonic_ns()))
|
||||
if not self.faster_takeoffs:
|
||||
@@ -424,3 +429,4 @@ class LongitudinalInputs:
|
||||
return None
|
||||
return PedalStartEvidence(drive_id, now_ns, any(lead.present for lead in leads), traffic_mode=traffic_mode,
|
||||
custom_acceleration=profile.custom_acceleration if profile is not None else False, profile_max_accel=ceiling)
|
||||
|
||||
|
||||
@@ -15,11 +15,75 @@ def frame(tick, **changes):
|
||||
|
||||
|
||||
class ForceStopTests(unittest.TestCase):
|
||||
def test_negative_offset_keeps_the_cold_approach_obstacle(self):
|
||||
for vehicle in ['OTHER','HYUNDAI_IONIQ_6']:
|
||||
plan = ForceStop().step(frame(0,speed_mps=8.,horizon_m=10.),vehicle_tune(vehicle),-20)
|
||||
self.assertFalse(plan.forcing)
|
||||
assert plan.obstacle_m is not None
|
||||
self.assertAlmostEqual(plan.obstacle_m,10*.93-20*.3048)
|
||||
|
||||
def test_ioniq_negative_offset_keeps_the_existing_conservative_obstacle(self):
|
||||
owner = ForceStop()
|
||||
owner.last_model_ns = BASE+10*TICK
|
||||
owner.distance = 10.
|
||||
owner.distance_cap = 10.
|
||||
owner.forcing = True
|
||||
owner.timer = 1.
|
||||
plan = owner.step(frame(11,speed_mps=4.,horizon_m=10.),vehicle_tune('HYUNDAI_IONIQ_6'),-20)
|
||||
assert plan.obstacle_m is not None
|
||||
self.assertAlmostEqual(plan.obstacle_m,9.8*.93-20*.3048)
|
||||
|
||||
def test_ioniq_offset_moves_the_endpoint_without_rescaling_the_offset(self):
|
||||
owner = ForceStop()
|
||||
owner.last_model_ns = BASE+10*TICK
|
||||
owner.distance = -1.
|
||||
owner.forcing = True
|
||||
owner.timer = 1.
|
||||
plan = owner.step(frame(11,speed_mps=4.,horizon_m=10.),vehicle_tune('HYUNDAI_IONIQ_6'),8)
|
||||
assert plan.obstacle_m is not None
|
||||
self.assertAlmostEqual(plan.obstacle_m,(-1.2+8*.3048)*.93)
|
||||
|
||||
def test_ioniq_positive_offset_continues_to_consume_remaining_distance(self):
|
||||
owner = ForceStop()
|
||||
tune = vehicle_tune('HYUNDAI_IONIQ_6')
|
||||
for tick in range(100):
|
||||
plan = owner.step(frame(tick,speed_mps=4.,horizon_m=10.),tune,8)
|
||||
self.assertTrue(plan.should_stop)
|
||||
self.assertEqual(plan.tracked_distance_m,0.)
|
||||
self.assertEqual(plan.obstacle_m,0.)
|
||||
|
||||
def test_ioniq_keeps_rolling_goal_and_stop_intent_separate(self):
|
||||
owner = ForceStop()
|
||||
tune = vehicle_tune('HYUNDAI_IONIQ_6')
|
||||
for tick in range(35):
|
||||
plan = owner.step(frame(tick,speed_mps=4.,horizon_m=10.),tune)
|
||||
if plan.forcing and plan.tracked_distance_m < 6:
|
||||
self.assertTrue(plan.should_stop)
|
||||
assert plan.speed_ceiling_mps is not None
|
||||
self.assertGreater(plan.speed_ceiling_mps,.5)
|
||||
self.assertIsNotNone(plan.obstacle_m)
|
||||
self.assertTrue(plan.should_stop)
|
||||
|
||||
def test_episode_is_retained_until_release_and_new_commit_gets_new_identity(self):
|
||||
owner = ForceStop()
|
||||
tune = vehicle_tune('HYUNDAI_IONIQ_6')
|
||||
for tick in range(20):
|
||||
plan = owner.step(frame(tick),tune)
|
||||
episode = plan.episode_ns
|
||||
self.assertGreater(episode,BASE)
|
||||
for tick in range(20,30):
|
||||
self.assertEqual(owner.step(frame(tick),tune).episode_ns,episode)
|
||||
self.assertEqual(owner.step(frame(30,pedal_override=True),tune).episode_ns,0)
|
||||
for tick in range(31,260):
|
||||
plan = owner.step(frame(tick),tune)
|
||||
self.assertGreater(plan.episode_ns,episode)
|
||||
|
||||
def setUp(self):
|
||||
self.owner = ForceStop()
|
||||
self.tune = vehicle_tune('HYUNDAI_IONIQ_6')
|
||||
|
||||
def test_original_approach_commit_kinematics_and_obstacle(self):
|
||||
self.tune = vehicle_tune('OTHER')
|
||||
for tick in range(40):
|
||||
plan = self.owner.step(frame(tick, model_should_stop=True), self.tune)
|
||||
# The existing 0.5s accumulator crosses .5 on frame 10 with binary floats.
|
||||
@@ -370,6 +434,7 @@ class ForceStopTests(unittest.TestCase):
|
||||
self.assertLessEqual(plan.tracked_distance_m, cap - .5 + 15)
|
||||
|
||||
def test_offset_and_handoff_keep_native_should_stop(self):
|
||||
self.tune = vehicle_tune('OTHER')
|
||||
for offset in (-20, 0, 20):
|
||||
with self.subTest(offset=offset):
|
||||
self.owner.reset()
|
||||
@@ -404,10 +469,6 @@ class ForceStopTests(unittest.TestCase):
|
||||
self.assertFalse(self.owner.forcing)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
||||
|
||||
def test_resume_override_is_only_for_a_previous_hold_and_ends_at_speed_lead_or_timeout(self):
|
||||
for cause in ('speed', 'lead', 'tracked', 'timeout'):
|
||||
with self.subTest(cause=cause):
|
||||
@@ -423,3 +484,94 @@ if __name__ == '__main__':
|
||||
self.owner.reset()
|
||||
first = self.owner.step(frame(0, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True, resume_requested=True), self.tune)
|
||||
self.assertFalse(first.resume_override)
|
||||
|
||||
def preview_frame(tick, **changes):
|
||||
return replace(frame(tick, speed_mps=4.0, horizon_m=30.0, raw_model_stopped=True), **changes)
|
||||
|
||||
|
||||
class MovingPreviewTests(unittest.TestCase):
|
||||
def test_moving_preview_does_not_become_an_odometry_countdown(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(180):
|
||||
plan = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertTrue(plan.forcing)
|
||||
self.assertFalse(plan.should_stop)
|
||||
self.assertEqual(plan.tracked_distance_m, 30.0)
|
||||
|
||||
def test_both_fresh_horizons_extend_the_rolling_preview(self):
|
||||
for horizon in [24.59375, 32.34375]:
|
||||
owner = ForceStop()
|
||||
owner.last_model_ns = BASE
|
||||
owner.distance = owner.distance_cap = 6.19
|
||||
owner.forcing = True
|
||||
owner.timer = 1.0
|
||||
owner.entry_speed = 4.0
|
||||
owner.preview_tracking = True
|
||||
plan = owner.step(preview_frame(1, horizon_m=horizon, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertFalse(plan.should_stop)
|
||||
self.assertEqual(plan.tracked_distance_m, horizon)
|
||||
|
||||
def test_terminal_stop_cannot_reopen(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(15):
|
||||
plan = owner.step(preview_frame(tick, horizon_m=5.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertTrue(plan.should_stop)
|
||||
before = plan.tracked_distance_m
|
||||
plan = owner.step(preview_frame(15, horizon_m=35.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertTrue(plan.should_stop)
|
||||
self.assertLess(plan.tracked_distance_m, before)
|
||||
|
||||
def test_missing_geometry_keeps_last_preview_and_odometry(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(15):
|
||||
before = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
after = owner.step(preview_frame(15, horizon_m=None, road_curvature=None, perception_available=False, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertAlmostEqual(after.tracked_distance_m, before.tracked_distance_m - 0.2)
|
||||
|
||||
def test_confirmed_stop_and_sign_cannot_resume_preview_tracking(self):
|
||||
for kw in [{'model_should_stop': True}, {'sign': True}]:
|
||||
owner = ForceStop()
|
||||
owner.last_model_ns = BASE
|
||||
owner.distance = owner.distance_cap = 8.0
|
||||
owner.forcing = True
|
||||
owner.timer = 1.0
|
||||
owner.entry_speed = 4.0
|
||||
owner.preview_tracking = True
|
||||
fixed = owner.step(preview_frame(1, horizon_m=35.0, moving_preview=True, **kw), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
after = owner.step(preview_frame(2, horizon_m=50.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertFalse(owner.preview_tracking)
|
||||
self.assertLess(after.tracked_distance_m, fixed.tracked_distance_m)
|
||||
|
||||
def test_terminal_stop_distance_cannot_reopen_on_new_sign_range(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(15):
|
||||
before = owner.step(preview_frame(tick, horizon_m=5.0), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertTrue(before.should_stop)
|
||||
after = owner.step(preview_frame(15, horizon_m=35.0, sign=True, vision_distance_m=30.), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertTrue(after.should_stop)
|
||||
self.assertLess(after.tracked_distance_m, before.tracked_distance_m)
|
||||
|
||||
def test_once_stationary_endpoint_seen_it_cannot_become_a_moving_preview_again(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(15):
|
||||
owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
fixed = owner.step(preview_frame(15, horizon_m=20.0, moving_preview=False), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
reopened = owner.step(preview_frame(16, horizon_m=35.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertLess(reopened.tracked_distance_m, fixed.tracked_distance_m)
|
||||
|
||||
def test_fixed_endpoint_above_forty_metres_cannot_reopen(self):
|
||||
owner = ForceStop()
|
||||
owner.last_model_ns = BASE
|
||||
owner.distance = owner.distance_cap = 80.
|
||||
owner.entry_speed = 4.
|
||||
owner.forcing = True
|
||||
owner.timer = 1.
|
||||
plan = owner.step(preview_frame(1, horizon_m=190., moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
|
||||
self.assertLess(plan.tracked_distance_m, 80.)
|
||||
|
||||
def test_other_profiles_retain_existing_distance_policy(self):
|
||||
owner = ForceStop()
|
||||
for tick in range(180):
|
||||
plan = owner.step(preview_frame(tick, moving_preview=True), vehicle_tune('OTHER'))
|
||||
self.assertTrue(plan.should_stop)
|
||||
self.assertLess(plan.tracked_distance_m, 6.0)
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
from types import SimpleNamespace as NS
|
||||
from unittest import TestCase
|
||||
import math
|
||||
import tempfile
|
||||
from unittest.mock import patch
|
||||
|
||||
from openpilot.starpilot.longitudinal.force_stop_braking import current_braking, distance_target
|
||||
|
||||
BASE = 100_000_000_000
|
||||
NOW = BASE+1_000_000_000
|
||||
|
||||
|
||||
def inputs():
|
||||
class Messages(dict):
|
||||
pass
|
||||
sm = Messages(deviceState=NS(started=True,startedMonoTime=BASE),
|
||||
carState=NS(canValid=True,canTimeout=False,brakePressed=False,gasPressed=False),
|
||||
longitudinalPlan=NS(shouldStop=True,modelMonoTime=NOW-20_000_000),
|
||||
starpilotLongitudinalPlan=NS(version=1,forceStopBraking=True,driveStartMonoTime=BASE,
|
||||
forceStopEpisodeMonoTime=BASE+1,modelMonoTime=NOW-20_000_000,sourcePlanMonoTime=NOW-10_000_000))
|
||||
sm.logMonoTime = dict.fromkeys(sm,NOW-10_000_000)
|
||||
sm.recv_time = dict.fromkeys(sm,(NOW-5_000_000)/1e9)
|
||||
sm.valid = dict.fromkeys(sm,True)
|
||||
sm.alive = dict.fromkeys(sm,True)
|
||||
sm.seen = dict.fromkeys(sm,True)
|
||||
return sm
|
||||
|
||||
|
||||
class ForceStopBrakingTests(TestCase):
|
||||
def test_only_current_matching_stop_intent_qualifies(self):
|
||||
self.assertTrue(current_braking(inputs(),NOW))
|
||||
for field,value in [('version',0),('forceStopBraking',False),('driveStartMonoTime',BASE-1),
|
||||
('forceStopEpisodeMonoTime',BASE),('forceStopEpisodeMonoTime',NOW+1),
|
||||
('modelMonoTime',NOW-21_000_000),('sourcePlanMonoTime',NOW-11_000_000)]:
|
||||
with self.subTest(field=field,value=value):
|
||||
sm = inputs()
|
||||
setattr(sm['starpilotLongitudinalPlan'],field,value)
|
||||
self.assertFalse(current_braking(sm,NOW))
|
||||
|
||||
def test_withdrawal_override_or_stale_source_keeps_original_stop(self):
|
||||
for field in ['brakePressed','gasPressed','canTimeout']:
|
||||
sm = inputs()
|
||||
setattr(sm['carState'],field,True)
|
||||
self.assertFalse(current_braking(sm,NOW))
|
||||
for name in inputs():
|
||||
for bad in ['seen','valid','alive']:
|
||||
sm = inputs()
|
||||
getattr(sm,bad)[name] = False
|
||||
self.assertFalse(current_braking(sm,NOW))
|
||||
sm = inputs()
|
||||
sm['longitudinalPlan'].shouldStop = False
|
||||
self.assertFalse(current_braking(sm,NOW))
|
||||
self.assertFalse(current_braking(inputs(),NOW+2_000_000_000))
|
||||
|
||||
def test_distance_target_strengthens_with_less_room_or_more_latency(self):
|
||||
self.assertLess(distance_target(4.,0.,6.,.5),distance_target(4.,0.,12.,.5))
|
||||
self.assertLess(distance_target(4.,0.,12.,.5),distance_target(4.,0.,12.,.1))
|
||||
self.assertLessEqual(distance_target(4.,1.,12.,.5),distance_target(4.,0.,12.,.5))
|
||||
self.assertEqual(distance_target(4.,0.,0.,.5),-3.5)
|
||||
self.assertEqual(distance_target(4.,float('nan'),12.,.5),-3.5)
|
||||
self.assertEqual(distance_target(4.,0.,12.,-1.),-3.5)
|
||||
self.assertEqual(distance_target(0.,0.,0.,.5),0.)
|
||||
|
||||
|
||||
class NativeForceStopBrakingTests(TestCase):
|
||||
def test_actual_inputs_control_hold_and_can_request(self):
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
||||
from openpilot.starpilot.longitudinal.inputs import LongitudinalInputs
|
||||
from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate, car_state
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.car.hyundai.hyundaicanfd import create_acc_control
|
||||
from opendbc.car.hyundai.ioniq6_longitudinal import Ioniq6LongitudinalPolicy
|
||||
|
||||
stock,cp = candidate()
|
||||
sm = inputs()
|
||||
sm.all_checks = lambda names: all(sm.valid.get(name,False) for name in names)
|
||||
stop = messaging.new_message('starpilotLongitudinalPlan',valid=True)
|
||||
stop.logMonoTime = NOW-10_000_000
|
||||
for key,value in vars(sm['starpilotLongitudinalPlan']).items():
|
||||
setattr(stop.starpilotLongitudinalPlan,key,value)
|
||||
plan = messaging.new_message('longitudinalPlan',valid=True)
|
||||
plan.logMonoTime = NOW-10_000_000
|
||||
plan.longitudinalPlan.modelMonoTime = NOW-20_000_000
|
||||
plan.longitudinalPlan.shouldStop = True
|
||||
sm['starpilotLongitudinalPlan'] = messaging.log_from_bytes(stop.to_bytes()).starpilotLongitudinalPlan
|
||||
sm['longitudinalPlan'] = messaging.log_from_bytes(plan.to_bytes()).longitudinalPlan
|
||||
car = car_state(speed=4.,standstill=False)
|
||||
sm['carState'] = car
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
reader = LongitudinalInputs(cp,Params(directory),lambda:sm)
|
||||
self.assertIn('starpilotLongitudinalPlan',reader.optional_services)
|
||||
with patch('openpilot.starpilot.longitudinal.inputs.time.monotonic_ns',return_value=NOW):
|
||||
context = reader.context(True)
|
||||
self.assertTrue(context.force_stop_braking)
|
||||
control = LongControl(cp)
|
||||
control.long_control_state = LongCtrlState.pid
|
||||
control.last_output_accel = -1.5
|
||||
result = control.update(True,car,-.8,True,(-3.5,2.),context=context)
|
||||
self.assertAlmostEqual(result,-.8)
|
||||
self.assertEqual(control.long_control_state,LongCtrlState.stopping)
|
||||
strong = control.update(True,car,-3.,True,(-3.5,2.),context=context)
|
||||
self.assertEqual(strong,-3.)
|
||||
self.assertTrue(math.isfinite(control.update(True,car,math.nan,True,(-3.5,2.),context=context)))
|
||||
with patch('openpilot.starpilot.longitudinal.inputs.time.monotonic_ns',return_value=NOW+200_000_000):
|
||||
stale = reader.context(True)
|
||||
self.assertFalse(stale.force_stop_braking)
|
||||
control.last_output_accel = -.8
|
||||
self.assertLess(control.update(True,car,-.8,True,(-3.5,2.),context=stale),-.8)
|
||||
stopped = car_state()
|
||||
control.last_output_accel = -1.5
|
||||
self.assertLess(control.update(True,stopped,0.,True,(-3.5,2.),context=context),-1.5)
|
||||
policy = Ioniq6LongitudinalPolicy()
|
||||
request = policy.update(0,active=True,override=False,accel=-.8,speed=4.,measured_accel=-1.5,
|
||||
control_state=LongCtrlState.stopping,last_sent_accel=-1.5)
|
||||
packet = create_acc_control(CANPacker('hyundai_canfd_generated'),NS(ECAN=1),True,-1.5,request.accel,
|
||||
request.stopping,False,40.,NS(leadDistanceBars=2),direct_accel=True)
|
||||
bits = int.from_bytes(packet[1],'little')
|
||||
self.assertEqual((bits>>184)&1,1)
|
||||
self.assertEqual((bits>>68)&7,1)
|
||||
normal = LongControl(stock)
|
||||
normal.long_control_state = LongCtrlState.pid
|
||||
normal.last_output_accel = -1.5
|
||||
self.assertLess(normal.update(True,car,-.8,True,(-3.5,2.),context=context),-1.5)
|
||||
@@ -86,6 +86,20 @@ class ForceStopRuntimeTests(unittest.TestCase):
|
||||
self.assertFalse(self.owner.policy.can_commit_next_frame)
|
||||
self.assertIsNotNone(self.owner.plan.speed_ceiling_mps)
|
||||
self.assertTrue(wheel_override_blocked(self.owner, None))
|
||||
def test_distance_braking_requires_exact_ioniq_control_profile(self):
|
||||
from unittest.mock import patch
|
||||
from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate
|
||||
|
||||
stock, admitted = candidate()
|
||||
stock.openpilotLongitudinalControl = True # An incomplete profile must still refuse the new tune.
|
||||
for cp, expected in [(self.cp, False), (stock, False), (admitted, True)]:
|
||||
self.cp = cp
|
||||
self.owner = ForceStopRuntime(self.params)
|
||||
with patch.object(self.owner.policy, 'step', wraps=self.owner.policy.step) as step:
|
||||
for tick in range(12):
|
||||
self.sample(Frame(tick))
|
||||
self.assertTrue(step.called)
|
||||
self.assertEqual(step.call_args.args[1].distance_control, expected)
|
||||
|
||||
def test_committed_stop_survives_valid_mode_settings_and_traffic_changes(self):
|
||||
import json
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,4 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
extern const uint8_t gitversion[29];
|
||||
const uint8_t gitversion[29] = "DEV-3a22ddda-Pf38f504f-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-b6d2afbc-Pf38f504f-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-3a22ddda-Pf38f504f-DEBUG
|
||||
DEV-b6d2afbc-Pf38f504f-DEBUG
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "b9672e51fdd2774a1f4c75e2ac164b85ab8bd9459710668197af0b6f4154ff0b", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "c10fc26627484c772df8b9e3b958f3f4c0fa0845cd858aa80a0c237a4f2ffd31", "schema_version": 1}
|
||||
|
||||
Binary file not shown.
@@ -27,7 +27,8 @@
|
||||
"FordLearningEvidence": "15123011525280866286",
|
||||
"FordLearningEvent": "12859710833724959940",
|
||||
"CentreApplicationWitness": "12325001751236334142",
|
||||
"LearningTorqueProjection": "17327869370982877613"
|
||||
"LearningTorqueProjection": "17327869370982877613",
|
||||
"StarPilotLongitudinalPlan": "15750873059323825205"
|
||||
},
|
||||
"nodes": {
|
||||
"14441422469354927087": {
|
||||
@@ -13433,6 +13434,128 @@
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"15750873059323825205": {
|
||||
"kind": "struct",
|
||||
"data_words": 5,
|
||||
"pointers": 0,
|
||||
"is_group": false,
|
||||
"union_count": 0,
|
||||
"union_offset": 0,
|
||||
"fields": {
|
||||
"0": {
|
||||
"slot": {
|
||||
"offset": 0,
|
||||
"type": {
|
||||
"uint16": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"uint16": 0
|
||||
}
|
||||
},
|
||||
"name": "version",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 0
|
||||
}
|
||||
},
|
||||
"1": {
|
||||
"slot": {
|
||||
"offset": 1,
|
||||
"type": {
|
||||
"uint64": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"uint64": 0
|
||||
}
|
||||
},
|
||||
"name": "sourcePlanMonoTime",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 1
|
||||
}
|
||||
},
|
||||
"2": {
|
||||
"slot": {
|
||||
"offset": 2,
|
||||
"type": {
|
||||
"uint64": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"uint64": 0
|
||||
}
|
||||
},
|
||||
"name": "modelMonoTime",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 2
|
||||
}
|
||||
},
|
||||
"3": {
|
||||
"slot": {
|
||||
"offset": 3,
|
||||
"type": {
|
||||
"uint64": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"uint64": 0
|
||||
}
|
||||
},
|
||||
"name": "driveStartMonoTime",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 3
|
||||
}
|
||||
},
|
||||
"4": {
|
||||
"slot": {
|
||||
"offset": 16,
|
||||
"type": {
|
||||
"bool": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"bool": false
|
||||
}
|
||||
},
|
||||
"name": "forceStopHolding",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 4
|
||||
}
|
||||
},
|
||||
"5": {
|
||||
"slot": {
|
||||
"offset": 17,
|
||||
"type": {
|
||||
"bool": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"bool": false
|
||||
}
|
||||
},
|
||||
"name": "forceStopBraking",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 5
|
||||
}
|
||||
},
|
||||
"6": {
|
||||
"slot": {
|
||||
"offset": 4,
|
||||
"type": {
|
||||
"uint64": null
|
||||
},
|
||||
"defaultValue": {
|
||||
"uint64": 0
|
||||
}
|
||||
},
|
||||
"name": "forceStopEpisodeMonoTime",
|
||||
"discriminantValue": 65535,
|
||||
"ordinal": {
|
||||
"explicit": 6
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -270,7 +270,7 @@ class SchemaPolicyTest(unittest.TestCase):
|
||||
original = b'forceStopHolding @4 :Bool;'
|
||||
self.assertIn(original, self.schemas[path])
|
||||
appended = self.schemas.copy()
|
||||
appended[path] = appended[path].replace(original, original + b'\n futureStatus @5 :UInt32;')
|
||||
appended[path] = appended[path].replace(original, original + b'\n futureStatus @7 :UInt32;')
|
||||
self.assertEqual(validate(appended, self.policy, self.sync), [])
|
||||
for replacement in (b'', b'forceStopHolding @5 :Bool;', b'forceStopHolding @4 :UInt32;',
|
||||
b'changedHold @4 :Bool;'):
|
||||
@@ -474,3 +474,4 @@ class SchemaPolicyTest(unittest.TestCase):
|
||||
changed = self.schemas.copy()
|
||||
changed[path] = changed[path].replace(original, original + b'\n duplicate @12 :Text;', 1)
|
||||
self.assertTrue(any('Duplicate member name or ordinal' in error for error in validate(changed, self.policy, self.sync)))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user