mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-01 03:24:05 +08:00
Fix Honda AOL engagement and authority loss
This commit is contained in:
firestar5683
@@ -70,11 +70,16 @@ def temporary_restriction(cp, state):
|
||||
(state.gearShifter not in (gears.drive, gears.sport, gears.low, gears.brake) or state.steerFaultTemporary))
|
||||
|
||||
|
||||
def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns):
|
||||
def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns,
|
||||
previously_acknowledged=False, host_requested_lateral=False):
|
||||
lost = not panda_ready or native is None
|
||||
if not latched or lost or restricted:
|
||||
return 0, lost, False
|
||||
if native.requestedLateral and native.lateralAllowed:
|
||||
return 0, False, False
|
||||
if (previously_acknowledged and host_requested_lateral and
|
||||
not native.requestedLateral and not native.lateralAllowed):
|
||||
# A current host request lost an established grant, not a cold handshake.
|
||||
return 0, False, True
|
||||
pending_since_ns = pending_since_ns or now_ns
|
||||
return pending_since_ns, False, now_ns - pending_since_ns >= 200_000_000
|
||||
|
||||
@@ -5,7 +5,7 @@ import pytest
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint, structs
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.stock_aol import CLASSIC_BOSCH, NIDEC, RADARLESS, qualified, temporary_restriction
|
||||
from opendbc.car.honda.stock_aol import CLASSIC_BOSCH, NIDEC, RADARLESS, qualified, temporary_restriction, native_observation
|
||||
from opendbc.car.honda.values import CAR, HondaFlags
|
||||
from openpilot.starpilot.aol.intent import AolSettings, AOL_TOGGLE
|
||||
from openpilot.starpilot.car.honda.aol import policy_for, create_intent, native_accepts_cp
|
||||
@@ -347,6 +347,37 @@ def test_actual_card_active_observation_requires_fresh_applied_command(monkeypat
|
||||
assert tick(previous=True, long=True)["standard_active"]
|
||||
assert intent.allowed_latch # Actual normal long activity also proves active engagement.
|
||||
|
||||
# The actual Card caller consumes the acknowledged native permission. Its
|
||||
# immediate rejection boundary remains strict; no host grace is extended.
|
||||
tick(enabled=False)
|
||||
cs = state(True)
|
||||
native = replace(native, requestedLateral=True, lateralAllowed=True,
|
||||
observedMonoTime=now + 10_000_000, validUntilMonoTime=now + 210_000_000)
|
||||
tick(enabled=False)
|
||||
assert not intent.allowed_latch # The previously active latch toggles OFF.
|
||||
cs = state(False)
|
||||
native = replace(native, observedMonoTime=now + 10_000_000)
|
||||
tick(enabled=False)
|
||||
cs = state(True)
|
||||
native = replace(native, observedMonoTime=now + 10_000_000)
|
||||
tick(enabled=False)
|
||||
assert intent.allowed_latch # Release then a new physical press toggles ON.
|
||||
cs = state(False)
|
||||
native = replace(native, observedMonoTime=now + 10_000_000)
|
||||
assert tick(enabled=False)["native_rejection_ns"] == 0
|
||||
assert intent.allowed_latch
|
||||
native = replace(native, lateralAllowed=False, observedMonoTime=now + 10_000_000)
|
||||
observed_native = native.observedMonoTime
|
||||
assert tick(enabled=False)["native_rejection_ns"] == observed_native
|
||||
assert not intent.allowed_latch # Native denial immediately revokes independent intent.
|
||||
native = replace(native, lateralAllowed=True, observedMonoTime=now + 10_000_000)
|
||||
assert tick(enabled=False)["native_rejection_ns"] == 0
|
||||
assert not intent.allowed_latch # Permission recovery alone cannot recreate a gesture.
|
||||
cs = state(True)
|
||||
native = replace(native, observedMonoTime=now + 10_000_000)
|
||||
tick(enabled=False)
|
||||
assert intent.allowed_latch # A genuinely new LKAS edge can arm again.
|
||||
|
||||
generic = AolCardIntent(intent.settings)
|
||||
host.aol_card_intent = generic
|
||||
|
||||
@@ -357,3 +388,97 @@ def test_actual_card_active_observation_requires_fresh_applied_command(monkeypat
|
||||
monkeypatch.setattr(generic, "update", generic_boundary)
|
||||
with pytest.raises(BoundaryReached):
|
||||
host.state_update()
|
||||
|
||||
|
||||
@pytest.mark.parametrize("acknowledged,requested,reset", ((False, True, False), (True, True, True),
|
||||
(True, False, False), (False, False, False)))
|
||||
def test_native_cold_pending_vs_requested_ack_loss(acknowledged, requested, reset):
|
||||
native = SimpleNamespace(requestedLateral=False, lateralAllowed=False)
|
||||
result = native_observation(native, latched=True, panda_ready=True, restricted=False,
|
||||
now_ns=1_000_000_000, pending_since_ns=0,
|
||||
previously_acknowledged=acknowledged, host_requested_lateral=requested)
|
||||
assert result == ((0, False, True) if reset else (1_000_000_000, False, False))
|
||||
if not reset:
|
||||
assert native_observation(native, latched=True, panda_ready=True, restricted=False,
|
||||
now_ns=1_200_000_000, pending_since_ns=result[0],
|
||||
previously_acknowledged=acknowledged, host_requested_lateral=requested) == (result[0], False, True)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("changed", ("cold", "session", "panda", "cp", "manual_off", "restriction",
|
||||
"software_pause", "host_withdrawal", "invalid_axis", "stale_axis",
|
||||
"replaced_producer", "older_native", "resuming_pause", "pending_pause", "requested_denial", "unpaired_event",
|
||||
"fault_event", "invalid_event", "stale_event", "none"))
|
||||
def test_actual_card_ack_loss_requires_current_requested_same_identity(changed):
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
|
||||
from openpilot.starpilot.aol.wire import SafetyState
|
||||
|
||||
cp = final_cp(CAR.HONDA_CIVIC_2022, alpha=True, hybrid=True)
|
||||
cp.alternativeExperience |= 32
|
||||
now = 2_000_000_000
|
||||
native = SafetyState(1, True, now, now + 200_000_000, int(cp.safetyConfigs[0].safetyModel.raw),
|
||||
cp.safetyConfigs[0].safetyParam, True, False, True, False, "panda", "axis")
|
||||
axis = SimpleNamespace(qualified=True, sessionId="axis", observedMonoTime=now, validUntilMonoTime=now+30_000_000,
|
||||
faultSessionId="axis", faultReason="none", faultEventMonoTime=now, sequence=1,
|
||||
nativeAcknowledged=True, desiredLateral=True, lateralActive=True, sourceCarStateMonoTime=now,
|
||||
intentProducerSessionId="producer")
|
||||
|
||||
class Samples(dict):
|
||||
pass
|
||||
sm = Samples(aolAxisState=axis, onroadEvents=[])
|
||||
sm.valid = sm.alive = sm.seen = {"aolAxisState": True, "onroadEvents": True}
|
||||
sm.logMonoTime = {"aolAxisState": now, "onroadEvents": now}
|
||||
host = Car.__new__(Car)
|
||||
host.CP, host.sm, host.slc_producer_session = cp, sm, "producer"
|
||||
host.aol_transport_feedback = TransportPauseFeedback(cp)
|
||||
kwargs = dict(latched=True, panda_ready=True, restricted=False, now_ns=now, pending_since_ns=0)
|
||||
assert host.aol_honda_native_observation(native, **kwargs) == (0, False, False)
|
||||
if changed == "cold":
|
||||
host._honda_aol_acknowledgment = None
|
||||
elif changed == "session":
|
||||
native = replace(native, axisSessionId="replacement")
|
||||
elif changed == "panda":
|
||||
native = replace(native, pandaSerial="replacement")
|
||||
elif changed == "cp":
|
||||
host.CP = final_cp(CAR.HONDA_CIVIC_2022, alpha=False, hybrid=True)
|
||||
elif changed == "manual_off":
|
||||
kwargs['latched'] = False
|
||||
elif changed == "restriction":
|
||||
kwargs['restricted'] = True
|
||||
elif changed == "software_pause":
|
||||
axis.faultReason = "availabilityPause"
|
||||
host.aol_transport_feedback.retention_qualified = True
|
||||
elif changed == "host_withdrawal":
|
||||
axis.desiredLateral = False
|
||||
elif changed == "invalid_axis":
|
||||
sm.valid = {"aolAxisState": False}
|
||||
elif changed == "stale_axis":
|
||||
kwargs['now_ns'] += 40_000_000
|
||||
elif changed == "replaced_producer":
|
||||
axis.intentProducerSessionId = "replacement"
|
||||
elif changed == "resuming_pause":
|
||||
axis.lateralActive = False
|
||||
elif changed == "pending_pause":
|
||||
host.aol_transport_feedback.awaiting = True
|
||||
elif changed == "unpaired_event":
|
||||
axis.faultEventMonoTime -= 1
|
||||
elif changed == "fault_event":
|
||||
sm["onroadEvents"] = [SimpleNamespace(immediateDisable=True, softDisable=False)]
|
||||
elif changed == "invalid_event":
|
||||
sm.valid = {"aolAxisState": True, "onroadEvents": False}
|
||||
elif changed == "stale_event":
|
||||
axis.faultEventMonoTime = sm.logMonoTime["onroadEvents"] = now-1_500_000_001
|
||||
elif changed == "older_native":
|
||||
native = replace(native, observedMonoTime=now-1)
|
||||
withdrawn = replace(native, requestedLateral=False, lateralAllowed=False)
|
||||
if changed == "requested_denial":
|
||||
withdrawn = replace(withdrawn, requestedLateral=True)
|
||||
result = host.aol_honda_native_observation(withdrawn, **kwargs)
|
||||
if changed == "none":
|
||||
assert result == (0, False, True)
|
||||
else:
|
||||
assert not result[2] # No unexpected-loss cause is inferred from ambiguous zero.
|
||||
if changed in ("manual_off", "restriction"):
|
||||
assert result == (0, False, False)
|
||||
if changed in ("cold", "session", "panda", "cp", "manual_off"):
|
||||
assert host._honda_aol_acknowledgment is None
|
||||
|
||||
@@ -2,6 +2,8 @@
|
||||
|
||||
static bool honda_stock_aol_enabled = false;
|
||||
static bool honda_stock_aol_token = false;
|
||||
static bool honda_stock_aol_pending = false;
|
||||
static uint32_t honda_stock_aol_grant_ts = 0U;
|
||||
static bool honda_stock_aol_main_neutral = false;
|
||||
static bool honda_stock_aol_lkas_neutral = false;
|
||||
static bool honda_stock_aol_lkas_previous = false;
|
||||
@@ -18,10 +20,20 @@ static RxCheck honda_rx_workspace[7];
|
||||
|
||||
static void honda_stock_aol_clear(void) {
|
||||
honda_stock_aol_token = false;
|
||||
honda_stock_aol_pending = false;
|
||||
honda_stock_aol_main_neutral = false;
|
||||
honda_stock_aol_lkas_neutral = false;
|
||||
}
|
||||
|
||||
// A new physical grant may precede the host engagement heartbeat. It grants no output
|
||||
// until an engaged heartbeat and a fresh host request acknowledge this gesture.
|
||||
static void honda_stock_aol_grant(void) {
|
||||
honda_stock_aol_token = true;
|
||||
honda_stock_aol_pending = !heartbeat_engaged;
|
||||
honda_stock_aol_grant_ts = microsecond_timer_get();
|
||||
honda_stock_aol_session = heartbeat_engaged;
|
||||
}
|
||||
|
||||
static void honda_stock_aol_reset(void) {
|
||||
honda_stock_aol_clear();
|
||||
honda_stock_aol_enabled = false;
|
||||
@@ -55,14 +67,25 @@ static bool honda_stock_aol_sources_current(void) {
|
||||
|
||||
static uint8_t honda_stock_aol_request_mask(void) {
|
||||
uint8_t request = 0U;
|
||||
const uint32_t now = microsecond_timer_get();
|
||||
const uint32_t grant_age = safety_get_ts_elapsed(now, honda_stock_aol_grant_ts);
|
||||
const bool pending_expired = honda_stock_aol_pending && (grant_age > AOL_HOST_REQUEST_TIMEOUT_US);
|
||||
const bool expired = honda_stock_aol_request_seen &&
|
||||
(safety_get_ts_elapsed(microsecond_timer_get(), aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US);
|
||||
if (relay_malfunction || expired || safety_rx_checks_invalid ||
|
||||
(honda_stock_aol_session && (!heartbeat_engaged || !aol_rx_healthy() || !honda_stock_aol_sources_current()))) {
|
||||
(safety_get_ts_elapsed(now, aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US);
|
||||
if (relay_malfunction || expired || pending_expired || safety_rx_checks_invalid ||
|
||||
(honda_stock_aol_session && !heartbeat_engaged) ||
|
||||
((honda_stock_aol_session || honda_stock_aol_pending) &&
|
||||
(!aol_rx_healthy() || !honda_stock_aol_sources_current()))) {
|
||||
honda_stock_aol_clear();
|
||||
honda_stock_aol_session = false;
|
||||
} else if (heartbeat_engaged && aol_rx_healthy() && honda_stock_aol_sources_current()) {
|
||||
honda_stock_aol_session = true;
|
||||
request = aol_host_axis_mask;
|
||||
const bool pending_ack = !honda_stock_aol_pending || ((aol_host_axis_mask != 0U) &&
|
||||
(safety_get_ts_elapsed(now, aol_host_request_ts) <= grant_age));
|
||||
if (pending_ack) {
|
||||
honda_stock_aol_pending = false;
|
||||
honda_stock_aol_session = true;
|
||||
request = aol_host_axis_mask;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
return request;
|
||||
@@ -94,6 +117,13 @@ static bool honda_stock_aol_selected(const CANPacket_t *msg, const RxCheck *chec
|
||||
|
||||
static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool normal_engagement) {
|
||||
if (honda_stock_aol_enabled) {
|
||||
// Retire the old session before interpreting a new validated physical edge.
|
||||
// The following gesture may create a bounded pending grant; a status read
|
||||
// must not mistake that new grant for the retired session.
|
||||
if (honda_stock_aol_session && !heartbeat_engaged) {
|
||||
honda_stock_aol_clear();
|
||||
honda_stock_aol_session = false;
|
||||
}
|
||||
const unsigned int main_addr = honda_stock_aol_alt_main ? 0x1A6U : 0x326U;
|
||||
const unsigned int buttons_addr = honda_stock_aol_alt_main ? 0x1A6U : 0x296U;
|
||||
const unsigned int buttons_len = honda_stock_aol_alt_main ? 8U : 4U;
|
||||
@@ -104,7 +134,7 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool
|
||||
honda_stock_aol_clear();
|
||||
honda_stock_aol_main_neutral = true;
|
||||
} else if (honda_stock_aol_main_neutral) {
|
||||
honda_stock_aol_token = true;
|
||||
honda_stock_aol_grant();
|
||||
honda_stock_aol_main_neutral = false;
|
||||
} else {
|
||||
}
|
||||
@@ -115,7 +145,7 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool
|
||||
if (!lkas) {
|
||||
honda_stock_aol_lkas_neutral = true;
|
||||
} else if (honda_stock_aol_lkas_neutral && !honda_stock_aol_lkas_previous && acc_main_on) {
|
||||
honda_stock_aol_token = true;
|
||||
honda_stock_aol_grant();
|
||||
honda_stock_aol_lkas_neutral = false;
|
||||
} else {
|
||||
}
|
||||
@@ -152,10 +182,9 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool
|
||||
(honda_stock_aol_eps == 3U) || (honda_stock_aol_eps == 4U) || (honda_stock_aol_eps == 6U);
|
||||
if (normal_engagement && engagement_source && controls_allowed && acc_main_on && honda_stock_aol_drive &&
|
||||
eps_known && !brake_pressed && !regen_braking && !steering_disengage &&
|
||||
heartbeat_engaged && !relay_malfunction && !safety_rx_checks_invalid && request_current &&
|
||||
!relay_malfunction && !safety_rx_checks_invalid && request_current &&
|
||||
aol_rx_healthy() && honda_stock_aol_sources_current()) {
|
||||
honda_stock_aol_token = true;
|
||||
honda_stock_aol_session = true;
|
||||
honda_stock_aol_grant();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -110,15 +110,13 @@ class TestHondaStockAol(unittest.TestCase):
|
||||
|
||||
def test_blocked_normal_edge_is_not_replayed_after_sources_recover(self):
|
||||
for case in CASES:
|
||||
for blocked in ("eps", "reverse", "heartbeat", "brake", "missing", "expired", "request_expired"):
|
||||
for blocked in ("eps", "reverse", "brake", "missing", "expired", "request_expired"):
|
||||
with self.subTest(case=case[0], blocked=blocked):
|
||||
self.configure(case)
|
||||
self.stream(eps=7 if blocked == "eps" else 0, gear=2 if blocked == "reverse" else None,
|
||||
brake=int(blocked == "brake"), omit="STEER_STATUS" if blocked == "missing" else None)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
if blocked == "heartbeat":
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
elif blocked in ("expired", "request_expired"):
|
||||
if blocked in ("expired", "request_expired"):
|
||||
self.safety.set_timer(1_300_001)
|
||||
if blocked == "expired":
|
||||
self.safety.aol_set_host_request(1)
|
||||
@@ -235,3 +233,120 @@ class TestHondaStockAol(unittest.TestCase):
|
||||
self.stream(main=False)
|
||||
self.stream(main=True, lkas=1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
|
||||
def test_cold_valid_normal_grant_precedes_engaged_heartbeat(self):
|
||||
for case in CASES:
|
||||
for button in ((3, 4) if case[2] in (10, 11) else (3,)):
|
||||
with self.subTest(case=case[0], button=button):
|
||||
self.configure(case)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.stream(request=False)
|
||||
self.normal_engage(button=button)
|
||||
self.assertEqual(self.safety.aol_get_request_mask(), 0)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.set_timer(1_010_000)
|
||||
self.safety.aol_set_host_request(3)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 3 if self.scoped else 0)
|
||||
self.press_brake()
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0)
|
||||
|
||||
def test_prior_session_new_lkas_survives_status_before_request_and_heartbeat(self):
|
||||
for case in CASES:
|
||||
with self.subTest(case=case[0]):
|
||||
self.configure(case)
|
||||
self.stream()
|
||||
self.normal_engage()
|
||||
self.safety.aol_set_host_request(3)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 3 if self.scoped else 0)
|
||||
self.press_brake()
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.safety.aol_set_host_request(0)
|
||||
self.safety.set_timer(1_010_000)
|
||||
self.stream(cruise=1, request=False) # New neutral after old session loss.
|
||||
self.stream(cruise=1, lkas=1, request=False)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertEqual(self.safety.aol_get_request_mask(), 0) # Actual first status order.
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.set_timer(1_020_000)
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.assertEqual(self.safety.aol_get_request_mask(), 1 if self.scoped else 0)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Active grant still revoked.
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.stream(cruise=1, lkas=1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Held button cannot replay.
|
||||
|
||||
def test_pending_physical_grant_requires_post_gesture_request(self):
|
||||
for case in CASES:
|
||||
with self.subTest(case=case[0]):
|
||||
self.configure(case)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.stream()
|
||||
self.safety.set_timer(1_010_000)
|
||||
self.stream(lkas=1, request=False)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Previous request is insufficient.
|
||||
self.safety.set_timer(1_020_000)
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0)
|
||||
|
||||
def test_expired_pending_heartbeat_and_sources_require_new_gesture(self):
|
||||
for case in CASES:
|
||||
for loss in ("heartbeat_expired", "eps_source", "gear_source", "main_source", "host_expired", "relay", "eps_invalid", "rx_checksum"):
|
||||
with self.subTest(case=case[0], loss=loss):
|
||||
self.configure(case)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.stream(request=False)
|
||||
self.normal_engage()
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.aol_set_host_request(0)
|
||||
if loss in ("heartbeat_expired", "eps_source", "gear_source", "main_source", "host_expired"):
|
||||
self.safety.set_timer(1_300_001)
|
||||
source = {"eps_source": "STEER_STATUS", "gear_source": case[6],
|
||||
"main_source": "SCM_BUTTONS" if self.alt else "SCM_FEEDBACK"}.get(loss)
|
||||
self.stream(cruise=1, omit=source, request=False)
|
||||
if loss != "host_expired":
|
||||
self.safety.aol_set_host_request(0)
|
||||
elif loss == "relay":
|
||||
self.safety.set_relay_malfunction(True)
|
||||
elif loss == "rx_checksum":
|
||||
invalid = self.packet("POWERTRAIN_DATA", {"ACC_STATUS": 1})
|
||||
invalid.data[7] ^= 1
|
||||
self.assertFalse(self.safety.safety_rx_hook(invalid))
|
||||
else:
|
||||
self.stream(cruise=1, eps=7, request=False)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.set_relay_malfunction(False)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.stream(cruise=1)
|
||||
self.press_brake()
|
||||
self.safety.aol_set_host_request(3)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.stream(cruise=0)
|
||||
self.normal_engage()
|
||||
self.press_brake()
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0)
|
||||
|
||||
def test_pending_grant_clock_wrap_and_deadline(self):
|
||||
for case in CASES:
|
||||
for delay in (100_000, 300_000, 300_001):
|
||||
with self.subTest(case=case[0], delay=delay):
|
||||
self.configure(case)
|
||||
self.safety.set_timer(0xffff_0000)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.stream(request=False)
|
||||
self.stream(lkas=1, request=False)
|
||||
self.safety.set_timer((0xffff_0000 + delay) & 0xffff_ffff)
|
||||
self.stream(lkas=1, request=False) # Actual fresh CAN, held level, no new edge.
|
||||
self.safety.aol_set_host_request(0)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(),
|
||||
int(self.scoped and delay <= 300_000))
|
||||
|
||||
@@ -35,7 +35,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
|
||||
from openpilot.selfdrive.car.cruise import VCruiseHelper, SlcPendingConfirmation
|
||||
from openpilot.starpilot.speed_limits import physical_actions as slc_physical
|
||||
from openpilot.starpilot.aol.intent import AolProcessFaultContext, independent_axis_requested, read_settings
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, cp_identity
|
||||
from opendbc.car.honda.stock_aol import (
|
||||
qualified as qualified_honda_stock_aol, native_observation as honda_native_observation,
|
||||
temporary_restriction as honda_temporary_restriction,
|
||||
@@ -55,7 +55,7 @@ from opendbc.car.hyundai.canfd_angle_aol import native_observation, temporary_re
|
||||
from opendbc.car.hyundai.canfd_owner import config_index as hyundai_canfd_config_index
|
||||
from opendbc.car.hyundai.captured_adrv_startup import CapturedADRVStartup
|
||||
from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config
|
||||
from openpilot.starpilot.aol.runtime import current_native
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_native
|
||||
from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for
|
||||
from openpilot.starpilot.aol.wire import IntentState, encode_intent
|
||||
from openpilot.starpilot.conditional_mode.manual import Button, ButtonTracker, IoniqMediaMapCache, WheelMapCache, Press, ioniq6_media_eligible
|
||||
@@ -460,7 +460,8 @@ class Car:
|
||||
CS = self.CI.update(can_list)
|
||||
self.timing_mark('interface_update_end')
|
||||
cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME)
|
||||
if getattr(self.CI.CS, 'ev6_aol_source_healthy', False) else 0)
|
||||
if (getattr(self.CI.CS, 'ev6_aol_source_healthy', False) or
|
||||
qualified_honda_stock_aol(self.CP, marked_only=True)) else 0)
|
||||
self.observe_ioniq6_long_authority(can_list, CS)
|
||||
self.timing_mark('ioniq_authority_end')
|
||||
media_owner = getattr(self, 'ioniq6_media', None)
|
||||
@@ -532,6 +533,7 @@ class Car:
|
||||
self.aol_transport_feedback = TransportPauseFeedback(self.CP)
|
||||
transport_reset = self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
|
||||
latched=self.aol_card_intent.allowed_latch, cancel_source_ns=self.aol_cancel_source(CS, now_ns),
|
||||
brake_source_ns=self.aol_honda_brake_source(CS, now_ns),
|
||||
intent_producer_session=getattr(self, 'slc_producer_session', ''))
|
||||
fault_active = True if transport_reset else None
|
||||
event_ns = int(self.sm.logMonoTime['onroadEvents'])
|
||||
@@ -549,7 +551,7 @@ class Car:
|
||||
)
|
||||
native_reset = False
|
||||
if permission_owner:
|
||||
observe = (preap_native_observation if preap_aol else mazda_native_observation if mazda_aol else honda_native_observation if honda_aol else
|
||||
observe = (preap_native_observation if preap_aol else mazda_native_observation if mazda_aol else self.aol_honda_native_observation if honda_aol else
|
||||
ford_native_observation if ford_aol else native_observation)
|
||||
restrict = (preap_temporary_restriction if preap_aol else mazda_temporary_restriction if mazda_aol else honda_temporary_restriction if honda_aol else
|
||||
ford_temporary_restriction if ford_aol else temporary_restriction)
|
||||
@@ -738,13 +740,35 @@ class Car:
|
||||
return 0
|
||||
return received_ns
|
||||
|
||||
def aol_honda_brake_source(self, CS, now_ns: int) -> int:
|
||||
observation = getattr(self, '_aol_can_observation', None)
|
||||
if (not CS.brakePressed or not qualified_honda_stock_aol(self.CP, marked_only=True) or
|
||||
observation is None or observation[0] is not CS):
|
||||
return 0
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.honda.values import HondaFlags
|
||||
parser = self.CI.can_parsers[Bus.pt]
|
||||
message = 'BRAKE_MODULE' if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA'
|
||||
signals = parser.vl.get(message, {})
|
||||
# Match the real parser's applied-brake or two-sample switch decision.
|
||||
brake = bool(signals.get('BRAKE_PRESSED', 0) or message == 'POWERTRAIN_DATA' and
|
||||
signals.get('BRAKE_SWITCH', 0) and self.CI.CS.brake_switch_active)
|
||||
stamp = int(parser.ts_nanos.get(message, {}).get('BRAKE_PRESSED', 0))
|
||||
_, received_ns, can_source_ns, boot_ns = observation
|
||||
# CANParser/CAN batch use BOOTTIME; retain the original MONOTONIC receipt.
|
||||
if (not brake or not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000 or
|
||||
not 0 < stamp <= can_source_ns <= boot_ns or boot_ns - stamp > 30_000_000 or
|
||||
can_source_ns - stamp > 30_000_000):
|
||||
return 0
|
||||
return received_ns
|
||||
|
||||
def aol_disarming_fault(self, CS, event_ns: int, now_ns: int) -> bool:
|
||||
from openpilot.starpilot.car.gm.aol import intent_disarming_fault
|
||||
if getattr(self, 'aol_transport_feedback', None) is None:
|
||||
self.aol_transport_feedback = TransportPauseFeedback(self.CP)
|
||||
if self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
|
||||
latched=getattr(getattr(self, 'aol_card_intent', None), 'allowed_latch', True),
|
||||
cancel_source_ns=self.aol_cancel_source(CS, now_ns),
|
||||
cancel_source_ns=self.aol_cancel_source(CS, now_ns), brake_source_ns=self.aol_honda_brake_source(CS, now_ns),
|
||||
intent_producer_session=getattr(self, 'slc_producer_session', '')):
|
||||
return True
|
||||
events = self.sm['onroadEvents']
|
||||
@@ -767,6 +791,44 @@ class Car:
|
||||
cached = self._aol_owner_cache = (self.CP, owners)
|
||||
return cached[1]
|
||||
|
||||
def aol_honda_native_observation(self, native, *, latched, panda_ready, restricted, now_ns, pending_since_ns):
|
||||
acknowledged = getattr(self, '_honda_aol_acknowledgment', None)
|
||||
key = None
|
||||
host_requested = False
|
||||
if native is not None and qualified_honda_stock_aol(self.CP, marked_only=True):
|
||||
key = (cp_identity(self.CP), str(native.axisSessionId), str(native.pandaSerial),
|
||||
tuple((p.hardwareSerial, p.safetyModel, p.safetyParam) for p in native.pandaInventory))
|
||||
try:
|
||||
axis = current_axis(self.sm, now_ns=now_ns)
|
||||
feedback = self.aol_transport_feedback
|
||||
host_requested = bool(axis is not None and axis.sessionId == native.axisSessionId and
|
||||
axis.observedMonoTime == self.sm.logMonoTime['aolAxisState'] and
|
||||
axis.validUntilMonoTime == axis.observedMonoTime + AXIS_MAX_AGE_NS and
|
||||
axis.faultSessionId == axis.sessionId and str(axis.faultReason) == 'none' and
|
||||
self.sm.seen['onroadEvents'] and self.sm.valid['onroadEvents'] and self.sm.alive['onroadEvents'] and
|
||||
0 < axis.faultEventMonoTime == self.sm.logMonoTime['onroadEvents'] <= axis.observedMonoTime and
|
||||
now_ns - axis.faultEventMonoTime <= 1_500_000_000 and
|
||||
not any(event.immediateDisable or event.softDisable for event in self.sm['onroadEvents']) and
|
||||
axis.sequence > 0 and axis.nativeAcknowledged and axis.desiredLateral and axis.lateralActive and
|
||||
axis.intentProducerSessionId == self.slc_producer_session and
|
||||
0 < axis.sourceCarStateMonoTime <= native.observedMonoTime <= now_ns and
|
||||
now_ns - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS and
|
||||
not feedback.qualified and not feedback.retention_qualified and not feedback.paused and
|
||||
not feedback.awaiting and not feedback.retention_pending)
|
||||
except (AttributeError, KeyError, TypeError, ValueError):
|
||||
host_requested = False
|
||||
previously_acknowledged = bool(acknowledged is not None and acknowledged[0] == key and
|
||||
0 < acknowledged[1] <= native.observedMonoTime) if native is not None else False
|
||||
if not latched or not panda_ready or key is None:
|
||||
self._honda_aol_acknowledgment = None
|
||||
elif native.requestedLateral and native.lateralAllowed:
|
||||
self._honda_aol_acknowledgment = (key, int(native.observedMonoTime))
|
||||
elif acknowledged is not None and acknowledged[0] != key:
|
||||
self._honda_aol_acknowledgment = None
|
||||
return honda_native_observation(native, latched=latched, panda_ready=panda_ready, restricted=restricted,
|
||||
now_ns=now_ns, pending_since_ns=pending_since_ns,
|
||||
previously_acknowledged=previously_acknowledged, host_requested_lateral=host_requested)
|
||||
|
||||
def aol_axis_session(self) -> str:
|
||||
# pandad stamps aolSafetyWire with selfdrived's aolAxisState session, not card's producer session.
|
||||
seen = getattr(self.sm, 'seen', {})
|
||||
|
||||
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_5032200201446894984) {
|
||||
out_5032200201446894984[0] = delta_x[0] + nom_x[0];
|
||||
out_5032200201446894984[1] = delta_x[1] + nom_x[1];
|
||||
out_5032200201446894984[2] = delta_x[2] + nom_x[2];
|
||||
out_5032200201446894984[3] = delta_x[3] + nom_x[3];
|
||||
out_5032200201446894984[4] = delta_x[4] + nom_x[4];
|
||||
out_5032200201446894984[5] = delta_x[5] + nom_x[5];
|
||||
out_5032200201446894984[6] = delta_x[6] + nom_x[6];
|
||||
out_5032200201446894984[7] = delta_x[7] + nom_x[7];
|
||||
out_5032200201446894984[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398) {
|
||||
out_1832524583205975398[0] = delta_x[0] + nom_x[0];
|
||||
out_1832524583205975398[1] = delta_x[1] + nom_x[1];
|
||||
out_1832524583205975398[2] = delta_x[2] + nom_x[2];
|
||||
out_1832524583205975398[3] = delta_x[3] + nom_x[3];
|
||||
out_1832524583205975398[4] = delta_x[4] + nom_x[4];
|
||||
out_1832524583205975398[5] = delta_x[5] + nom_x[5];
|
||||
out_1832524583205975398[6] = delta_x[6] + nom_x[6];
|
||||
out_1832524583205975398[7] = delta_x[7] + nom_x[7];
|
||||
out_1832524583205975398[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280) {
|
||||
out_7143447445886649280[0] = -nom_x[0] + true_x[0];
|
||||
out_7143447445886649280[1] = -nom_x[1] + true_x[1];
|
||||
out_7143447445886649280[2] = -nom_x[2] + true_x[2];
|
||||
out_7143447445886649280[3] = -nom_x[3] + true_x[3];
|
||||
out_7143447445886649280[4] = -nom_x[4] + true_x[4];
|
||||
out_7143447445886649280[5] = -nom_x[5] + true_x[5];
|
||||
out_7143447445886649280[6] = -nom_x[6] + true_x[6];
|
||||
out_7143447445886649280[7] = -nom_x[7] + true_x[7];
|
||||
out_7143447445886649280[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486) {
|
||||
out_970452396869078486[0] = -nom_x[0] + true_x[0];
|
||||
out_970452396869078486[1] = -nom_x[1] + true_x[1];
|
||||
out_970452396869078486[2] = -nom_x[2] + true_x[2];
|
||||
out_970452396869078486[3] = -nom_x[3] + true_x[3];
|
||||
out_970452396869078486[4] = -nom_x[4] + true_x[4];
|
||||
out_970452396869078486[5] = -nom_x[5] + true_x[5];
|
||||
out_970452396869078486[6] = -nom_x[6] + true_x[6];
|
||||
out_970452396869078486[7] = -nom_x[7] + true_x[7];
|
||||
out_970452396869078486[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_1685754253580569061) {
|
||||
out_1685754253580569061[0] = 1.0;
|
||||
out_1685754253580569061[1] = 0.0;
|
||||
out_1685754253580569061[2] = 0.0;
|
||||
out_1685754253580569061[3] = 0.0;
|
||||
out_1685754253580569061[4] = 0.0;
|
||||
out_1685754253580569061[5] = 0.0;
|
||||
out_1685754253580569061[6] = 0.0;
|
||||
out_1685754253580569061[7] = 0.0;
|
||||
out_1685754253580569061[8] = 0.0;
|
||||
out_1685754253580569061[9] = 0.0;
|
||||
out_1685754253580569061[10] = 1.0;
|
||||
out_1685754253580569061[11] = 0.0;
|
||||
out_1685754253580569061[12] = 0.0;
|
||||
out_1685754253580569061[13] = 0.0;
|
||||
out_1685754253580569061[14] = 0.0;
|
||||
out_1685754253580569061[15] = 0.0;
|
||||
out_1685754253580569061[16] = 0.0;
|
||||
out_1685754253580569061[17] = 0.0;
|
||||
out_1685754253580569061[18] = 0.0;
|
||||
out_1685754253580569061[19] = 0.0;
|
||||
out_1685754253580569061[20] = 1.0;
|
||||
out_1685754253580569061[21] = 0.0;
|
||||
out_1685754253580569061[22] = 0.0;
|
||||
out_1685754253580569061[23] = 0.0;
|
||||
out_1685754253580569061[24] = 0.0;
|
||||
out_1685754253580569061[25] = 0.0;
|
||||
out_1685754253580569061[26] = 0.0;
|
||||
out_1685754253580569061[27] = 0.0;
|
||||
out_1685754253580569061[28] = 0.0;
|
||||
out_1685754253580569061[29] = 0.0;
|
||||
out_1685754253580569061[30] = 1.0;
|
||||
out_1685754253580569061[31] = 0.0;
|
||||
out_1685754253580569061[32] = 0.0;
|
||||
out_1685754253580569061[33] = 0.0;
|
||||
out_1685754253580569061[34] = 0.0;
|
||||
out_1685754253580569061[35] = 0.0;
|
||||
out_1685754253580569061[36] = 0.0;
|
||||
out_1685754253580569061[37] = 0.0;
|
||||
out_1685754253580569061[38] = 0.0;
|
||||
out_1685754253580569061[39] = 0.0;
|
||||
out_1685754253580569061[40] = 1.0;
|
||||
out_1685754253580569061[41] = 0.0;
|
||||
out_1685754253580569061[42] = 0.0;
|
||||
out_1685754253580569061[43] = 0.0;
|
||||
out_1685754253580569061[44] = 0.0;
|
||||
out_1685754253580569061[45] = 0.0;
|
||||
out_1685754253580569061[46] = 0.0;
|
||||
out_1685754253580569061[47] = 0.0;
|
||||
out_1685754253580569061[48] = 0.0;
|
||||
out_1685754253580569061[49] = 0.0;
|
||||
out_1685754253580569061[50] = 1.0;
|
||||
out_1685754253580569061[51] = 0.0;
|
||||
out_1685754253580569061[52] = 0.0;
|
||||
out_1685754253580569061[53] = 0.0;
|
||||
out_1685754253580569061[54] = 0.0;
|
||||
out_1685754253580569061[55] = 0.0;
|
||||
out_1685754253580569061[56] = 0.0;
|
||||
out_1685754253580569061[57] = 0.0;
|
||||
out_1685754253580569061[58] = 0.0;
|
||||
out_1685754253580569061[59] = 0.0;
|
||||
out_1685754253580569061[60] = 1.0;
|
||||
out_1685754253580569061[61] = 0.0;
|
||||
out_1685754253580569061[62] = 0.0;
|
||||
out_1685754253580569061[63] = 0.0;
|
||||
out_1685754253580569061[64] = 0.0;
|
||||
out_1685754253580569061[65] = 0.0;
|
||||
out_1685754253580569061[66] = 0.0;
|
||||
out_1685754253580569061[67] = 0.0;
|
||||
out_1685754253580569061[68] = 0.0;
|
||||
out_1685754253580569061[69] = 0.0;
|
||||
out_1685754253580569061[70] = 1.0;
|
||||
out_1685754253580569061[71] = 0.0;
|
||||
out_1685754253580569061[72] = 0.0;
|
||||
out_1685754253580569061[73] = 0.0;
|
||||
out_1685754253580569061[74] = 0.0;
|
||||
out_1685754253580569061[75] = 0.0;
|
||||
out_1685754253580569061[76] = 0.0;
|
||||
out_1685754253580569061[77] = 0.0;
|
||||
out_1685754253580569061[78] = 0.0;
|
||||
out_1685754253580569061[79] = 0.0;
|
||||
out_1685754253580569061[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4593964073461568413) {
|
||||
out_4593964073461568413[0] = 1.0;
|
||||
out_4593964073461568413[1] = 0.0;
|
||||
out_4593964073461568413[2] = 0.0;
|
||||
out_4593964073461568413[3] = 0.0;
|
||||
out_4593964073461568413[4] = 0.0;
|
||||
out_4593964073461568413[5] = 0.0;
|
||||
out_4593964073461568413[6] = 0.0;
|
||||
out_4593964073461568413[7] = 0.0;
|
||||
out_4593964073461568413[8] = 0.0;
|
||||
out_4593964073461568413[9] = 0.0;
|
||||
out_4593964073461568413[10] = 1.0;
|
||||
out_4593964073461568413[11] = 0.0;
|
||||
out_4593964073461568413[12] = 0.0;
|
||||
out_4593964073461568413[13] = 0.0;
|
||||
out_4593964073461568413[14] = 0.0;
|
||||
out_4593964073461568413[15] = 0.0;
|
||||
out_4593964073461568413[16] = 0.0;
|
||||
out_4593964073461568413[17] = 0.0;
|
||||
out_4593964073461568413[18] = 0.0;
|
||||
out_4593964073461568413[19] = 0.0;
|
||||
out_4593964073461568413[20] = 1.0;
|
||||
out_4593964073461568413[21] = 0.0;
|
||||
out_4593964073461568413[22] = 0.0;
|
||||
out_4593964073461568413[23] = 0.0;
|
||||
out_4593964073461568413[24] = 0.0;
|
||||
out_4593964073461568413[25] = 0.0;
|
||||
out_4593964073461568413[26] = 0.0;
|
||||
out_4593964073461568413[27] = 0.0;
|
||||
out_4593964073461568413[28] = 0.0;
|
||||
out_4593964073461568413[29] = 0.0;
|
||||
out_4593964073461568413[30] = 1.0;
|
||||
out_4593964073461568413[31] = 0.0;
|
||||
out_4593964073461568413[32] = 0.0;
|
||||
out_4593964073461568413[33] = 0.0;
|
||||
out_4593964073461568413[34] = 0.0;
|
||||
out_4593964073461568413[35] = 0.0;
|
||||
out_4593964073461568413[36] = 0.0;
|
||||
out_4593964073461568413[37] = 0.0;
|
||||
out_4593964073461568413[38] = 0.0;
|
||||
out_4593964073461568413[39] = 0.0;
|
||||
out_4593964073461568413[40] = 1.0;
|
||||
out_4593964073461568413[41] = 0.0;
|
||||
out_4593964073461568413[42] = 0.0;
|
||||
out_4593964073461568413[43] = 0.0;
|
||||
out_4593964073461568413[44] = 0.0;
|
||||
out_4593964073461568413[45] = 0.0;
|
||||
out_4593964073461568413[46] = 0.0;
|
||||
out_4593964073461568413[47] = 0.0;
|
||||
out_4593964073461568413[48] = 0.0;
|
||||
out_4593964073461568413[49] = 0.0;
|
||||
out_4593964073461568413[50] = 1.0;
|
||||
out_4593964073461568413[51] = 0.0;
|
||||
out_4593964073461568413[52] = 0.0;
|
||||
out_4593964073461568413[53] = 0.0;
|
||||
out_4593964073461568413[54] = 0.0;
|
||||
out_4593964073461568413[55] = 0.0;
|
||||
out_4593964073461568413[56] = 0.0;
|
||||
out_4593964073461568413[57] = 0.0;
|
||||
out_4593964073461568413[58] = 0.0;
|
||||
out_4593964073461568413[59] = 0.0;
|
||||
out_4593964073461568413[60] = 1.0;
|
||||
out_4593964073461568413[61] = 0.0;
|
||||
out_4593964073461568413[62] = 0.0;
|
||||
out_4593964073461568413[63] = 0.0;
|
||||
out_4593964073461568413[64] = 0.0;
|
||||
out_4593964073461568413[65] = 0.0;
|
||||
out_4593964073461568413[66] = 0.0;
|
||||
out_4593964073461568413[67] = 0.0;
|
||||
out_4593964073461568413[68] = 0.0;
|
||||
out_4593964073461568413[69] = 0.0;
|
||||
out_4593964073461568413[70] = 1.0;
|
||||
out_4593964073461568413[71] = 0.0;
|
||||
out_4593964073461568413[72] = 0.0;
|
||||
out_4593964073461568413[73] = 0.0;
|
||||
out_4593964073461568413[74] = 0.0;
|
||||
out_4593964073461568413[75] = 0.0;
|
||||
out_4593964073461568413[76] = 0.0;
|
||||
out_4593964073461568413[77] = 0.0;
|
||||
out_4593964073461568413[78] = 0.0;
|
||||
out_4593964073461568413[79] = 0.0;
|
||||
out_4593964073461568413[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_8909179533363476452) {
|
||||
out_8909179533363476452[0] = state[0];
|
||||
out_8909179533363476452[1] = state[1];
|
||||
out_8909179533363476452[2] = state[2];
|
||||
out_8909179533363476452[3] = state[3];
|
||||
out_8909179533363476452[4] = state[4];
|
||||
out_8909179533363476452[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_8909179533363476452[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_8909179533363476452[7] = state[7];
|
||||
out_8909179533363476452[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_231308644161984314) {
|
||||
out_231308644161984314[0] = state[0];
|
||||
out_231308644161984314[1] = state[1];
|
||||
out_231308644161984314[2] = state[2];
|
||||
out_231308644161984314[3] = state[3];
|
||||
out_231308644161984314[4] = state[4];
|
||||
out_231308644161984314[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_231308644161984314[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_231308644161984314[7] = state[7];
|
||||
out_231308644161984314[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_3419672683889849262) {
|
||||
out_3419672683889849262[0] = 1;
|
||||
out_3419672683889849262[1] = 0;
|
||||
out_3419672683889849262[2] = 0;
|
||||
out_3419672683889849262[3] = 0;
|
||||
out_3419672683889849262[4] = 0;
|
||||
out_3419672683889849262[5] = 0;
|
||||
out_3419672683889849262[6] = 0;
|
||||
out_3419672683889849262[7] = 0;
|
||||
out_3419672683889849262[8] = 0;
|
||||
out_3419672683889849262[9] = 0;
|
||||
out_3419672683889849262[10] = 1;
|
||||
out_3419672683889849262[11] = 0;
|
||||
out_3419672683889849262[12] = 0;
|
||||
out_3419672683889849262[13] = 0;
|
||||
out_3419672683889849262[14] = 0;
|
||||
out_3419672683889849262[15] = 0;
|
||||
out_3419672683889849262[16] = 0;
|
||||
out_3419672683889849262[17] = 0;
|
||||
out_3419672683889849262[18] = 0;
|
||||
out_3419672683889849262[19] = 0;
|
||||
out_3419672683889849262[20] = 1;
|
||||
out_3419672683889849262[21] = 0;
|
||||
out_3419672683889849262[22] = 0;
|
||||
out_3419672683889849262[23] = 0;
|
||||
out_3419672683889849262[24] = 0;
|
||||
out_3419672683889849262[25] = 0;
|
||||
out_3419672683889849262[26] = 0;
|
||||
out_3419672683889849262[27] = 0;
|
||||
out_3419672683889849262[28] = 0;
|
||||
out_3419672683889849262[29] = 0;
|
||||
out_3419672683889849262[30] = 1;
|
||||
out_3419672683889849262[31] = 0;
|
||||
out_3419672683889849262[32] = 0;
|
||||
out_3419672683889849262[33] = 0;
|
||||
out_3419672683889849262[34] = 0;
|
||||
out_3419672683889849262[35] = 0;
|
||||
out_3419672683889849262[36] = 0;
|
||||
out_3419672683889849262[37] = 0;
|
||||
out_3419672683889849262[38] = 0;
|
||||
out_3419672683889849262[39] = 0;
|
||||
out_3419672683889849262[40] = 1;
|
||||
out_3419672683889849262[41] = 0;
|
||||
out_3419672683889849262[42] = 0;
|
||||
out_3419672683889849262[43] = 0;
|
||||
out_3419672683889849262[44] = 0;
|
||||
out_3419672683889849262[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_3419672683889849262[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3419672683889849262[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3419672683889849262[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3419672683889849262[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_3419672683889849262[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3419672683889849262[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3419672683889849262[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3419672683889849262[53] = -9.8100000000000005*dt;
|
||||
out_3419672683889849262[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_3419672683889849262[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3419672683889849262[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3419672683889849262[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3419672683889849262[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_3419672683889849262[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3419672683889849262[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_3419672683889849262[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3419672683889849262[62] = 0;
|
||||
out_3419672683889849262[63] = 0;
|
||||
out_3419672683889849262[64] = 0;
|
||||
out_3419672683889849262[65] = 0;
|
||||
out_3419672683889849262[66] = 0;
|
||||
out_3419672683889849262[67] = 0;
|
||||
out_3419672683889849262[68] = 0;
|
||||
out_3419672683889849262[69] = 0;
|
||||
out_3419672683889849262[70] = 1;
|
||||
out_3419672683889849262[71] = 0;
|
||||
out_3419672683889849262[72] = 0;
|
||||
out_3419672683889849262[73] = 0;
|
||||
out_3419672683889849262[74] = 0;
|
||||
out_3419672683889849262[75] = 0;
|
||||
out_3419672683889849262[76] = 0;
|
||||
out_3419672683889849262[77] = 0;
|
||||
out_3419672683889849262[78] = 0;
|
||||
out_3419672683889849262[79] = 0;
|
||||
out_3419672683889849262[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2868289483219690466) {
|
||||
out_2868289483219690466[0] = 1;
|
||||
out_2868289483219690466[1] = 0;
|
||||
out_2868289483219690466[2] = 0;
|
||||
out_2868289483219690466[3] = 0;
|
||||
out_2868289483219690466[4] = 0;
|
||||
out_2868289483219690466[5] = 0;
|
||||
out_2868289483219690466[6] = 0;
|
||||
out_2868289483219690466[7] = 0;
|
||||
out_2868289483219690466[8] = 0;
|
||||
out_2868289483219690466[9] = 0;
|
||||
out_2868289483219690466[10] = 1;
|
||||
out_2868289483219690466[11] = 0;
|
||||
out_2868289483219690466[12] = 0;
|
||||
out_2868289483219690466[13] = 0;
|
||||
out_2868289483219690466[14] = 0;
|
||||
out_2868289483219690466[15] = 0;
|
||||
out_2868289483219690466[16] = 0;
|
||||
out_2868289483219690466[17] = 0;
|
||||
out_2868289483219690466[18] = 0;
|
||||
out_2868289483219690466[19] = 0;
|
||||
out_2868289483219690466[20] = 1;
|
||||
out_2868289483219690466[21] = 0;
|
||||
out_2868289483219690466[22] = 0;
|
||||
out_2868289483219690466[23] = 0;
|
||||
out_2868289483219690466[24] = 0;
|
||||
out_2868289483219690466[25] = 0;
|
||||
out_2868289483219690466[26] = 0;
|
||||
out_2868289483219690466[27] = 0;
|
||||
out_2868289483219690466[28] = 0;
|
||||
out_2868289483219690466[29] = 0;
|
||||
out_2868289483219690466[30] = 1;
|
||||
out_2868289483219690466[31] = 0;
|
||||
out_2868289483219690466[32] = 0;
|
||||
out_2868289483219690466[33] = 0;
|
||||
out_2868289483219690466[34] = 0;
|
||||
out_2868289483219690466[35] = 0;
|
||||
out_2868289483219690466[36] = 0;
|
||||
out_2868289483219690466[37] = 0;
|
||||
out_2868289483219690466[38] = 0;
|
||||
out_2868289483219690466[39] = 0;
|
||||
out_2868289483219690466[40] = 1;
|
||||
out_2868289483219690466[41] = 0;
|
||||
out_2868289483219690466[42] = 0;
|
||||
out_2868289483219690466[43] = 0;
|
||||
out_2868289483219690466[44] = 0;
|
||||
out_2868289483219690466[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_2868289483219690466[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2868289483219690466[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2868289483219690466[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2868289483219690466[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_2868289483219690466[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2868289483219690466[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2868289483219690466[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2868289483219690466[53] = -9.8100000000000005*dt;
|
||||
out_2868289483219690466[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_2868289483219690466[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2868289483219690466[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2868289483219690466[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2868289483219690466[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_2868289483219690466[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2868289483219690466[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_2868289483219690466[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2868289483219690466[62] = 0;
|
||||
out_2868289483219690466[63] = 0;
|
||||
out_2868289483219690466[64] = 0;
|
||||
out_2868289483219690466[65] = 0;
|
||||
out_2868289483219690466[66] = 0;
|
||||
out_2868289483219690466[67] = 0;
|
||||
out_2868289483219690466[68] = 0;
|
||||
out_2868289483219690466[69] = 0;
|
||||
out_2868289483219690466[70] = 1;
|
||||
out_2868289483219690466[71] = 0;
|
||||
out_2868289483219690466[72] = 0;
|
||||
out_2868289483219690466[73] = 0;
|
||||
out_2868289483219690466[74] = 0;
|
||||
out_2868289483219690466[75] = 0;
|
||||
out_2868289483219690466[76] = 0;
|
||||
out_2868289483219690466[77] = 0;
|
||||
out_2868289483219690466[78] = 0;
|
||||
out_2868289483219690466[79] = 0;
|
||||
out_2868289483219690466[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_5555793351249648290) {
|
||||
out_5555793351249648290[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_7797736608546404621) {
|
||||
out_7797736608546404621[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_4322311178595995255) {
|
||||
out_4322311178595995255[0] = 0;
|
||||
out_4322311178595995255[1] = 0;
|
||||
out_4322311178595995255[2] = 0;
|
||||
out_4322311178595995255[3] = 0;
|
||||
out_4322311178595995255[4] = 0;
|
||||
out_4322311178595995255[5] = 0;
|
||||
out_4322311178595995255[6] = 1;
|
||||
out_4322311178595995255[7] = 0;
|
||||
out_4322311178595995255[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_5931441967274123398) {
|
||||
out_5931441967274123398[0] = 0;
|
||||
out_5931441967274123398[1] = 0;
|
||||
out_5931441967274123398[2] = 0;
|
||||
out_5931441967274123398[3] = 0;
|
||||
out_5931441967274123398[4] = 0;
|
||||
out_5931441967274123398[5] = 0;
|
||||
out_5931441967274123398[6] = 1;
|
||||
out_5931441967274123398[7] = 0;
|
||||
out_5931441967274123398[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_8637534363345985562) {
|
||||
out_8637534363345985562[0] = state[4];
|
||||
out_8637534363345985562[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_9133422513623254856) {
|
||||
out_9133422513623254856[0] = state[4];
|
||||
out_9133422513623254856[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_2909282930557586418) {
|
||||
out_2909282930557586418[0] = 0;
|
||||
out_2909282930557586418[1] = 0;
|
||||
out_2909282930557586418[2] = 0;
|
||||
out_2909282930557586418[3] = 0;
|
||||
out_2909282930557586418[4] = 1;
|
||||
out_2909282930557586418[5] = 0;
|
||||
out_2909282930557586418[6] = 0;
|
||||
out_2909282930557586418[7] = 0;
|
||||
out_2909282930557586418[8] = 0;
|
||||
out_2909282930557586418[9] = 0;
|
||||
out_2909282930557586418[10] = 0;
|
||||
out_2909282930557586418[11] = 0;
|
||||
out_2909282930557586418[12] = 0;
|
||||
out_2909282930557586418[13] = 0;
|
||||
out_2909282930557586418[14] = 1;
|
||||
out_2909282930557586418[15] = 0;
|
||||
out_2909282930557586418[16] = 0;
|
||||
out_2909282930557586418[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_9101010594292373758) {
|
||||
out_9101010594292373758[0] = 0;
|
||||
out_9101010594292373758[1] = 0;
|
||||
out_9101010594292373758[2] = 0;
|
||||
out_9101010594292373758[3] = 0;
|
||||
out_9101010594292373758[4] = 1;
|
||||
out_9101010594292373758[5] = 0;
|
||||
out_9101010594292373758[6] = 0;
|
||||
out_9101010594292373758[7] = 0;
|
||||
out_9101010594292373758[8] = 0;
|
||||
out_9101010594292373758[9] = 0;
|
||||
out_9101010594292373758[10] = 0;
|
||||
out_9101010594292373758[11] = 0;
|
||||
out_9101010594292373758[12] = 0;
|
||||
out_9101010594292373758[13] = 0;
|
||||
out_9101010594292373758[14] = 1;
|
||||
out_9101010594292373758[15] = 0;
|
||||
out_9101010594292373758[16] = 0;
|
||||
out_9101010594292373758[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_5127685465639230885) {
|
||||
out_5127685465639230885[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_2885742208342474554) {
|
||||
out_2885742208342474554[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_8850007508723603453) {
|
||||
out_8850007508723603453[0] = 0;
|
||||
out_8850007508723603453[1] = 0;
|
||||
out_8850007508723603453[2] = 0;
|
||||
out_8850007508723603453[3] = 0;
|
||||
out_8850007508723603453[4] = 1;
|
||||
out_8850007508723603453[5] = 0;
|
||||
out_8850007508723603453[6] = 0;
|
||||
out_8850007508723603453[7] = 0;
|
||||
out_8850007508723603453[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3413109008766874771) {
|
||||
out_3413109008766874771[0] = 0;
|
||||
out_3413109008766874771[1] = 0;
|
||||
out_3413109008766874771[2] = 0;
|
||||
out_3413109008766874771[3] = 0;
|
||||
out_3413109008766874771[4] = 1;
|
||||
out_3413109008766874771[5] = 0;
|
||||
out_3413109008766874771[6] = 0;
|
||||
out_3413109008766874771[7] = 0;
|
||||
out_3413109008766874771[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_4524369580962753368) {
|
||||
out_4524369580962753368[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4918137225453201081) {
|
||||
out_4918137225453201081[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_8063814497470051479) {
|
||||
out_8063814497470051479[0] = 0;
|
||||
out_8063814497470051479[1] = 0;
|
||||
out_8063814497470051479[2] = 0;
|
||||
out_8063814497470051479[3] = 0;
|
||||
out_8063814497470051479[4] = 0;
|
||||
out_8063814497470051479[5] = 0;
|
||||
out_8063814497470051479[6] = 0;
|
||||
out_8063814497470051479[7] = 1;
|
||||
out_8063814497470051479[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_8773798787561371994) {
|
||||
out_8773798787561371994[0] = 0;
|
||||
out_8773798787561371994[1] = 0;
|
||||
out_8773798787561371994[2] = 0;
|
||||
out_8773798787561371994[3] = 0;
|
||||
out_8773798787561371994[4] = 0;
|
||||
out_8773798787561371994[5] = 0;
|
||||
out_8773798787561371994[6] = 0;
|
||||
out_8773798787561371994[7] = 1;
|
||||
out_8773798787561371994[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_3908981550453062561) {
|
||||
out_3908981550453062561[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_8741246461448067056) {
|
||||
out_8741246461448067056[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_7421973253185523252) {
|
||||
out_7421973253185523252[0] = 0;
|
||||
out_7421973253185523252[1] = 0;
|
||||
out_7421973253185523252[2] = 0;
|
||||
out_7421973253185523252[3] = 1;
|
||||
out_7421973253185523252[4] = 0;
|
||||
out_7421973253185523252[5] = 0;
|
||||
out_7421973253185523252[6] = 0;
|
||||
out_7421973253185523252[7] = 0;
|
||||
out_7421973253185523252[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5587872320567299682) {
|
||||
out_5587872320567299682[0] = 0;
|
||||
out_5587872320567299682[1] = 0;
|
||||
out_5587872320567299682[2] = 0;
|
||||
out_5587872320567299682[3] = 1;
|
||||
out_5587872320567299682[4] = 0;
|
||||
out_5587872320567299682[5] = 0;
|
||||
out_5587872320567299682[6] = 0;
|
||||
out_5587872320567299682[7] = 0;
|
||||
out_5587872320567299682[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_3307563392839699007) {
|
||||
out_3307563392839699007[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_3097181630963667199) {
|
||||
out_3097181630963667199[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_8339776164409211269) {
|
||||
out_8339776164409211269[0] = 0;
|
||||
out_8339776164409211269[1] = 1;
|
||||
out_8339776164409211269[2] = 0;
|
||||
out_8339776164409211269[3] = 0;
|
||||
out_8339776164409211269[4] = 0;
|
||||
out_8339776164409211269[5] = 0;
|
||||
out_8339776164409211269[6] = 0;
|
||||
out_8339776164409211269[7] = 0;
|
||||
out_8339776164409211269[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_2902877664452482587) {
|
||||
out_2902877664452482587[0] = 0;
|
||||
out_2902877664452482587[1] = 1;
|
||||
out_2902877664452482587[2] = 0;
|
||||
out_2902877664452482587[3] = 0;
|
||||
out_2902877664452482587[4] = 0;
|
||||
out_2902877664452482587[5] = 0;
|
||||
out_2902877664452482587[6] = 0;
|
||||
out_2902877664452482587[7] = 0;
|
||||
out_2902877664452482587[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_8910654818113908206) {
|
||||
out_8910654818113908206[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6668711560817151875) {
|
||||
out_6668711560817151875[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_6376145892843885018) {
|
||||
out_6376145892843885018[0] = 1;
|
||||
out_6376145892843885018[1] = 0;
|
||||
out_6376145892843885018[2] = 0;
|
||||
out_6376145892843885018[3] = 0;
|
||||
out_6376145892843885018[4] = 0;
|
||||
out_6376145892843885018[5] = 0;
|
||||
out_6376145892843885018[6] = 0;
|
||||
out_6376145892843885018[7] = 0;
|
||||
out_6376145892843885018[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_7985276681522013161) {
|
||||
out_7985276681522013161[0] = 1;
|
||||
out_7985276681522013161[1] = 0;
|
||||
out_7985276681522013161[2] = 0;
|
||||
out_7985276681522013161[3] = 0;
|
||||
out_7985276681522013161[4] = 0;
|
||||
out_7985276681522013161[5] = 0;
|
||||
out_7985276681522013161[6] = 0;
|
||||
out_7985276681522013161[7] = 0;
|
||||
out_7985276681522013161[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_357939675421100083) {
|
||||
out_357939675421100083[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_3787573678896021836) {
|
||||
out_3787573678896021836[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_8690022599703402955) {
|
||||
out_8690022599703402955[0] = 0;
|
||||
out_8690022599703402955[1] = 0;
|
||||
out_8690022599703402955[2] = 0;
|
||||
out_8690022599703402955[3] = 0;
|
||||
out_8690022599703402955[4] = 0;
|
||||
out_8690022599703402955[5] = 0;
|
||||
out_8690022599703402955[6] = 0;
|
||||
out_8690022599703402955[7] = 0;
|
||||
out_8690022599703402955[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_8147590685328020518) {
|
||||
out_8147590685328020518[0] = 0;
|
||||
out_8147590685328020518[1] = 0;
|
||||
out_8147590685328020518[2] = 0;
|
||||
out_8147590685328020518[3] = 0;
|
||||
out_8147590685328020518[4] = 0;
|
||||
out_8147590685328020518[5] = 0;
|
||||
out_8147590685328020518[6] = 0;
|
||||
out_8147590685328020518[7] = 0;
|
||||
out_8147590685328020518[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_5032200201446894984) {
|
||||
err_fun(nom_x, delta_x, out_5032200201446894984);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398) {
|
||||
err_fun(nom_x, delta_x, out_1832524583205975398);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280) {
|
||||
inv_err_fun(nom_x, true_x, out_7143447445886649280);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486) {
|
||||
inv_err_fun(nom_x, true_x, out_970452396869078486);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_1685754253580569061) {
|
||||
H_mod_fun(state, out_1685754253580569061);
|
||||
void car_H_mod_fun(double *state, double *out_4593964073461568413) {
|
||||
H_mod_fun(state, out_4593964073461568413);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_8909179533363476452) {
|
||||
f_fun(state, dt, out_8909179533363476452);
|
||||
void car_f_fun(double *state, double dt, double *out_231308644161984314) {
|
||||
f_fun(state, dt, out_231308644161984314);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_3419672683889849262) {
|
||||
F_fun(state, dt, out_3419672683889849262);
|
||||
void car_F_fun(double *state, double dt, double *out_2868289483219690466) {
|
||||
F_fun(state, dt, out_2868289483219690466);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_5555793351249648290) {
|
||||
h_25(state, unused, out_5555793351249648290);
|
||||
void car_h_25(double *state, double *unused, double *out_7797736608546404621) {
|
||||
h_25(state, unused, out_7797736608546404621);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_4322311178595995255) {
|
||||
H_25(state, unused, out_4322311178595995255);
|
||||
void car_H_25(double *state, double *unused, double *out_5931441967274123398) {
|
||||
H_25(state, unused, out_5931441967274123398);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_8637534363345985562) {
|
||||
h_24(state, unused, out_8637534363345985562);
|
||||
void car_h_24(double *state, double *unused, double *out_9133422513623254856) {
|
||||
h_24(state, unused, out_9133422513623254856);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_2909282930557586418) {
|
||||
H_24(state, unused, out_2909282930557586418);
|
||||
void car_H_24(double *state, double *unused, double *out_9101010594292373758) {
|
||||
H_24(state, unused, out_9101010594292373758);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_5127685465639230885) {
|
||||
h_30(state, unused, out_5127685465639230885);
|
||||
void car_h_30(double *state, double *unused, double *out_2885742208342474554) {
|
||||
h_30(state, unused, out_2885742208342474554);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_8850007508723603453) {
|
||||
H_30(state, unused, out_8850007508723603453);
|
||||
void car_H_30(double *state, double *unused, double *out_3413109008766874771) {
|
||||
H_30(state, unused, out_3413109008766874771);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_4524369580962753368) {
|
||||
h_26(state, unused, out_4524369580962753368);
|
||||
void car_h_26(double *state, double *unused, double *out_4918137225453201081) {
|
||||
h_26(state, unused, out_4918137225453201081);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_8063814497470051479) {
|
||||
H_26(state, unused, out_8063814497470051479);
|
||||
void car_H_26(double *state, double *unused, double *out_8773798787561371994) {
|
||||
H_26(state, unused, out_8773798787561371994);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_3908981550453062561) {
|
||||
h_27(state, unused, out_3908981550453062561);
|
||||
void car_h_27(double *state, double *unused, double *out_8741246461448067056) {
|
||||
h_27(state, unused, out_8741246461448067056);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_7421973253185523252) {
|
||||
H_27(state, unused, out_7421973253185523252);
|
||||
void car_H_27(double *state, double *unused, double *out_5587872320567299682) {
|
||||
H_27(state, unused, out_5587872320567299682);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_3307563392839699007) {
|
||||
h_29(state, unused, out_3307563392839699007);
|
||||
void car_h_29(double *state, double *unused, double *out_3097181630963667199) {
|
||||
h_29(state, unused, out_3097181630963667199);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_8339776164409211269) {
|
||||
H_29(state, unused, out_8339776164409211269);
|
||||
void car_H_29(double *state, double *unused, double *out_2902877664452482587) {
|
||||
H_29(state, unused, out_2902877664452482587);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_8910654818113908206) {
|
||||
h_28(state, unused, out_8910654818113908206);
|
||||
void car_h_28(double *state, double *unused, double *out_6668711560817151875) {
|
||||
h_28(state, unused, out_6668711560817151875);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_6376145892843885018) {
|
||||
H_28(state, unused, out_6376145892843885018);
|
||||
void car_H_28(double *state, double *unused, double *out_7985276681522013161) {
|
||||
H_28(state, unused, out_7985276681522013161);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_357939675421100083) {
|
||||
h_31(state, unused, out_357939675421100083);
|
||||
void car_h_31(double *state, double *unused, double *out_3787573678896021836) {
|
||||
h_31(state, unused, out_3787573678896021836);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_8690022599703402955) {
|
||||
H_31(state, unused, out_8690022599703402955);
|
||||
void car_H_31(double *state, double *unused, double *out_8147590685328020518) {
|
||||
H_31(state, unused, out_8147590685328020518);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_5032200201446894984);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280);
|
||||
void car_H_mod_fun(double *state, double *out_1685754253580569061);
|
||||
void car_f_fun(double *state, double dt, double *out_8909179533363476452);
|
||||
void car_F_fun(double *state, double dt, double *out_3419672683889849262);
|
||||
void car_h_25(double *state, double *unused, double *out_5555793351249648290);
|
||||
void car_H_25(double *state, double *unused, double *out_4322311178595995255);
|
||||
void car_h_24(double *state, double *unused, double *out_8637534363345985562);
|
||||
void car_H_24(double *state, double *unused, double *out_2909282930557586418);
|
||||
void car_h_30(double *state, double *unused, double *out_5127685465639230885);
|
||||
void car_H_30(double *state, double *unused, double *out_8850007508723603453);
|
||||
void car_h_26(double *state, double *unused, double *out_4524369580962753368);
|
||||
void car_H_26(double *state, double *unused, double *out_8063814497470051479);
|
||||
void car_h_27(double *state, double *unused, double *out_3908981550453062561);
|
||||
void car_H_27(double *state, double *unused, double *out_7421973253185523252);
|
||||
void car_h_29(double *state, double *unused, double *out_3307563392839699007);
|
||||
void car_H_29(double *state, double *unused, double *out_8339776164409211269);
|
||||
void car_h_28(double *state, double *unused, double *out_8910654818113908206);
|
||||
void car_H_28(double *state, double *unused, double *out_6376145892843885018);
|
||||
void car_h_31(double *state, double *unused, double *out_357939675421100083);
|
||||
void car_H_31(double *state, double *unused, double *out_8690022599703402955);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486);
|
||||
void car_H_mod_fun(double *state, double *out_4593964073461568413);
|
||||
void car_f_fun(double *state, double dt, double *out_231308644161984314);
|
||||
void car_F_fun(double *state, double dt, double *out_2868289483219690466);
|
||||
void car_h_25(double *state, double *unused, double *out_7797736608546404621);
|
||||
void car_H_25(double *state, double *unused, double *out_5931441967274123398);
|
||||
void car_h_24(double *state, double *unused, double *out_9133422513623254856);
|
||||
void car_H_24(double *state, double *unused, double *out_9101010594292373758);
|
||||
void car_h_30(double *state, double *unused, double *out_2885742208342474554);
|
||||
void car_H_30(double *state, double *unused, double *out_3413109008766874771);
|
||||
void car_h_26(double *state, double *unused, double *out_4918137225453201081);
|
||||
void car_H_26(double *state, double *unused, double *out_8773798787561371994);
|
||||
void car_h_27(double *state, double *unused, double *out_8741246461448067056);
|
||||
void car_H_27(double *state, double *unused, double *out_5587872320567299682);
|
||||
void car_h_29(double *state, double *unused, double *out_3097181630963667199);
|
||||
void car_H_29(double *state, double *unused, double *out_2902877664452482587);
|
||||
void car_h_28(double *state, double *unused, double *out_6668711560817151875);
|
||||
void car_H_28(double *state, double *unused, double *out_7985276681522013161);
|
||||
void car_h_31(double *state, double *unused, double *out_3787573678896021836);
|
||||
void car_H_31(double *state, double *unused, double *out_8147590685328020518);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_5239387107130576177);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8017465374679690286);
|
||||
void pose_H_mod_fun(double *state, double *out_3050146727026118690);
|
||||
void pose_f_fun(double *state, double dt, double *out_7401195424910099109);
|
||||
void pose_F_fun(double *state, double dt, double *out_7953355824038160444);
|
||||
void pose_h_4(double *state, double *unused, double *out_5685529689104176696);
|
||||
void pose_H_4(double *state, double *unused, double *out_8898228533778077411);
|
||||
void pose_h_10(double *state, double *unused, double *out_5395236827797613020);
|
||||
void pose_H_10(double *state, double *unused, double *out_4460820551654126260);
|
||||
void pose_h_13(double *state, double *unused, double *out_5553209120626692209);
|
||||
void pose_H_13(double *state, double *unused, double *out_5685954708445744610);
|
||||
void pose_h_14(double *state, double *unused, double *out_8764675248435452423);
|
||||
void pose_H_14(double *state, double *unused, double *out_4934987677438592882);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_2903893253895971355);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3928278332152438487);
|
||||
void pose_H_mod_fun(double *state, double *out_5666984403538513454);
|
||||
void pose_f_fun(double *state, double dt, double *out_3108653889513613454);
|
||||
void pose_F_fun(double *state, double dt, double *out_3156103136607478463);
|
||||
void pose_h_4(double *state, double *unused, double *out_4668712772901425910);
|
||||
void pose_H_4(double *state, double *unused, double *out_3277377262519307888);
|
||||
void pose_h_10(double *state, double *unused, double *out_5024807863031837328);
|
||||
void pose_H_10(double *state, double *unused, double *out_4567419581298515838);
|
||||
void pose_h_13(double *state, double *unused, double *out_711061843632216754);
|
||||
void pose_H_13(double *state, double *unused, double *out_4333253945797393041);
|
||||
void pose_h_14(double *state, double *unused, double *out_1546878073048898636);
|
||||
void pose_H_14(double *state, double *unused, double *out_685863593820176641);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -12,6 +12,7 @@ import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.hyundai.ev6_aol import qualified as ev6_aol_qualified
|
||||
from opendbc.car.honda.stock_aol import qualified as honda_aol_qualified, temporary_restriction as honda_temporary_restriction
|
||||
from openpilot.cereal.visionipc import VisionStreamType
|
||||
from msgq.visionipc import VisionIpcClient
|
||||
|
||||
@@ -35,7 +36,7 @@ from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion
|
||||
from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal
|
||||
from openpilot.starpilot.aol.intent import read_settings
|
||||
from openpilot.starpilot.aol.intent_companion import IntentCompanion
|
||||
from openpilot.starpilot.aol.transport_pause import AvailabilityPause, TransportPause, availability_only
|
||||
from openpilot.starpilot.aol.transport_pause import AvailabilityPause, TransportPause, availability_only, cp_identity, healthy_transport
|
||||
from openpilot.starpilot.aol.runtime import (INTENT_MAX_AGE_NS, SAFETY_MAX_AGE_NS, AxisDecision, current_intent, current_native, decide_axes,
|
||||
ordinary_lateral_requested, decide_ordinary_axis)
|
||||
from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed
|
||||
@@ -1068,6 +1069,38 @@ class SelfdriveD:
|
||||
sample.vEgo < (self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0)
|
||||
for sample, _ in getattr(self, 'car_state_samples', ())))
|
||||
|
||||
def honda_native_lateral_grant_lost(self, CS, native, previous, requested, *, source_current, previous_intent, intent, now_ns):
|
||||
"""An established host LAT request lost its native grant, not a cold wait."""
|
||||
pause = getattr(self, 'aol_transport_pause', None)
|
||||
prior = getattr(pause, 'observed_native', None)
|
||||
if (not honda_aol_qualified(self.CP, marked_only=True) or not source_current or
|
||||
not self.initialized or not previous.lateral_active or not requested.desired_lateral or
|
||||
native is None or previous_intent is None or intent is None or not previous_intent.producerSessionId or
|
||||
previous_intent.producerSessionId != intent.producerSessionId or
|
||||
not 0 < previous_intent.carStateLogMonoTime <= native.observedMonoTime <= now_ns or
|
||||
now_ns - previous_intent.carStateLogMonoTime > INTENT_MAX_AGE_NS or
|
||||
native.requestedLateral or native.lateralAllowed or
|
||||
prior is None or not prior.requestedLateral or not prior.lateralAllowed or
|
||||
getattr(self, 'aol_fault_reason', 'none') != 'none' or
|
||||
pause.active or getattr(getattr(self, 'aol_availability_pause', None), 'active', False) or
|
||||
CS.brakePressed or CS.regenBraking or CS.gasPressed or CS.steeringPressed or
|
||||
honda_temporary_restriction(self.CP, CS) or
|
||||
any(self.events.contains(event) for event in
|
||||
(ET.USER_DISABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL, ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE)) or
|
||||
not self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) or
|
||||
self.sm['extrinsicsCalibration'].calStatus != log.ExtrinsicsCalibration.Status.calibrated):
|
||||
return False
|
||||
def identity(state):
|
||||
return (state.axisSessionId, state.pandaSerial, state.safetyModel, state.safetyParam,
|
||||
tuple((p.slotIndex, p.hardwareSerial, p.safetyModel, p.safetyParam,
|
||||
p.alternativeExperience) for p in state.pandaInventory))
|
||||
return bool(pause.cp == cp_identity(self.CP) and pause.session == self.aol_session_id and
|
||||
identity(prior) == identity(native) and
|
||||
0 < prior.observedMonoTime <= native.observedMonoTime <= now_ns <= prior.validUntilMonoTime and
|
||||
now_ns - prior.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id) == native and
|
||||
healthy_transport(self.sm, self.CP, CS, self.aol_session_id, now_ns, native))
|
||||
|
||||
def record_aol_authority_loss(self, *, now_ns, companion, drained, native, source_current,
|
||||
lost_active, lost_companion, lost_permission, upgraded):
|
||||
if now_ns - getattr(self, 'aol_last_authority_report_ns', 0) < 5_000_000_000:
|
||||
@@ -1141,7 +1174,10 @@ class SelfdriveD:
|
||||
self.events.contains(ET.USER_DISABLE))
|
||||
lost_companion = bool((previous.lateral_active or previous.longitudinal_active) and
|
||||
not source_current and not self.events.contains(ET.USER_DISABLE))
|
||||
lost_permission = bool(native is not None and not driver_override and (
|
||||
lost_honda_lateral_grant = self.honda_native_lateral_grant_lost(
|
||||
CS, native, previous, requested, source_current=source_current,
|
||||
previous_intent=previous_intent, intent=intent, now_ns=now_ns)
|
||||
lost_permission = bool(lost_honda_lateral_grant or native is not None and not driver_override and (
|
||||
previous.lateral_active and requested.desired_lateral and native.requestedLateral and
|
||||
not native.lateralAllowed and not CS.steeringPressed and not self.events.contains(ET.OVERRIDE_LATERAL) or
|
||||
previous.longitudinal_active and requested.desired_longitudinal and native.requestedLongitudinal and
|
||||
|
||||
@@ -0,0 +1,277 @@
|
||||
"""Honda brake only retains existing LAT intent; these are controlled unit peers."""
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car import Bus, gen_empty_fingerprint, structs
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.values import CAR, HondaFlags
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.starpilot.aol.runtime import decide_axes
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, independent_lateral_health, _healthy_transport_native
|
||||
from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety
|
||||
|
||||
NOW = 10_000_000_000
|
||||
|
||||
|
||||
class Peers:
|
||||
def __init__(self, messages):
|
||||
self.messages = {m.which(): m for m in messages}
|
||||
self.logMonoTime = {n: int(m.logMonoTime) for n, m in self.messages.items()}
|
||||
self.seen = self.valid = self.alive = dict.fromkeys(self.messages, True)
|
||||
|
||||
def __getitem__(self, name):
|
||||
return getattr(self.messages[name], name)
|
||||
|
||||
|
||||
def fixture(*, coherent=False, long_allowed=True, brake=True):
|
||||
fp = gen_empty_fingerprint()
|
||||
fp[0][0x1A3] = 8
|
||||
cp = CarInterface.get_params(CAR.HONDA_CIVIC_2022, fp, [], True, False, False)
|
||||
cp.alternativeExperience = 32
|
||||
cp.fingerprintSource = structs.CarParams.FingerprintSource.fw
|
||||
config = cp.safetyConfigs[0]
|
||||
assert config.safetyParam == 10
|
||||
slots = ((PandaSlot(0, 'unit-honda', config.safetyModel.raw, 10, 32, long_allowed),) if coherent else ())
|
||||
native = SafetyState(1, True, NOW - 20_000_000, NOW + 180_000_000,
|
||||
config.safetyModel.raw, 10, True, long_allowed, True, True,
|
||||
'unit-honda', 'unit-session', sourcePandaStatesMonoTime=NOW + 9_000_000_000 if coherent else 0,
|
||||
pandaInventory=slots)
|
||||
safety = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime)
|
||||
safety.aolSafetyWire = encode_safety(native)
|
||||
pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=NOW)
|
||||
p = pandas.pandaStates[0]
|
||||
p.safetyModel, p.safetyParam, p.alternativeExperience = config.safetyModel, 10, 32
|
||||
p.controlsAllowed = False
|
||||
axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=NOW - 1_000_000)
|
||||
a = axis.aolAxisState
|
||||
a.sessionId = a.faultSessionId = 'unit-session'
|
||||
a.qualified, a.sequence, a.faultReason = True, 2, 'none'
|
||||
a.observedMonoTime, a.validUntilMonoTime = axis.logMonoTime, axis.logMonoTime + 30_000_000
|
||||
a.sourceCarStateMonoTime = axis.logMonoTime - 1000
|
||||
a.desiredLateral = a.lateralActive = True
|
||||
a.nativeAcknowledged = True
|
||||
events = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=NOW - 1_000_000)
|
||||
sm = Peers([safety.as_reader(), pandas.as_reader(), axis.as_reader(), events.as_reader()])
|
||||
cs = structs.CarState(canValid=True, brakePressed=brake, vEgo=12.5,
|
||||
gearShifter=structs.CarState.GearShifter.drive)
|
||||
feedback = TransportPauseFeedback(cp)
|
||||
# Unit precondition representing an already observed healthy acknowledged LAT baseline.
|
||||
feedback.session = native.axisSessionId
|
||||
feedback.healthy_feedback_ns = NOW - 10_000_000
|
||||
feedback.healthy_native_ns = native.observedMonoTime
|
||||
feedback.healthy_inventory = (native.pandaSerial, tuple(p.hardwareSerial for p in slots))
|
||||
return cp, cs, sm, native, feedback
|
||||
|
||||
|
||||
@pytest.mark.parametrize('coherent', [False, True])
|
||||
@pytest.mark.parametrize('long_allowed', [False, True])
|
||||
def test_honda_brake_lateral_intent_only_and_long_fail_closed(coherent, long_allowed):
|
||||
cp, cs, sm, native, feedback = fixture(coherent=coherent, long_allowed=long_allowed)
|
||||
assert _healthy_transport_native(sm, cp, cs, 'unit-session', NOW) is None
|
||||
assert independent_lateral_health(sm, cp, cs, 'unit-session', NOW) == native
|
||||
assert not feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=NOW - 1000)
|
||||
assert feedback.honda_brake_ns == NOW - 1000
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=False,
|
||||
intent=SimpleNamespace(pauseLateral=False, allowedLatch=True, pauseLongitudinal=False),
|
||||
native=native, car_state=cs, initialized=True, model_ready=True, no_entry=False,
|
||||
immediate_disable=False, dm_lockout=False, pause_brake_mps=5.)
|
||||
assert decision.desired_lateral and decision.lateral_active
|
||||
assert not decision.desired_longitudinal and not decision.longitudinal_active
|
||||
assert native.validUntilMonoTime == NOW + 180_000_000
|
||||
|
||||
|
||||
@pytest.mark.parametrize('mutation', ['no_brake', 'no_source', 'stale_source', 'future_source', 'no_latch',
|
||||
'wrong_profile', 'wrong_session', 'serial', 'native_expired', 'no_lat', 'lat_denied',
|
||||
'rx_fault', 'heartbeat', 'panda_config', 'axis_fault', 'hard_event', 'stale_axis', 'unacknowledged_long'])
|
||||
def test_honda_brake_cannot_borrow_or_renew_authority(mutation):
|
||||
cp, cs, sm, native, feedback = fixture(coherent=True, long_allowed=False)
|
||||
source, now, latched = NOW - 1000, NOW, True
|
||||
axis = sm.messages['aolAxisState'].as_builder()
|
||||
pandas = sm.messages['pandaStates'].as_builder()
|
||||
if mutation == 'no_brake':
|
||||
cs.brakePressed = False
|
||||
if mutation == 'no_source':
|
||||
source = 0
|
||||
if mutation == 'stale_source':
|
||||
source = NOW - 30_000_001
|
||||
if mutation == 'future_source':
|
||||
source = NOW + 1
|
||||
if mutation == 'no_latch':
|
||||
latched = False
|
||||
if mutation == 'wrong_profile':
|
||||
cp.alternativeExperience = 0
|
||||
if mutation == 'wrong_session':
|
||||
native = replace(native, axisSessionId='replaced-session')
|
||||
if mutation == 'serial':
|
||||
native = replace(native, pandaSerial='replaced-serial')
|
||||
if mutation == 'native_expired':
|
||||
native = replace(native, validUntilMonoTime=NOW - 1)
|
||||
if mutation == 'no_lat':
|
||||
native = replace(native, requestedLateral=False)
|
||||
if mutation == 'lat_denied':
|
||||
native = replace(native, lateralAllowed=False)
|
||||
if mutation == 'rx_fault':
|
||||
pandas.pandaStates[0].safetyRxChecksInvalid = True
|
||||
if mutation == 'heartbeat':
|
||||
pandas.pandaStates[0].heartbeatLost = True
|
||||
if mutation == 'panda_config':
|
||||
pandas.pandaStates[0].safetyParam = 8
|
||||
if mutation == 'axis_fault':
|
||||
axis.aolAxisState.faultReason = 'critical'
|
||||
if mutation == 'stale_axis':
|
||||
now = NOW + 30_000_001
|
||||
if mutation == 'unacknowledged_long':
|
||||
axis.aolAxisState.nativeAcknowledged = False
|
||||
axis.aolAxisState.desiredLongitudinal = True
|
||||
events = sm.messages['onroadEvents']
|
||||
if mutation == 'hard_event':
|
||||
event = messaging.new_message('onroadEvents', 1, valid=True, logMonoTime=NOW - 1000)
|
||||
event.onroadEvents[0].name = 'controlsMismatch'
|
||||
event.onroadEvents[0].immediateDisable = True
|
||||
events = event.as_reader()
|
||||
safety = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime)
|
||||
safety.aolSafetyWire = encode_safety(native)
|
||||
sm = Peers([safety.as_reader(), pandas.as_reader(), axis.as_reader(), events])
|
||||
assert feedback._honda_brake_lateral_current(sm, cp, cs, sm['aolAxisState'], now,
|
||||
latched=latched, brake_source_ns=source) is None
|
||||
if mutation != 'no_latch':
|
||||
assert feedback.observe(sm, cp, cs, now, latched=latched, brake_source_ns=source)
|
||||
assert feedback.honda_brake_ns == 0
|
||||
|
||||
|
||||
def test_release_and_held_brake_cannot_extend_original_deadline():
|
||||
cp, cs, sm, native, feedback = fixture(long_allowed=False)
|
||||
source = NOW - 1000
|
||||
assert not feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=source)
|
||||
cs.brakePressed = False
|
||||
assert not feedback.observe(sm, cp, cs, NOW + 1, latched=True)
|
||||
assert feedback.honda_brake_ns == source
|
||||
cs.brakePressed = True
|
||||
assert not feedback.observe(sm, cp, cs, NOW + 2, latched=True, brake_source_ns=NOW + 2)
|
||||
assert feedback.honda_brake_ns == source
|
||||
axis = sm['aolAxisState']
|
||||
assert feedback._honda_brake_lateral_current(sm, cp, cs, axis, source + 200_000_001,
|
||||
latched=True, brake_source_ns=source + 200_000_001) is None
|
||||
assert feedback.honda_brake_ns == source
|
||||
|
||||
|
||||
def test_card_brake_source_keeps_boot_and_monotonic_clocks_separate():
|
||||
cp, cs, _, _, _ = fixture()
|
||||
card: Any = Car.__new__(Car)
|
||||
card.CP = cp
|
||||
message = 'BRAKE_MODULE' if cp.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA'
|
||||
parser = SimpleNamespace(vl={message: {'BRAKE_PRESSED': 1}},
|
||||
ts_nanos={message: {'BRAKE_PRESSED': NOW + 9_000_000_000}})
|
||||
card.CI = SimpleNamespace(can_parsers={Bus.pt: parser}, CS=SimpleNamespace(brake_switch_active=False))
|
||||
card._aol_can_observation = (cs, NOW - 1000, NOW + 9_000_000_000, NOW + 9_000_001_000)
|
||||
assert card.aol_honda_brake_source(cs, NOW) == NOW - 1000
|
||||
parser.ts_nanos[message]['BRAKE_PRESSED'] -= 30_000_001
|
||||
assert card.aol_honda_brake_source(cs, NOW) == 0
|
||||
parser.ts_nanos[message]['BRAKE_PRESSED'] += 30_000_001
|
||||
assert card.aol_honda_brake_source(structs.CarState(**cs.to_dict()), NOW) == 0
|
||||
parser.vl[message]['BRAKE_PRESSED'] = 0
|
||||
assert card.aol_honda_brake_source(cs, NOW) == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('mutation', ['none', 'cold', 'withdrawn', 'no_source', 'brake', 'steering', 'gear',
|
||||
'soft_pause', 'transport_pause', 'availability_pause', 'session', 'serial', 'cp', 'older', 'expired',
|
||||
'fault', 'model', 'requested_denial', 'new_session', 'no_prior', 'uninitialized',
|
||||
'latest_expired', 'temporary_steer', 'unqualified', 'user_disable', 'rx_fault',
|
||||
'receipt_before_source', 'stale_source', 'producer', 'no_prior_source', 'long_override'])
|
||||
def test_sd_honda_unexpected_zero_grant_loss_is_not_cold_or_commanded_pause(mutation):
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
||||
from openpilot.starpilot.aol.runtime import AxisDecision
|
||||
from openpilot.starpilot.aol.transport_pause import cp_identity
|
||||
|
||||
cp, cs, sm, prior, _ = fixture(coherent=True, brake=False)
|
||||
prior = replace(prior, observedMonoTime=NOW - 40_000_000, validUntilMonoTime=NOW + 160_000_000)
|
||||
native = replace(prior, observedMonoTime=NOW - 10_000_000, validUntilMonoTime=NOW + 190_000_000,
|
||||
requestedLateral=False, lateralAllowed=False, requestedLongitudinal=False, longitudinalAllowed=False)
|
||||
sd: Any = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.CP, sd.sm, sd.aol_session_id = cp, sm, 'unit-session'
|
||||
sd.initialized, sd.events, sd.aol_fault_reason = True, Events(), 'none'
|
||||
sd.aol_transport_pause = SimpleNamespace(cp=cp_identity(cp), session='unit-session', active=False, observed_native=prior)
|
||||
sd.aol_availability_pause = SimpleNamespace(active=False)
|
||||
sm.all_checks = lambda services: True
|
||||
sm.messages['extrinsicsCalibration'] = SimpleNamespace(extrinsicsCalibration=SimpleNamespace(
|
||||
calStatus=log.ExtrinsicsCalibration.Status.calibrated))
|
||||
previous, requested = AxisDecision(lateral_active=True), AxisDecision(desired_lateral=True)
|
||||
source_current = True
|
||||
prior_intent = SimpleNamespace(producerSessionId='unit-producer', carStateLogMonoTime=NOW - 10_000_000)
|
||||
intent = SimpleNamespace(producerSessionId='unit-producer')
|
||||
if mutation == 'cold':
|
||||
previous = AxisDecision()
|
||||
elif mutation == 'withdrawn':
|
||||
requested = AxisDecision()
|
||||
elif mutation == 'no_source':
|
||||
source_current = False
|
||||
elif mutation == 'brake':
|
||||
cs.brakePressed = True
|
||||
elif mutation == 'steering':
|
||||
cs.steeringPressed = True
|
||||
elif mutation == 'gear':
|
||||
cs.gearShifter = structs.CarState.GearShifter.park
|
||||
elif mutation == 'soft_pause':
|
||||
sd.aol_fault_reason = 'availabilityPause'
|
||||
elif mutation == 'transport_pause':
|
||||
sd.aol_transport_pause.active = True
|
||||
elif mutation == 'availability_pause':
|
||||
sd.aol_availability_pause.active = True
|
||||
elif mutation == 'session':
|
||||
native = replace(native, axisSessionId='replacement')
|
||||
elif mutation == 'serial':
|
||||
native = replace(native, pandaSerial='replacement')
|
||||
elif mutation == 'cp':
|
||||
sd.aol_transport_pause.cp = ()
|
||||
elif mutation == 'older':
|
||||
native = replace(native, observedMonoTime=prior.observedMonoTime - 1)
|
||||
elif mutation == 'expired':
|
||||
prior = replace(prior, validUntilMonoTime=NOW - 1)
|
||||
elif mutation == 'fault':
|
||||
sd.events.add(log.OnroadEvent.EventName.controlsMismatch)
|
||||
elif mutation == 'model':
|
||||
sm.all_checks = lambda services: False
|
||||
elif mutation == 'requested_denial':
|
||||
native = replace(native, requestedLateral=True)
|
||||
elif mutation == 'new_session':
|
||||
sd.aol_transport_pause.session = 'replacement'
|
||||
elif mutation == 'no_prior':
|
||||
prior = None
|
||||
elif mutation == 'uninitialized':
|
||||
sd.initialized = False
|
||||
elif mutation == 'latest_expired':
|
||||
native = replace(native, validUntilMonoTime=NOW - 1)
|
||||
elif mutation == 'temporary_steer':
|
||||
cs.steerFaultTemporary = True
|
||||
elif mutation == 'unqualified':
|
||||
cp.alternativeExperience = 0
|
||||
elif mutation == 'user_disable':
|
||||
sd.events.add(log.OnroadEvent.EventName.buttonCancel)
|
||||
elif mutation == 'rx_fault':
|
||||
pandas = sm.messages['pandaStates'].as_builder()
|
||||
pandas.pandaStates[0].safetyRxChecksInvalid = True
|
||||
sm.messages['pandaStates'] = pandas.as_reader()
|
||||
if mutation == 'receipt_before_source':
|
||||
prior_intent.carStateLogMonoTime = NOW - 9_999_999
|
||||
elif mutation == 'stale_source':
|
||||
prior_intent.carStateLogMonoTime = NOW - 30_000_001
|
||||
elif mutation == 'producer':
|
||||
intent.producerSessionId = 'replacement'
|
||||
elif mutation == 'no_prior_source':
|
||||
prior_intent.carStateLogMonoTime = 0
|
||||
elif mutation == 'long_override':
|
||||
sd.events.add(log.OnroadEvent.EventName.gasPressedOverride)
|
||||
sd.aol_transport_pause.observed_native = prior
|
||||
event = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime)
|
||||
event.aolSafetyWire = encode_safety(native)
|
||||
sm.messages['aolSafetyWire'] = event.as_reader()
|
||||
sm.logMonoTime['aolSafetyWire'] = native.observedMonoTime
|
||||
assert sd.honda_native_lateral_grant_lost(cs, native, previous, requested,
|
||||
source_current=source_current, previous_intent=prior_intent, intent=intent, now_ns=NOW) == (mutation == 'none')
|
||||
assert native.validUntilMonoTime == (NOW - 1 if mutation == 'latest_expired' else NOW + 190_000_000)
|
||||
assert not native.lateralAllowed and not native.longitudinalAllowed
|
||||
@@ -11,14 +11,14 @@ def cp_identity(cp):
|
||||
tuple((int(c.safetyModel.raw), int(c.safetyParam)) for c in cp.safetyConfigs))
|
||||
|
||||
|
||||
def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None):
|
||||
def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None, *, require_longitudinal=True):
|
||||
"""Require the live, exact native configuration, including every Panda."""
|
||||
try:
|
||||
native = native or current_native(sm, cp, now_ns=now_ns, axis_session_id=session)
|
||||
if (not session or native is None or str(native.axisSessionId) != session or
|
||||
not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent or
|
||||
native.requestedLateral and not native.lateralAllowed or
|
||||
native.requestedLongitudinal and not native.longitudinalAllowed):
|
||||
require_longitudinal and native.requestedLongitudinal and not native.longitudinalAllowed):
|
||||
return None
|
||||
coherent = bool(native.pandaInventory)
|
||||
if coherent and (int(sm.logMonoTime['aolSafetyWire']) != native.observedMonoTime or
|
||||
@@ -28,7 +28,7 @@ def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None):
|
||||
return None
|
||||
if coherent and (not native_inventory_matches_cp(native, cp) or
|
||||
any(p.safetyRxChecksInvalid or p.heartbeatLost or p.faults or
|
||||
(native.requestedLongitudinal and str(config.safetyModel) not in ('silent', 'noOutput') and
|
||||
(require_longitudinal and native.requestedLongitudinal and str(config.safetyModel) not in ('silent', 'noOutput') and
|
||||
not p.controlsAllowed)
|
||||
for p, config in zip(native.pandaInventory, cp.safetyConfigs, strict=True))):
|
||||
return None
|
||||
@@ -44,12 +44,18 @@ def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None):
|
||||
return native if all(int(p.safetyModel.raw) == int(c.safetyModel.raw) and p.safetyParam == c.safetyParam and
|
||||
p.alternativeExperience == cp.alternativeExperience and not p.safetyRxChecksInvalid and
|
||||
not p.heartbeatLost and not p.faults and
|
||||
(not native.requestedLongitudinal or str(c.safetyModel) in ('silent', 'noOutput') or p.controlsAllowed)
|
||||
(not require_longitudinal or not native.requestedLongitudinal or str(c.safetyModel) in ('silent', 'noOutput') or p.controlsAllowed)
|
||||
for p, c in zip(pandas, cp.safetyConfigs, strict=True)) else None
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def independent_lateral_health(sm, cp, cs, session, now_ns):
|
||||
"""Structural health for retaining LAT intent; never grant either output axis."""
|
||||
native = _healthy_transport_native(sm, cp, cs, session, now_ns, require_longitudinal=False)
|
||||
return native if native is not None and native.requestedLateral and native.lateralAllowed else None
|
||||
|
||||
|
||||
def healthy_transport(sm, cp, cs, session, now_ns, native=None):
|
||||
"""Require the live, exact native configuration, including every Panda."""
|
||||
return _healthy_transport_native(sm, cp, cs, session, now_ns, native) is not None
|
||||
@@ -202,6 +208,7 @@ class TransportPauseFeedback:
|
||||
self.lateral_native_ns = 0
|
||||
self.longitudinal_cancel_ns = 0
|
||||
self.last_cancel_source_ns = 0
|
||||
self.honda_brake_ns = 0
|
||||
self.retention_qualified = False
|
||||
self.retention_pending = False
|
||||
self.retention_event_ns = 0
|
||||
@@ -214,6 +221,38 @@ class TransportPauseFeedback:
|
||||
self.healthy_native_ns = 0
|
||||
self.healthy_inventory = None
|
||||
|
||||
def _honda_brake_lateral_current(self, sm, cp, cs, axis, now_ns, *, latched, brake_source_ns):
|
||||
"""Preserve existing LAT intent through brake's LONG withdrawal, never output."""
|
||||
from opendbc.car.honda.stock_aol import qualified
|
||||
from opendbc.car.structs import car
|
||||
|
||||
source_ns = self.honda_brake_ns or (brake_source_ns if cs.brakePressed else 0)
|
||||
if (not qualified(cp, marked_only=True) or not latched or self.failed or self.paused or self.pending_event or
|
||||
not self.session or str(axis.sessionId) != self.session or
|
||||
not self.healthy_feedback_ns or not 0 <= now_ns - self.healthy_feedback_ns <= AXIS_MAX_AGE_NS or
|
||||
not 0 < source_ns <= now_ns or now_ns - source_ns > SAFETY_MAX_AGE_NS or
|
||||
(not self.honda_brake_ns and now_ns - source_ns > AXIS_MAX_AGE_NS) or
|
||||
cs.accFaulted or cs.seatbeltUnlatched or
|
||||
cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport,
|
||||
car.CarState.GearShifter.low, car.CarState.GearShifter.brake) or
|
||||
str(axis.faultReason) != 'none' or not axis.desiredLateral or not axis.lateralActive or
|
||||
(not axis.nativeAcknowledged and (axis.desiredLongitudinal or axis.longitudinalActive)) or
|
||||
not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime or
|
||||
axis.observedMonoTime - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS):
|
||||
return None
|
||||
event_ns = int(sm.logMonoTime['onroadEvents'])
|
||||
if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or
|
||||
not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000 or
|
||||
any(e.immediateDisable or e.softDisable for e in sm['onroadEvents'])):
|
||||
return None
|
||||
native = independent_lateral_health(sm, cp, cs, self.session, now_ns)
|
||||
if (native is None or not native.requestedLateral or not native.lateralAllowed or
|
||||
not native.requestedLongitudinal or native.observedMonoTime < self.healthy_native_ns or
|
||||
(native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.healthy_inventory):
|
||||
return None
|
||||
self.honda_brake_ns = source_ns
|
||||
return native
|
||||
|
||||
def _longitudinal_cancel_current(self, sm, cp, cs, axis, now_ns, *, latched, cancel_source_ns, withdrawal_required=True) -> SafetyState | None:
|
||||
"""Retain existing intent during Cancel's bounded LONG withdrawal handshake."""
|
||||
from opendbc.car.hyundai.ev6_aol import qualified
|
||||
@@ -399,7 +438,7 @@ class TransportPauseFeedback:
|
||||
self.awaiting = True
|
||||
return True
|
||||
|
||||
def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, intent_producer_session=''):
|
||||
def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, brake_source_ns=0, intent_producer_session=''):
|
||||
self.qualified = False
|
||||
self.awaiting = False
|
||||
self.retention_qualified = False
|
||||
@@ -420,6 +459,15 @@ class TransportPauseFeedback:
|
||||
cp_identity(cp) == self.cp)
|
||||
continuity = bool(not self.session or axis.sessionId == self.session and
|
||||
(axis.sequence > self.sequence or axis.sequence == self.sequence and signature == self.signature))
|
||||
brake_native = (self._honda_brake_lateral_current(sm, cp, cs, axis, now_ns,
|
||||
latched=latched, brake_source_ns=brake_source_ns) if current and continuity else None)
|
||||
if brake_native is not None:
|
||||
# Keep only existing intent. The original native lease and source stamps
|
||||
# remain unchanged; decide_axes/native TX independently withdraw LONG.
|
||||
self.healthy_feedback_ns = stamp
|
||||
self.healthy_native_ns = int(brake_native.observedMonoTime)
|
||||
self.sequence, self.signature = int(axis.sequence), signature
|
||||
return False
|
||||
if self._source_availability_retention(sm, cp, cs, axis, stamp, now_ns,
|
||||
latched=latched, continuity=continuity, producer=intent_producer_session, signature=signature):
|
||||
self.sequence, self.signature = int(axis.sequence), signature
|
||||
@@ -468,6 +516,8 @@ class TransportPauseFeedback:
|
||||
self.signature = signature
|
||||
return False
|
||||
if transport_healthy:
|
||||
if native is not None and not native.requestedLongitudinal:
|
||||
self.honda_brake_ns = 0
|
||||
if (not latched or native is not None and not native.requestedLongitudinal or
|
||||
self.longitudinal_cancel_ns and now_ns - self.longitudinal_cancel_ns > SAFETY_MAX_AGE_NS):
|
||||
self.longitudinal_cancel_ns = 0
|
||||
|
||||
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-c204f0b1-P92eb5651-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-7fc969bf-P92eb5651-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-c204f0b1-P92eb5651-DEBUG
|
||||
DEV-7fc969bf-P92eb5651-DEBUG
|
||||
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "b54a2d6ca333c30e432a21152166f29f2b5ee1ec880b8bf2d6c28e1ddc8687fa", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "26215417739b0673ec6dd0e0a744a0323c8f0d8dc846286e4582a2917588caf2", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user