From 63243d7e2d768514b61b5cd5d357200ecab7e208 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 28 Oct 2026 08:39:55 -0500 Subject: [PATCH] Preserve Encouragement Through Acceleration Noise --- .../starpilot/longitudinal/encouragement.py | 10 +++++- .../encouragement_clear_road_noise.json | 1 + .../longitudinal/tests/test_encouragement.py | 33 +++++++++++++++++++ prebuilt.json | 2 +- 4 files changed, 44 insertions(+), 2 deletions(-) create mode 100644 openpilot/starpilot/longitudinal/tests/fixtures/encouragement_clear_road_noise.json diff --git a/openpilot/starpilot/longitudinal/encouragement.py b/openpilot/starpilot/longitudinal/encouragement.py index 498ecfe9e7..72f6cae621 100644 --- a/openpilot/starpilot/longitudinal/encouragement.py +++ b/openpilot/starpilot/longitudinal/encouragement.py @@ -10,6 +10,7 @@ SETTLE_SECONDS = 3. MIN_HESITATION_GAP = 1. HESITATION_ACCEL = .15 RECOVERY_ACCEL = .3 +HESITATION_FILTER_SECONDS = .5 class Encouragement: @@ -18,6 +19,7 @@ class Encouragement: self.boost = 0. self.drive_id = None self.last_model_ns = 0 + self.hesitation_accel: float | None = None def update(self, sm, cp, *, now_ns, drive_id, dt, target_speed, model_accel, mpc_accel, cruise_accel, lateral_accel, blocked): @@ -52,8 +54,14 @@ class Encouragement: self.last_model_ns = model_ns if not eligible: self.clear_seconds = self.boost = 0. + self.hesitation_accel = None return model_accel - if self.boost == 0. and (target_speed - car.vEgo < MIN_HESITATION_GAP or car.aEgo > HESITATION_ACCEL): + if self.hesitation_accel is None: + self.hesitation_accel = car.aEgo + else: + self.hesitation_accel += dt / (HESITATION_FILTER_SECONDS + dt) * (car.aEgo - self.hesitation_accel) + if self.boost == 0. and (target_speed - car.vEgo < MIN_HESITATION_GAP or + car.aEgo >= RECOVERY_ACCEL or self.hesitation_accel > HESITATION_ACCEL): self.clear_seconds = 0. return model_accel self.clear_seconds += dt diff --git a/openpilot/starpilot/longitudinal/tests/fixtures/encouragement_clear_road_noise.json b/openpilot/starpilot/longitudinal/tests/fixtures/encouragement_clear_road_noise.json new file mode 100644 index 0000000000..82bbe65e82 --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/fixtures/encouragement_clear_road_noise.json @@ -0,0 +1 @@ +{"route":"8bc6f0b166ab516e/000000dc--1f3c08a411","device_revision":"af48674faee8f2dbe1c30e207a9f0c37cc562fcf","scope":"Recorded speed, acceleration and model request; remaining planner inputs use the existing clear-road test fixture. Not a full route replay.","columns":["vEgo","aEgo","modelAcceleration","setKph","experimental"],"rows":[[24.8428897857666,0.09275878220796585,0.01712176762521267,96.0,true],[24.838369369506836,0.00010180626122746617,0.02303871512413025,96.0,true],[24.838239669799805,-0.006318395957350731,0.026698021218180656,96.0,true],[24.844383239746094,0.037247736006975174,0.029345795512199402,96.0,true],[24.849985122680664,0.06986137479543686,0.027539243921637535,96.0,true],[24.841344833374023,-0.02705453149974346,0.024510828778147697,96.0,true],[24.822601318359375,-0.15818916261196136,0.01917380653321743,96.0,true],[24.822708129882812,-0.10945083945989609,0.015330756083130836,96.0,true],[24.81289291381836,-0.1254749298095703,0.012827284634113312,96.0,true],[24.806671142578125,-0.12676775455474854,0.017004789784550667,96.0,true],[24.793079376220703,-0.19111204147338867,0.024288976565003395,96.0,true],[24.79303550720215,-0.1164826825261116,0.031354427337646484,96.0,true],[24.79088020324707,-0.07821905612945557,0.036885444074869156,96.0,true],[24.793472290039062,-0.02040817216038704,0.04261678829789162,96.0,true],[24.802480697631836,0.05776520073413849,0.04282074049115181,96.0,true],[24.798532485961914,0.0017337828176096082,0.043443143367767334,96.0,true],[24.799062728881836,0.003675724845379591,0.04382007569074631,96.0,true],[24.803747177124023,0.051101695746183395,0.049836110323667526,96.0,true],[24.806360244750977,0.06001850962638855,0.05088072642683983,96.0,true],[24.804718017578125,0.022342205047607422,0.05146513506770134,96.0,true],[24.79849624633789,-0.03783663734793663,0.05450846999883652,96.0,true],[24.80587387084961,0.022367432713508606,0.06083260104060173,96.0,true],[24.810142517089844,0.05377781018614769,0.06318745762109756,96.0,true],[24.80190658569336,-0.03387061133980751,0.06428128480911255,96.0,true],[24.802410125732422,-0.017365343868732452,0.06445758789777756,96.0,true],[24.808378219604492,0.04212447628378868,0.061908259987831116,96.0,true],[24.806148529052734,0.003683564718812704,0.05615221709012985,96.0,true],[24.80396842956543,-0.027033085003495216,0.047981586307287216,96.0,true],[24.803380966186523,-0.021636707708239555,0.04106529802083969,96.0,true],[24.81167221069336,0.06318787485361099,0.038509029895067215,96.0,true],[24.818700790405273,0.10370618849992752,0.0346960723400116,96.0,true],[24.826095581054688,0.12022710591554642,0.029369590803980827,96.0,true],[24.83063507080078,0.09150933474302292,0.028308985754847527,96.0,true],[24.832542419433594,0.06187552213668823,0.0325084887444973,96.0,true],[24.83472442626953,0.04932554438710213,0.038312096148729324,96.0,true],[24.845060348510742,0.11722565442323685,0.04142570123076439,96.0,true],[24.847652435302734,0.09927523881196976,0.046010274440050125,96.0,true],[24.837987899780273,-0.01627694070339203,0.05288943275809288,96.0,true],[24.837156295776367,-0.013299399055540562,0.05526435002684593,96.0,true],[24.834699630737305,-0.024964293465018272,0.0557754747569561,96.0,true],[24.837949752807617,0.008511592634022236,0.058307014405727386,96.0,true],[24.839033126831055,0.01794644631445408,0.06359823793172836,96.0,true],[24.838428497314453,0.0174248106777668,0.06282995641231537,96.0,true],[24.83913803100586,0.026319097727537155,0.061130180954933167,96.0,true],[24.832355499267578,-0.03981902450323105,0.0571427084505558,96.0,true],[24.836271286010742,-0.0028232731856405735,0.05886467173695564,96.0,true],[24.841442108154297,0.04141177609562874,0.05507507547736168,96.0,true],[24.8579044342041,0.13398322463035583,0.05336645245552063,96.0,true],[24.853002548217773,0.053674228489398956,0.04742252826690674,96.0,true],[24.855127334594727,0.045971501618623734,0.04463990777730942,96.0,true],[24.863136291503906,0.08603562414646149,0.04273423179984093,96.0,true],[24.872148513793945,0.13047920167446136,0.042920153588056564,96.0,true],[24.88032341003418,0.1380923092365265,0.03902968764305115,96.0,true],[24.885011672973633,0.12329734861850739,0.04038400575518608,96.0,true],[24.889644622802734,0.09704408794641495,0.045578256249427795,96.0,true],[24.901153564453125,0.15221120417118073,0.05057477578520775,96.0,true],[24.891809463500977,0.0166784655302763,0.05015671253204346,96.0,true],[24.8890438079834,-0.023566704243421555,0.04890330880880356,96.0,true],[24.886075973510742,-0.041416235268116,0.0463431216776371,96.0,true],[24.891693115234375,0.009615566581487656,0.04222701117396355,96.0,true],[24.90023422241211,0.08297374844551086,0.03447007015347481,96.0,true],[24.895143508911133,0.018978163599967957,0.026404760777950287,96.0,true],[24.896770477294922,0.024506667628884315,0.019502663984894753,96.0,true],[24.889007568359375,-0.04611498862504959,0.011486322619020939,96.0,true],[24.888980865478516,-0.03842372074723244,0.003801114624366164,96.0,true],[24.888336181640625,-0.027402468025684357,0.00156845198944211,96.0,true],[24.88798713684082,-0.016298450529575348,0.005225590895861387,96.0,true],[24.88896369934082,0.0035255970433354378,0.013268658891320229,96.0,true],[24.883634567260742,-0.04028395190834999,0.022925451397895813,96.0,true],[24.886436462402344,-0.009126788936555386,0.02870102785527706,96.0,true],[24.898387908935547,0.10070539265871048,0.03598866984248161,96.0,true],[24.908876419067383,0.14716671407222748,0.039908722043037415,96.0,true],[24.91356086730957,0.15277054905891418,0.04397657513618469,96.0,true],[24.924230575561523,0.17919614911079407,0.0468202605843544,96.0,true],[24.928987503051758,0.15295271575450897,0.05012690648436546,96.0,true],[24.92745018005371,0.07328998297452927,0.0512768030166626,96.0,true],[24.930030822753906,0.06058600917458534,0.05269992724061012,96.0,true],[24.935291290283203,0.08916196972131729,0.051355935633182526,96.0,true],[24.925373077392578,-0.026196641847491264,0.04377170279622078,96.0,true],[24.927366256713867,0.004496083594858646,0.035177942365407944,96.0,true],[24.91891098022461,-0.06328055262565613,0.0332256481051445,96.0,true],[24.921222686767578,-0.03304330259561539,0.040118519216775894,96.0,true],[24.92186737060547,-0.008584922179579735,0.04880169406533241,96.0,true],[24.921924591064453,-0.001365946140140295,0.051654234528541565,96.0,true],[24.91988182067871,-0.0034628601279109716,0.04927141219377518,96.0,true],[24.92007064819336,-0.00801014807075262,0.05789873003959656,96.0,true],[24.93352699279785,0.11467602103948593,0.07239776849746704,96.0,true],[24.932958602905273,0.07224900275468826,0.0815226137638092,96.0,true],[24.93302345275879,0.03209598362445831,0.0857984647154808,96.0,true],[24.941373825073242,0.09557466953992844,0.09991230815649033,96.0,true],[24.937475204467773,0.021452011540532112,0.11560742557048798,96.0,true],[24.946805953979492,0.09693202376365662,0.1243954449892044,96.0,true],[24.935293197631836,-0.029103334993124008,0.12778650224208832,96.0,true],[24.94087028503418,0.010203639045357704,0.13665378093719482,96.0,true],[24.939491271972656,0.007343429606407881,0.1473080813884735,96.0,true],[24.94407081604004,0.04458480700850487,0.15272867679595947,96.0,true],[24.9530086517334,0.10265133529901505,0.14997102320194244,96.0,true],[24.944589614868164,-0.011564242653548717,0.14913591742515564,96.0,true],[24.9539794921875,0.06563642621040344,0.1478293389081955,96.0,true],[24.952957153320312,0.03567939251661301,0.13862764835357666,96.0,true],[24.95639419555664,0.04692801088094711,0.12604114413261414,96.0,true],[24.966873168945312,0.11925972253084183,0.11643633991479874,96.0,true],[24.969282150268555,0.07429195940494537,0.11175420880317688,96.0,true],[24.97561264038086,0.09211070090532303,0.10164414346218109,96.0,true],[24.97374725341797,0.04274860396981239,0.09128711372613907,96.0,true],[24.9985408782959,0.20817476511001587,0.08596824109554291,96.0,true],[24.996158599853516,0.12970463931560516,0.09300976991653442,96.0,true],[24.99248695373535,0.04316392540931702,0.09642165154218674,96.0,true],[24.997133255004883,0.0821438729763031,0.09930974990129471,96.0,true],[24.999168395996094,0.051318053156137466,0.10460295528173447,96.0,true],[25.007137298583984,0.07615192234516144,0.11388099938631058,96.0,true],[25.013198852539062,0.09883963316679001,0.12008557468652725,96.0,true],[25.018823623657227,0.09524913132190704,0.12398835271596909,96.0,true],[25.022823333740234,0.10333400964736938,0.1272919774055481,96.0,true],[25.022138595581055,0.06565183401107788,0.13263708353042603,96.0,true],[25.03289794921875,0.13374194502830505,0.13506273925304413,96.0,true],[25.031492233276367,0.06648893654346466,0.13216863572597504,96.0,true],[25.040706634521484,0.09669184684753418,0.12687034904956818,96.0,true],[25.03687286376953,0.044707853347063065,0.11953696608543396,96.0,true],[25.045074462890625,0.08145260065793991,0.10928154736757278,96.0,false],[25.0545597076416,0.12990061938762665,0.1022496446967125,96.0,true],[25.05936622619629,0.12072364240884781,0.0970468670129776,96.0,true],[25.073726654052734,0.17489749193191528,0.09549129754304886,96.0,true],[25.082624435424805,0.18236535787582397,0.09687309712171555,96.0,true],[25.084714889526367,0.1245071217417717,0.10328997671604156,96.0,true],[25.092985153198242,0.14741754531860352,0.11007102578878403,96.0,true],[25.09085464477539,0.05478587746620178,0.11701042205095291,96.0,true],[25.105335235595703,0.14463849365711212,0.12183505296707153,96.0,true],[25.109159469604492,0.11840344965457916,0.12771806120872498,96.0,true],[25.121976852416992,0.1833639293909073,0.132547989487648,96.0,true],[25.130512237548828,0.1601291298866272,0.13303835690021515,96.0,true],[25.135427474975586,0.16106262803077698,0.13135455548763275,96.0,true],[25.143939971923828,0.14488281309604645,0.1279802918434143,96.0,true],[25.14504051208496,0.10760556161403656,0.12467427551746368,96.0,true],[25.154953002929688,0.1251167207956314,0.1202266737818718,96.0,true],[25.156652450561523,0.11154737323522568,0.11556234210729599,96.0,true],[25.15854263305664,0.05177555978298187,0.11296334862709045,96.0,true],[25.170671463012695,0.12975458800792694,0.1125623881816864,96.0,true],[25.178634643554688,0.13701479136943817,0.11447178572416306,96.0,true],[25.19451332092285,0.21279019117355347,0.1123400554060936,96.0,true],[25.194664001464844,0.13048696517944336,0.11188486218452454,96.0,true],[25.19589614868164,0.10438530892133713,0.11089987307786942,96.0,true],[25.207469940185547,0.1556883007287979,0.11066577583551407,96.0,true],[25.204120635986328,0.07373019307851791,0.10731929540634155,96.0,true],[25.228342056274414,0.24831831455230713,0.10583584010601044,96.0,true],[25.23714256286621,0.21937841176986694,0.10263115912675858,96.0,true],[25.239521026611328,0.1267850250005722,0.1011178195476532,96.0,true],[25.244882583618164,0.1352718472480774,0.09878735989332199,96.0,true],[25.238969802856445,0.03243398666381836,0.09351642429828644,96.0,true],[25.245594024658203,0.07396842539310455,0.08695579320192337,96.0,true],[25.243492126464844,0.0383421890437603,0.08200202137231827,96.0,true],[25.251405715942383,0.07674277573823929,0.0802074670791626,96.0,true],[25.25144386291504,0.05387159436941147,0.07479048520326614,96.0,true],[25.2508544921875,0.0187094546854496,0.07140447199344635,96.0,true],[25.259441375732422,0.07723577320575714,0.06838835030794144,96.0,true],[25.255220413208008,0.018294312059879303,0.06508565694093704,96.0,true],[25.253829956054688,0.0002947291068267077,0.059891264885663986,96.0,true],[25.258464813232422,0.036296166479587555,0.05894247069954872,96.0,true],[25.259967803955078,0.01824694499373436,0.05813933536410332,96.0,true],[25.260534286499023,0.01778491772711277,0.05596029385924339,96.0,true]]} diff --git a/openpilot/starpilot/longitudinal/tests/test_encouragement.py b/openpilot/starpilot/longitudinal/tests/test_encouragement.py index f84993f973..247497e0f0 100644 --- a/openpilot/starpilot/longitudinal/tests/test_encouragement.py +++ b/openpilot/starpilot/longitudinal/tests/test_encouragement.py @@ -1,3 +1,5 @@ +import json +from pathlib import Path from typing import TypedDict from unittest.mock import patch @@ -309,3 +311,34 @@ def test_native_planner_reaches_clear_road_boost_plateau(factory, car): assert planner.output_a_target <= planner.a_cruise + 1e-6 assert planner.encouragement.boost == pytest.approx(MAX_BOOST) assert planner.output_a_target == pytest.approx(MAX_BOOST, abs=.01) + + +@pytest.mark.parametrize('factory,car', [(HondaInterface, HONDA.HONDA_CIVIC), + (HyundaiInterface, HYUNDAI.HYUNDAI_IONIQ_6), + (ToyotaInterface, TOYOTA.TOYOTA_RAV4_TSS2)]) +def test_native_planner_recovers_despite_recorded_acceleration_noise(factory, car): + tape = json.loads((Path(__file__).parent / 'fixtures' / 'encouragement_clear_road_noise.json').read_text()) + params = factory.get_non_essential_params(car) + params.openpilotLongitudinalControl = True + planner = LongitudinalPlanner(params, init_v=tape['rows'][0][0]) + maximum_boost = maximum_extra = 0. + for tick in range(40): + sm, now = frame(tick) + sm['carState'].vEgo = tape['rows'][0][0] + sm['modelV2'].action.desiredAcceleration = -.1 + planner.update(sm, now_ns=now, curve_ceiling=CurveCeiling(20., sm.logMonoTime['modelV2'])) + assert planner.encouragement.boost == 0. + assert planner.output_a_target <= planner.a_cruise + 1e-6 + for offset, (speed, accel, model_accel, set_kph, experimental) in enumerate(tape['rows']): + sm, now = frame(offset + 40) + sm['carState'].vEgo, sm['carState'].aEgo, sm['carState'].vCruise = speed, accel, set_kph + sm['selfdriveState'].experimentalMode = experimental + sm['modelV2'].action.desiredAcceleration = model_accel + planner.update(sm, now_ns=now) + assert planner.mpc.solution_status == 0 + assert planner.output_a_target <= planner.a_cruise + 1e-6 + assert not planner.output_should_stop + maximum_boost = max(maximum_boost, planner.encouragement.boost) + maximum_extra = max(maximum_extra, planner.output_a_target - model_accel) + assert maximum_boost > .1 + assert maximum_extra > .05 diff --git a/prebuilt.json b/prebuilt.json index 2bf574f723..94e52e1789 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "4627d1bed9bc481df0b0d41734adf62966be0a41ab45407749ffbab884741bef", "schema_version": 1} +{"protected_inventory_sha256": "a27afad4c569dd619c0917fd680561638a23478786581e1917e8c3c5df248786", "schema_version": 1}