Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions opendbc_repo/opendbc/car/tesla/preap/nap_conf.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
'radar_donor_vin': '',
'radar_epas_type': 0,
'radar_position': 0,
'radar_upside_down': False,
}


Expand Down Expand Up @@ -133,6 +134,15 @@ def _save(self):
pass

def _get(self, key, default):
try:
if os.path.exists(CONFIG_FILE):
mtime = os.path.getmtime(CONFIG_FILE)
if getattr(self, "_last_mtime", 0) != mtime:
self._last_mtime = mtime
with open(CONFIG_FILE) as f:
self._cache.update(json.load(f))
except Exception:
pass
return self._cache.get(key, default)

def _put(self, key, value):
Expand Down Expand Up @@ -337,6 +347,14 @@ def radar_offset(self):
def radar_offset(self, value):
self._put_param_float(NAPParamKeys.RADAR_OFFSET, 'radar_offset', value)

@property
def radar_upside_down(self):
return self._get('radar_upside_down', False)

@radar_upside_down.setter
def radar_upside_down(self, value):
self._put('radar_upside_down', bool(value))

@property
def pedal_calib_min(self):
return self._get_param_float(NAPParamKeys.PEDAL_CALIB_MIN, 'pedal_calib_min', -3.0)
Expand Down
6 changes: 4 additions & 2 deletions opendbc_repo/opendbc/car/tesla/radar_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,8 @@ def update(self, can_msgs):
# Parse track data
self.pts[i].dRel = msg_a['LongDist']
self.pts[i].yRel = msg_a['LatDist'] + self.radar_offset
if nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False):
self.pts[i].yRel = -self.pts[i].yRel
self.pts[i].vRel = msg_a['LongSpeed']
self.pts[i].aRel = msg_a['LongAccel']
self.pts[i].yvRel = msg_b['LatSpeed']
Expand All @@ -274,9 +276,9 @@ def _parse_bosch_track(self, msg_a, msg_b):

return BoschTrackObservation(
d_rel=msg_a['LongDist'],
y_rel=msg_a['LatDist'] + self.radar_offset,
y_rel=-(msg_a['LatDist'] + self.radar_offset) if (nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False)) else (msg_a['LatDist'] + self.radar_offset),
v_rel=msg_a['LongSpeed'],
a_rel=msg_a['LongAccel'],
yv_rel=msg_b['LatSpeed'],
yv_rel=-msg_b['LatSpeed'] if (nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False)) else msg_b['LatSpeed'],
measured=bool(msg_a['Meas']),
)
13 changes: 13 additions & 0 deletions openpilot/selfdrive/ui/sunnypilot/layouts/settings/nap.py
Original file line number Diff line number Diff line change
Expand Up @@ -224,6 +224,19 @@ def _build_radar_items(self):
tr("Let's you engage when Bosch raises HWFail even though tracks are still live."),
dest=self._radar_items)

def on_upside_down(state):
from opendbc.car.tesla.preap.nap_conf import nap_conf
nap_conf.radar_upside_down = state
self._show_reboot_modal()

from opendbc.car.tesla.preap.nap_conf import nap_conf
self._radar_items.append(toggle_item_sp(
tr("Invert Radar Orientation (Upside Down)"),
tr("Flip the radar's left/right axis. Use this if your radar is mounted upside down. Requires reboot."),
initial_state=nap_conf.radar_upside_down,
callback=on_upside_down,
))

def on_radar_hud(state):
self._params.put_bool(NAPParamKeys.RADAR_HUD, state)
ui_state.radar_hud = state
Expand Down
30 changes: 26 additions & 4 deletions openpilot/sunnypilot/mads/mads.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
MadsSteeringModeOnBrake, persist_required_mads, read_steering_mode_param, resolve_mads_capabilities,
unified_engagement_locked_off,
)
from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT, ENABLED_STATES

State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
ButtonType = structs.CarState.ButtonEvent.Type
Expand All @@ -32,6 +32,9 @@
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
HANDS_ON_PAUSE_LEVEL = 2
HANDS_ON_RESUME_US = 1_000_000
# Same 200-sample bound already used for controlsMismatchLateral. Pending first
# grant uses it; a drop after grant is loss and disables immediately.
LATERAL_MISMATCH_MAX_COUNT = 200
UINT32_MASK = 0xFFFFFFFF


