Compare commits

...

5 Commits

Author SHA1 Message Date
Jason Wen 50b860c928 Reapply "plannerd: check all services for validity (#38341)"
This reverts commit 0265ae5f76.
2026-08-01 23:27:36 -04:00
Jason Wen 978ec800fe plannerd & selfdriveStateSP: poll modelV2, relay button state via bitmask (#1893)
* selfdrived: continuous button state and release counters in selfdriveStateSP

* tests and more
2026-08-01 23:05:03 -04:00
Jason Wen 3a05c03079 ci: no more docker (#1886) 2026-07-25 15:34:36 -04:00
Eitan fd22de1c9a SCC-M: fix operator precedence in quadratic roots (#1816)
* controls/scc: fix operator precedence in map controller quadratic roots

* not async

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
2026-07-25 09:37:18 -04:00
Christopher Haucke ee3583df33 [tizi/tici] ui: Camera offset controls (#1813)
* Camera offset controls

* final

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
2026-07-25 09:15:05 -04:00
14 changed files with 236 additions and 91 deletions
-39
View File
@@ -1,39 +0,0 @@
name: prebuilt
on:
schedule:
- cron: '0 * * * *'
workflow_dispatch:
env:
DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
BUILD: release/ci/docker_build_sp.sh
jobs:
build_prebuilt:
name: build prebuilt
runs-on: ubuntu-latest
if: github.repository == 'sunnypilot/sunnypilot'
env:
PUSH_IMAGE: true
permissions:
checks: read
contents: read
packages: write
steps:
- name: Wait for green check mark
if: ${{ github.event_name != 'workflow_dispatch' }}
uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc
with:
ref: master
wait-interval: 30
running-workflow-name: 'build prebuilt'
repo-token: ${{ secrets.GITHUB_TOKEN }}
check-regexp: ^((?!.*(build master-ci|create badges).*).)*$
- uses: actions/checkout@v6
with:
submodules: true
- run: git lfs pull
- name: Build and Push docker image
run: |
$DOCKER_LOGIN
eval "$BUILD"
+2
View File
@@ -69,6 +69,8 @@ struct LeadData {
struct SelfdriveStateSP @0x81c2f05a394cf4af {
mads @0 :ModularAssistiveDrivingSystem;
intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement;
buttonsPressed @2 :UInt16;
buttonsReleaseToggle @3 :UInt16;
enum AudibleAlert {
none @0;
@@ -158,7 +158,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'radarState'])
plan_send.valid = sm.all_checks()
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+4 -4
View File
@@ -29,19 +29,19 @@ def main():
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
'liveMapDataSP', 'carStateSP', gps_location_service],
poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service],
poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
while True:
sm.update()
longitudinal_planner.sla.update_car_state(sm['carState'])
longitudinal_planner.sla.update_buttons(sm['selfdriveStateSP'].buttonsReleaseToggle)
if sm.updated['modelV2']:
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
msg = messaging.new_message('driverAssistance')
msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters'])
msg.valid = sm.all_checks()
msg.driverAssistance.leftLaneDeparture = ldw.left
msg.driverAssistance.rightLaneDeparture = ldw.right
pm.send('driverAssistance', msg)
@@ -30,6 +30,7 @@ from openpilot.sunnypilot import get_sanitize_int_param
from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
REPLAY = "REPLAY" in os.environ
@@ -177,6 +178,7 @@ class SelfdriveD(CruiseHelper):
self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
CruiseHelper.__init__(self, self.CP)
self.button_state_tracker = ButtonStateTracker()
def update_events(self, CS):
"""Compute onroadEvents from carState"""
@@ -597,6 +599,8 @@ class SelfdriveD(CruiseHelper):
icbm.sendButton = self.icbm.cruise_button
icbm.vTarget = self.icbm.v_target
self.button_state_tracker.publish(ss_sp)
self.pm.send('selfdriveStateSP', ss_sp_msg)
# onroadEventsSP - logged every second or on change
@@ -616,6 +620,7 @@ class SelfdriveD(CruiseHelper):
self.mads.update(CS)
self.update_alerts(CS)
self.button_state_tracker.update(CS)
self.publish_selfdriveState(CS)
self.CS_prev = CS
@@ -43,7 +43,7 @@ class ModelsLayout(Widget):
self._initialize_items()
self.clear_cache_item.action_item.set_value(f"{self.calculate_cache_size():.2f} MB")
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay")]:
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay"), (self.camera_offset, "CameraOffset")]:
ctrl.action_item.set_value(int(float(ui_state.params.get(key, return_default=True)) * 100))
self._scroller = Scroller(self.items, line_separator=True, spacing=0)
@@ -93,9 +93,14 @@ class ModelsLayout(Widget):
self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle")
self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35,
tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"),
1, None, True, "", style.BUTTON_ACTION_WIDTH, None, True,
lambda v: f"{v / 100:.2f} m")
self.items = [self.current_model_item, self.cancel_download_item, self.supercombo_label, self.vision_label,
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item, self.lane_turn_desire_toggle,
self.lane_turn_value_control, self.lagd_toggle, self.delay_control]
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item,
self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, self.camera_offset]
def _update_lagd_description(self, lagd_toggle: bool):
desc = tr("Enable this for the car to learn and adapt its steering response time. Disable to use a fixed steering response time. " +
@@ -232,6 +237,7 @@ class ModelsLayout(Widget):
advanced_controls: bool = ui_state.params.get_bool("ShowAdvancedControls")
turn_desire: bool = ui_state.params.get_bool("LaneTurnDesire")
live_delay: bool = ui_state.params.get_bool("LagdToggle")
camera_offset: bool = ui_state.params.get("ModelManager_ActiveBundle") is not None
self.lane_turn_desire_toggle.action_item.set_state(turn_desire)
self.lane_turn_value_control.set_visible(turn_desire and advanced_controls)
@@ -240,6 +246,7 @@ class ModelsLayout(Widget):
new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step:
self.lane_turn_value_control.action_item.value_change_step = new_step
self.camera_offset.set_visible(camera_offset)
self._update_lagd_description(live_delay)
self.model_manager = ui_state.sm["modelManagerSP"]
@@ -151,8 +151,8 @@ class SmartCruiseControlMap:
a = 0.5 * TARGET_JERK
b = self.a_ego
c = self.v_ego - tv
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / (2 * a)
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / (2 * a)
if not isinstance(t_a, complex) and t_a > 0:
t = t_a
else:
@@ -4,13 +4,17 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
import pytest
from openpilot.cereal import custom
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import R, SmartCruiseControlMap
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
@@ -55,4 +59,17 @@ class TestSmartCruiseControlMap:
self.scc_m.update(True, False, 0., 0., 0.)
assert self.scc_m.state == VisionState.enabled
def test_moderate_curve(self):
# Regression: `... / 2 * a` parsed as `(.../2)*a` instead of `.../(2*a)`,
# making max_d ~11x too small so the moderate-curve branch never tripped.
# v_ego=25, a_ego=0, tv=24: fixed max_d≈45m vs buggy ≈4m at a 40m waypoint.
waypoint_lon_deg = (40.0 / R) * (180.0 / math.pi)
self.mem_params.put("LastGPSPosition", json.dumps({"latitude": 0.0, "longitude": 0.0}), block=True)
self.mem_params.put("MapTargetVelocities",
json.dumps([{"latitude": 0.0, "longitude": waypoint_lon_deg, "velocity": 24.0}]), block=True)
self.scc_m.update(True, False, 25.0, 0.0, 30.0)
assert self.scc_m.v_target == pytest.approx(24.0)
# TODO-SP: mock data from modelV2 to test other states
@@ -91,7 +91,7 @@ class SpeedLimitAssist:
self._plus_hold = 0.
self._minus_hold = 0.
self._last_carstate_ts = 0.
self._release_toggle_prev = 0
# TODO-SP: SLA's own output_a_target for planner
# Solution functions mapped to respective states
@@ -146,16 +146,16 @@ class SpeedLimitAssist:
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
def update_car_state(self, CS: car.CarState) -> None:
def update_buttons(self, release_toggle: int) -> None:
released = self._release_toggle_prev ^ release_toggle
self._release_toggle_prev = release_toggle
if not released:
return
now = time.monotonic()
self._last_carstate_ts = now
for b in CS.buttonEvents:
if not b.pressed:
if b.type in CRUISE_BUTTONS_PLUS:
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
elif b.type in CRUISE_BUTTONS_MINUS:
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
if any((released >> b) & 1 for b in CRUISE_BUTTONS_PLUS):
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
if any((released >> b) & 1 for b in CRUISE_BUTTONS_MINUS):
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
now = time.monotonic()
@@ -5,11 +5,14 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import time
import pytest
from openpilot.cereal import custom
from opendbc.car.car_helpers import interfaces
from opendbc.car.rivian.values import CAR as RIVIAN
from opendbc.car.structs import car
from opendbc.car.tesla.values import CAR as TESLA
from opendbc.car.toyota.values import CAR as TOYOTA
from openpilot.common.constants import CV
@@ -21,9 +24,13 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES, CRUISE_BUTTON_CONFIRM_HOLD
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
ButtonEvent = car.CarState.ButtonEvent
ButtonType = car.CarState.ButtonEvent.Type
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
@@ -276,3 +283,86 @@ class TestSpeedLimitAssist:
assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
elif initial_state in ACTIVE_STATES:
assert self.sla.state in ACTIVE_STATES
class TestButtonStateTrackerSLAIntegration:
def setup_method(self, method):
self.tracker = ButtonStateTracker()
self.params = Params()
self.params.put("IsReleaseSpBranch", True, block=True)
self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
self.params.put_bool("IsMetric", False, block=True)
self.params.put("SpeedLimitOffsetType", 0, block=True)
self.params.put("SpeedLimitValueOffset", 0, block=True)
CarInterface = interfaces[DEFAULT_CAR]
CP = CarInterface.get_non_essential_params(DEFAULT_CAR)
CP.openpilotLongitudinalControl = True
CP_SP = CarInterface.get_non_essential_params_sp(CP, DEFAULT_CAR)
self.sla = SpeedLimitAssist(CP, CP_SP)
def _make_cs(self, events=None) -> car.CarState:
CS = car.CarState()
CS.buttonEvents = events or []
return CS
def _run_ctrl_frames(self, frames: list[car.CarState]) -> None:
for cs in frames:
self.tracker.update(cs)
def test_button_confirm_via_tracker(self) -> None:
self._run_ctrl_frames([
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
self._make_cs(),
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
self._make_cs(),
self._make_cs(),
])
self.sla.update_buttons(self.tracker.release_toggle)
assert self.sla._get_button_release(req_plus=True, req_minus=False)
def test_rapid_press_release_between_polls(self) -> None:
self.sla.update_buttons(self.tracker.release_toggle)
self._run_ctrl_frames([
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]),
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=False)]),
self._make_cs(),
self._make_cs(),
self._make_cs(),
])
self.sla.update_buttons(self.tracker.release_toggle)
assert self.sla._get_button_release(req_plus=False, req_minus=True)
def test_multiple_releases_between_polls(self) -> None:
self.sla.update_buttons(self.tracker.release_toggle)
self._run_ctrl_frames([
self._make_cs([
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
]),
self._make_cs([
ButtonEvent(type=ButtonType.accelCruise, pressed=False),
ButtonEvent(type=ButtonType.decelCruise, pressed=False),
]),
])
self.sla.update_buttons(self.tracker.release_toggle)
assert self.sla._get_button_release(req_plus=True, req_minus=False)
assert self.sla._get_button_release(req_plus=False, req_minus=True)
def test_no_false_positive_same_toggle(self) -> None:
self.sla.update_buttons(self.tracker.release_toggle)
self.sla.update_buttons(self.tracker.release_toggle)
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
assert not self.sla._get_button_release(req_plus=False, req_minus=True)
def test_button_confirm_expires(self) -> None:
self._run_ctrl_frames([
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
])
self.sla.update_buttons(self.tracker.release_toggle)
time.sleep(CRUISE_BUTTON_CONFIRM_HOLD + 0.1)
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
@@ -0,0 +1,26 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from opendbc.car import structs
class ButtonStateTracker:
def __init__(self) -> None:
self.pressed: int = 0
self.release_toggle: int = 0
def update(self, CS: structs.CarState) -> None:
for b in CS.buttonEvents:
bit = 1 << b.type.raw
if b.pressed:
self.pressed |= bit
else:
self.pressed &= ~bit
self.release_toggle ^= bit
def publish(self, ss_sp) -> None:
ss_sp.buttonsPressed = self.pressed
ss_sp.buttonsReleaseToggle = self.release_toggle
@@ -0,0 +1,67 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from opendbc.car.structs import car
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
ButtonEvent = car.CarState.ButtonEvent
ButtonType = car.CarState.ButtonEvent.Type
class TestButtonStateTracker:
def setup_method(self) -> None:
self.tracker = ButtonStateTracker()
def make_cs(self, events: list) -> car.CarState:
CS = car.CarState()
CS.buttonEvents = events
return CS
def test_initial_state(self) -> None:
assert self.tracker.pressed == 0
assert self.tracker.release_toggle == 0
def test_press_sets_bit(self) -> None:
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
assert self.tracker.pressed == (1 << ButtonType.accelCruise)
assert self.tracker.release_toggle == 0
def test_release_clears_and_toggles(self) -> None:
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
assert self.tracker.pressed == 0
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
def test_multiple_buttons(self) -> None:
self.tracker.update(self.make_cs([
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
]))
assert self.tracker.pressed == (1 << ButtonType.accelCruise) | (1 << ButtonType.decelCruise)
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
assert self.tracker.pressed == (1 << ButtonType.decelCruise)
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
def test_release_toggle_flips(self) -> None:
for _ in range(2):
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)]))
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]))
assert self.tracker.release_toggle == 0
def test_publish(self) -> None:
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]))
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
class MockSP:
buttonsPressed = 0
buttonsReleaseToggle = 0
sp = MockSP()
self.tracker.publish(sp)
assert sp.buttonsPressed == self.tracker.pressed
assert sp.buttonsReleaseToggle == self.tracker.release_toggle
-30
View File
@@ -1,30 +0,0 @@
#!/usr/bin/env bash
set -e
SCRIPT_DIR=$(dirname "$0")
OPENPILOT_DIR=$SCRIPT_DIR/../../
DOCKER_IMAGE=sunnypilot
DOCKER_FILE=Dockerfile.openpilot
DOCKER_REGISTRY=ghcr.io/sunnypilot
COMMIT_SHA=$(git rev-parse HEAD)
if [ -n "$TARGET_ARCHITECTURE" ]; then
PLATFORM="linux/$TARGET_ARCHITECTURE"
TAG_SUFFIX="-$TARGET_ARCHITECTURE"
else
PLATFORM="linux/$(uname -m)"
TAG_SUFFIX=""
fi
LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX
REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG
REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA
DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR
if [ -n "$PUSH_IMAGE" ]; then
docker push $REMOTE_TAG
docker tag $REMOTE_TAG $REMOTE_SHA_TAG
docker push $REMOTE_SHA_TAG
fi