controlsd: 5-state ScreenDisplayMode machine + park->drive auto-wake
Replaces the 3-state cycle in clearpilot_state_control with broken's 5-state machine: - Onroad (drive gear): 0→4, 1→2, 2→3, 3→4, 4→2 (auto modes never reached via the button — gpsd's day/night calc is the only path back) - Not in drive: anything except 3 → 3 (screen off), state 3 → 0 (auto) Also tracks park→drive edge to auto-wake from screen-off (mode 3 → 0) when shifting into drive, regardless of how the screen got turned off. Speed/cruise overlay state intentionally left out — owned by speed_logicd, not controlsd. events.py: remove clp_debug_notice + EventName.clpDebug binding (no more "Clearpilot Debug Function Executed (N)" on-screen message). The dead commented-out events.add(EventName.clpDebug) and stray print of buttonEvents in controlsd are also cleaned up.
This commit is contained in:
@@ -78,6 +78,8 @@ class Controls:
|
||||
|
||||
self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
# CLEARPILOT: edge tracking for park→drive auto-wake of screen
|
||||
self.was_driving_gear = False
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
@@ -1233,32 +1235,35 @@ class Controls:
|
||||
self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable")
|
||||
|
||||
def update_clearpilot_events(self, CS):
|
||||
if (len(CS.buttonEvents) > 0):
|
||||
if (len(CS.buttonEvents) > 0):
|
||||
print (CS.buttonEvents)
|
||||
|
||||
# Uncomment to alert when lkas button pressed
|
||||
# if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
# self.events.add(EventName.clpDebug)
|
||||
|
||||
def clearpilot_state_control(self, CC, CS):
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
|
||||
# Uncomment for debug testing.
|
||||
# if self.params_memory.get_bool("CPTLkasButtonAction"):
|
||||
# self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
# else:
|
||||
# self.params_memory.put_bool("CPTLkasButtonAction", True)
|
||||
|
||||
# Rotate display mode. These are mostly used in the frontend ui app.
|
||||
max_display_mode = 2
|
||||
current_display_mode = self.params_memory.get_int("ScreenDisplayMode")
|
||||
current_display_mode = current_display_mode + 1
|
||||
if current_display_mode > max_display_mode:
|
||||
current_display_mode = 0
|
||||
self.params_memory.put_int("ScreenDisplayMode", current_display_mode)
|
||||
# CLEARPILOT: pure UI plumbing — does not modify CC/actuators. Maintains
|
||||
# ScreenDisplayMode (5-state machine driven by the LFA/debug button + gear
|
||||
# edges). Speed/cruise overlay state is owned by speed_logicd, not here.
|
||||
driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park,
|
||||
GearShifter.reverse, GearShifter.unknown)
|
||||
|
||||
# Auto-wake screen when shifting into drive from screen-off
|
||||
if driving_gear and not self.was_driving_gear:
|
||||
if self.params_memory.get_int("ScreenDisplayMode") == 3:
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
self.was_driving_gear = driving_gear
|
||||
|
||||
# LFA/debug button cycles ScreenDisplayMode. Onroad and offroad use
|
||||
# different transition tables.
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
current = self.params_memory.get_int("ScreenDisplayMode")
|
||||
if driving_gear:
|
||||
# Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button)
|
||||
transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2}
|
||||
new_mode = transitions.get(current, 0)
|
||||
else:
|
||||
# Not in drive: anything except 3 → 3 (screen off), state 3 → 0 (auto)
|
||||
new_mode = 0 if current == 3 else 3
|
||||
self.params_memory.put_int("ScreenDisplayMode", new_mode)
|
||||
|
||||
# Leftovers
|
||||
# self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
|
||||
return CC
|
||||
|
||||
def main():
|
||||
|
||||
@@ -262,16 +262,6 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
|
||||
|
||||
def clp_debug_notice(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
|
||||
if not hasattr(clp_debug_notice, "counter"):
|
||||
clp_debug_notice.counter = 0
|
||||
clp_debug_notice.counter += 1
|
||||
return Alert(
|
||||
f"Clearpilot Debug Function Executed ({clp_debug_notice.counter})",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 3.0)
|
||||
|
||||
|
||||
def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
|
||||
# Clearpilot todo:
|
||||
@@ -778,10 +768,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
|
||||
},
|
||||
|
||||
EventName.clpDebug: {
|
||||
ET.PERMANENT: clp_debug_notice,
|
||||
},
|
||||
|
||||
EventName.noGps: {
|
||||
ET.PERMANENT: no_gps_alert,
|
||||
},
|
||||
|
||||
Reference in New Issue
Block a user