Expand Down Expand Up @@ -80,6 +83,8 @@ def __init__(self, selfdrive):
self._hands_on_steering_inhibited = False
self._hands_on_clear_timing = False
self._hands_on_clear_ts = 0
self._lateral_permission_acquired = False
self._lateral_permission_acquire_counter = 0

# Required-MADS platforms consume only the frozen typed snapshot.
if persist_required_mads(self.params, self.CP_SP):
Expand Down Expand Up @@ -171,6 +176,8 @@ def _reset_hands_on_clear_timer(self):
def _hard_disable_from_pause(self) -> None:
self._hands_on_steering_inhibited = False
self._reset_hands_on_clear_timer()
self._lateral_permission_acquired = False
self._lateral_permission_acquire_counter = 0
if self.events_sp.has(EventNameSP.silentLkasDisable):
self.events_sp.remove(EventNameSP.silentLkasDisable)
if self.events_sp.has(EventNameSP.silentLkasEnable):
Expand Down Expand Up @@ -205,19 +212,34 @@ def _update_hands_on_pause(self, CS: structs.CarState) -> None:
dm_lock = bool(
self.events.has(EventName.driverUnresponsive3) or self.events.has(EventName.driverDistracted3)
)
mads_enable = self.events_sp.has(EventNameSP.lkasEnable) or self.events_sp.has(EventNameSP.silentLkasEnable)
lat_requested = bool(self.enabled or self.state_machine.state in ENABLED_STATES or mads_enable)
if not lat_requested:
self._lateral_permission_acquired = False
self._lateral_permission_acquire_counter = 0

established_lat_lost = panda_lat_lost and self._lateral_permission_acquired
hard_event = bool(
epas_fault
or self.events_sp.has(EventNameSP.lkasDisable)
or door_or_gear
or (not cs_fresh)
or (not pandas)
or (not panda_fresh)
or panda_lat_lost
or established_lat_lost
or dm_lock
or self.events_sp.has(EventNameSP.controlsMismatchLateral)
or self.events.contains(ET.IMMEDIATE_DISABLE)
or self.events_sp.contains(ET.IMMEDIATE_DISABLE)
)
if lat_requested and not hard_event:
if panda_lat_lost:
self._lateral_permission_acquire_counter += 1
if self._lateral_permission_acquire_counter >= LATERAL_MISMATCH_MAX_COUNT:
hard_event = True
else:
self._lateral_permission_acquired = True
self._lateral_permission_acquire_counter = 0
if hard_event:
self._hard_disable_from_pause()
return
Expand Down Expand Up @@ -269,7 +291,7 @@ def data_sample(self):
# When the safety and selfdrived do not agree on controls_allowed_lateral
# we want to disengage sunnypilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
# Allow LATERAL_MISMATCH_MAX_COUNT control frames for that transport mismatch.
if not self.active or self.selfdrive.enabled:
self.lateral_mismatch_counter = 0
elif any(not ps.controlsAllowedLateral for ps in self.selfdrive.sm['pandaStates']
Expand Down Expand Up @@ -393,7 +415,7 @@ def update_events(self, CS: structs.CarState):
if self.state_machine.state == State.paused:
self.events_sp.add(EventNameSP.silentLkasEnable)

if self.lateral_mismatch_counter >= 200:
if self.lateral_mismatch_counter >= LATERAL_MISMATCH_MAX_COUNT:
self.events_sp.add(EventNameSP.controlsMismatchLateral)

self.events.remove(EventName.pcmDisable)
Expand Down
114 changes: 113 additions & 1 deletion openpilot/sunnypilot/mads/tests/test_preap_hands_on_pause.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.mads.mads import HANDS_ON_RESUME_US, ModularAssistiveDrivingSystem
from openpilot.sunnypilot.mads.mads import HANDS_ON_RESUME_US, LATERAL_MISMATCH_MAX_COUNT, ModularAssistiveDrivingSystem

State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
EventName = log.OnroadEvent.EventName
Expand Down Expand Up @@ -89,6 +89,26 @@ def make_mads(hands_on=True, capability=True):
mads.state_machine.state = State.enabled
return mads, sd

def _disengage(mads):
mads.enabled = False
mads.active = False
mads.state_machine.state = State.disabled


def _step_mads(mads, sd, cs, *, allowed=True, inhibited=False, panda_valid=True, enable=False, override=False):
ps = _panda(inhibited)
ps.controlsAllowedLateral = allowed
sd.sm = FakeSM([ps], panda_valid=panda_valid)
sd.events.clear()
sd.events_sp.clear()
if enable:
sd.events_sp.add(EventNameSP.lkasEnable)
if override:
sd.events.add(EventName.steerOverride)
mads.update(cs)
sd.CS_prev = cs



class TestPreAPHandsOnPause(unittest.TestCase):
def test_hands_on_level_pauses_not_steering_pressed(self):
Expand Down Expand Up @@ -327,6 +347,98 @@ def test_generic_platform_unchanged(self):
self.assertFalse(mads._hands_on_steering_inhibited)
self.assertFalse(sd.events_sp.has(EventNameSP.silentLkasDisable))

def test_first_pull_delayed_permission_50ms_does_not_false_disable(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
self.assertNotEqual(mads.state_machine.state, State.disabled)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
for _ in range(5):
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertNotEqual(mads.state_machine.state, State.disabled)
_step_mads(mads, sd, cs, allowed=True, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.overriding)

def test_first_pull_delayed_permission_over_100ms_does_not_false_disable(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
for _ in range(15):
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertNotEqual(mads.state_machine.state, State.disabled)
_step_mads(mads, sd, cs, allowed=True, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.overriding)

def test_never_granted_permission_hits_mismatch_bound(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
for _ in range(LATERAL_MISMATCH_MAX_COUNT - 2):
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertNotEqual(mads.state_machine.state, State.disabled)
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.disabled)

def test_acquired_permission_then_lost_hard_disables(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=True, enable=True, override=True)
_step_mads(mads, sd, cs, allowed=True, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.overriding)
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.disabled)

def test_pause_cannot_evade_acquisition_deadline(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
for _ in range(49):
_step_mads(mads, sd, cs, allowed=False, override=True)
self.assertNotEqual(mads.state_machine.state, State.disabled)
cs_pause = FakeCS(hands_on_level=2)
remaining = LATERAL_MISMATCH_MAX_COUNT - 50
for _ in range(remaining - 1):
_step_mads(mads, sd, cs_pause, allowed=False, inhibited=True)
self.assertNotEqual(mads.state_machine.state, State.disabled)
_step_mads(mads, sd, cs_pause, allowed=False, inhibited=True)
self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.disabled)

def test_stale_positive_grant_during_pending_hard_disables(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
for _ in range(10):
_step_mads(mads, sd, cs, allowed=False, override=True)
_step_mads(mads, sd, cs, allowed=True, override=True, panda_valid=False)
self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.disabled)

def test_door_during_pending_acquisition_hard_disables(self):
mads, sd = make_mads()
_disengage(mads)
cs = FakeCS(hands_on_level=1, steering_pressed=True)
_step_mads(mads, sd, cs, allowed=False, enable=True, override=True)
self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable))
_step_mads(mads, sd, FakeCS(hands_on_level=1, steering_pressed=True, door_open=True),
allowed=False, override=True)
self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable))
self.assertEqual(mads.state_machine.state, State.disabled)

if __name__ == "__main__":
unittest.main()
Original file line number Diff line number Diff line change
Expand Up @@ -237,9 +237,9 @@ def check_safety(self) -> bool:
return True

def _advance_on_pedal_message(self) -> None:
if self.pedal_interceptor_state != 0:
return
self.pedal_error_count = 0
if self.status == 2 and self.pedal_interceptor_state != 0:
return

if self.status == 2:
self.pedal_zero_sum += self.pedal_interceptor_value
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,7 @@ def can_send(self, addr: int, dat: bytes, bus: int) -> None:
if self._mode in (SAFETY_ELM327, SAFETY_ALLOUTPUT) or self._mode != SAFETY_TESLA_PREAP:
raise TransportError("pedal frames require teslaPreap calibration safety")
try:
self.panda.send_heartbeat(False, False)
self.panda.can_send(addr, dat, bus)
except Exception as exc:
raise TransportError(f"can_send failed: {exc}") from exc
Expand Down
Loading
Loading