Compare commits

..

129 Commits

Author SHA1 Message Date
royjr
672053db65 Update display_panel.cc 2025-11-08 09:57:57 -08:00
royjr
774710e95e Squashed commit of the following:
commit 669f3b7945
Author: royjr <royjr96@gmail.com>
Date:   Fri Nov 7 21:51:35 2025 -0500

    fix

commit 63122e1a33
Merge: c9b1afb15 c1d3ae427
Author: royjr <royjr96@gmail.com>
Date:   Fri Nov 7 21:25:46 2025 -0500

    Merge branch 'master' into visual-style

commit c9b1afb154
Merge: 6e3bd3fbe 9b92cdd2c
Author: royjr <royjr96@gmail.com>
Date:   Fri Oct 10 00:06:55 2025 -0400

    Merge branch 'master' into visual-style

commit 6e3bd3fbed
Author: royjr <royjr96@gmail.com>
Date:   Thu Oct 9 23:56:37 2025 -0400

    explicit radius

commit 42592dd550
Author: royjr <royjr96@gmail.com>
Date:   Thu Oct 9 23:54:34 2025 -0400

    match what we currently send

commit b2f7d72a33
Author: royjr <royjr96@gmail.com>
Date:   Thu Oct 9 23:53:07 2025 -0400

    no need

commit 2e0ce18c84
Author: royjr <royjr96@gmail.com>
Date:   Thu Oct 9 23:52:51 2025 -0400

    group

commit 2a9a4a9263
Merge: e63fb10fd d6317ffd2
Author: royjr <royjr96@gmail.com>
Date:   Thu Oct 9 23:51:18 2025 -0400

    Merge branch 'master' into visual-style

commit e63fb10fdb
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:35:23 2025 -0400

    Revert "metric threshold"

    This reverts commit b54941928d.

commit b54941928d
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:35:15 2025 -0400

    metric threshold

commit b85b8ffacf
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:28:10 2025 -0400

    better VisualStyleOverheadThreshold

commit 3ff2e9b26a
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:25:37 2025 -0400

    reorder

commit 4b3ffc722a
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:24:52 2025 -0400

    show visual_radar_tracks_delay_settings on VisualRadarTracks

commit 5f49066829
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:18:51 2025 -0400

    more

commit ab2eb218d5
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:16:06 2025 -0400

    clean

commit 6212b174e9
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 21:12:08 2025 -0400

    descs

commit a8a6e5708a
Merge: 54b060f17 ae21d40a1
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:47:03 2025 -0400

    Merge branch 'master' into visual-style

commit 54b060f178
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:45:17 2025 -0400

    move with

commit 8266386cd0
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:44:06 2025 -0400

    better

commit 8bbe87ee22
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:40:23 2025 -0400

    simple

commit d35ac0c145
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:34:26 2025 -0400

    a bit better

commit a134ae1e29
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:25:55 2025 -0400

    combine

commit 6a69759b9e
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 20:24:51 2025 -0400

    hide options based on options

commit b9063e2966
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:55:40 2025 -0400

    cleanup

commit 4397a4387a
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:49:14 2025 -0400

    mooooore fps

commit 32dc384524
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:47:08 2025 -0400

    not needed for now

commit 68fa239b97
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:45:37 2025 -0400

    unused

commit c8367fbc25
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:44:45 2025 -0400

    more fps remove

commit 76fc4514e1
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:43:32 2025 -0400

    reorder

commit 073ce2b4df
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 19:35:37 2025 -0400

    remove VisualFPS

commit 5d516ba89f
Merge: e819a0dcb 014baf8e9
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 12:26:38 2025 -0400

    Merge branch 'master' into visual-style

commit e819a0dcb1
Merge: ba4b583e6 8050c56a4
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 02:14:22 2025 -0400

    Merge branch 'master' into visual-style

commit ba4b583e6e
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:55:38 2025 -0400

    fix visual_style_overhead_zoom

commit 5954354356
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:48:06 2025 -0400

    fix visual_style_overhead

commit 23ff232333
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:40:10 2025 -0400

    fix visual_style_overhead_settings

commit 0bb47fcfa9
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:29:15 2025 -0400

    fix visual_style_overhead_threshold_settings

commit 99a5682371
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:09:08 2025 -0400

    todo

commit 22ca343050
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:07:53 2025 -0400

    fix visual_style_overhead_threshold_settings

commit 0049d20151
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 01:05:17 2025 -0400

    VisualStyleZoom fix

commit b9f8f4e8ac
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 00:55:20 2025 -0400

    visual style better

commit 26564dd42f
Author: royjr <royjr96@gmail.com>
Date:   Wed Oct 8 00:07:31 2025 -0400

    better VisualWideCam

commit ec440e4568
Merge: 69817f887 408d52d72
Author: royjr <royjr96@gmail.com>
Date:   Tue Oct 7 22:38:57 2025 -0400

    Merge branch 'master' into visual-style

commit 69817f887b
Merge: df21208b7 0f4828df8
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 30 15:01:21 2025 -0400

    Merge branch 'master' into visual-style

commit df21208b7c
Merge: 660c994c5 082ea8119
Author: royjr <royjr96@gmail.com>
Date:   Thu Sep 25 02:08:38 2025 -0400

    Merge branch 'master' into visual-style

commit 660c994c5e
Author: royjr <royjr96@gmail.com>
Date:   Thu Sep 25 02:08:28 2025 -0400

    visual_style_blend  vs visual_style_overhead_blend

commit 904cc796b0
Author: royjr <royjr96@gmail.com>
Date:   Thu Sep 25 01:44:06 2025 -0400

    prevent jitter

commit 1a9a1e1b8a
Merge: 2ae7078c0 1465e38c7
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 23:00:46 2025 -0400

    Merge branch 'master' into visual-style

commit 2ae7078c0f
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 14:37:21 2025 -0400

    use ParamWatcher

commit 0fde830a30
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 10:35:46 2025 -0400

    fix params

commit 7c744a42e5
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 08:18:51 2025 -0400

    safe font

commit 90cc169dec
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 08:17:59 2025 -0400

    VisualRadarTracksDelay

commit 36c1e11a9e
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 07:47:16 2025 -0400

    add FPS toggle

commit 5e26a99337
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 07:40:59 2025 -0400

    better fps

commit 53683cf90b
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 07:25:00 2025 -0400

    constant track size

commit 95dcc23887
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 07:21:57 2025 -0400

    better tracks for overhead

commit cef3163c7c
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 06:54:46 2025 -0400

    more more more cached params

commit cbc34dd1f6
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 06:49:41 2025 -0400

    more cached params

commit fbd1f6bad1
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 06:42:14 2025 -0400

    cached params

commit f2ddf9abba
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 04:04:52 2025 -0400

    debug ui lag

commit 8a5eaf5ba6
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 03:35:06 2025 -0400

    prepare for lag compensation

commit ddb377ac5b
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 03:24:15 2025 -0400

    radar lag compensate

commit 512a01d28c
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 01:33:50 2025 -0400

    VisualWideCam toggle (untested)

commit b30e275169
Merge: ee9fe5c9e ecee67dd6
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 01:15:18 2025 -0400

    Merge branch 'master' into visual-style

commit ee9fe5c9ed
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 01:10:44 2025 -0400

    VisualRadarTracks toggle

commit e672a352d3
Author: royjr <royjr96@gmail.com>
Date:   Tue Sep 23 00:24:38 2025 -0400

    bigger points

commit 361d107040
Author: royjr <royjr96@gmail.com>
Date:   Mon Sep 22 23:55:55 2025 -0400

    basic radar

commit 36eb047cd3
Merge: 218c6172e d5a873ed8
Author: royjr <royjr96@gmail.com>
Date:   Mon Sep 22 00:10:31 2025 -0400

    Merge branch 'master' into visual-style

commit 218c6172e6
Author: royjr <royjr96@gmail.com>
Date:   Mon Sep 22 00:03:29 2025 -0400

    darker fills

commit 8f35e4fc3c
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 23:02:48 2025 -0400

    Revert "smooooth"

    This reverts commit c965df39d6.

commit c965df39d6
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 22:10:35 2025 -0400

    smooooth

commit 53ef69f3c3
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 21:55:23 2025 -0400

    hide horizon if no data

commit ea9ca18c8b
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 21:49:01 2025 -0400

    horizon at end

commit 225858261e
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 21:34:14 2025 -0400

    better horizon

commit 43de43b34a
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 21:25:39 2025 -0400

    better lines

commit 96ddbe35a1
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 15:37:54 2025 -0400

    dynamically adjust background

commit 7d547ad533
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 15:30:13 2025 -0400

    fix default

commit 13de58b845
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 15:30:06 2025 -0400

    add more speeds

commit 3d8c563a4b
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 15:19:57 2025 -0400

    dynamic zoom

commit bc75199d5a
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 14:34:38 2025 -0400

    overhead

commit ba6e18ed91
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 14:32:30 2025 -0400

    only for vision

commit 74dbcd699b
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 14:31:58 2025 -0400

    add road edges to vision

commit 35c6af0190
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 13:55:56 2025 -0400

    theme

commit 827de88c8b
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 13:03:37 2025 -0400

    stump top down

commit 54ca4b537d
Author: royjr <royjr96@gmail.com>
Date:   Sun Sep 21 12:48:32 2025 -0400

    VisualStyleBlendThreshold

commit c61f327076
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 13:04:07 2025 -0400

    allow always overhead view

commit d569913e5a
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 12:57:17 2025 -0400

    VisualStyleBlend

commit 418c93be06
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 12:16:49 2025 -0400

    animate overhead

commit 5acc040a89
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 11:24:24 2025 -0400

    overhead

commit e45b17c230
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 03:00:00 2025 -0400

    better

commit b14b6246d7
Author: royjr <royjr96@gmail.com>
Date:   Sat Sep 20 00:31:07 2025 -0400

    visual style init
2025-11-07 22:11:17 -05:00
royjr
8cdc236585 Merge branch 'master' into nav-commacon-visual-style 2025-11-07 22:10:20 -05:00
discountchubbs
ec031ffeb6 n overlap and more code diff! 2025-11-07 17:59:23 -08:00
discountchubbs
9c5ee2e12a start cleanup 2025-11-07 12:37:22 -08:00
discountchubbs
65386da6d4 yesssss 2025-11-07 09:34:52 -08:00
discountchubbs
00799154ef merge origin/nav-desires into nav-commacon 2025-11-07 07:47:16 -08:00
discountchubbs
d6731c30da play with moving stuff when not valid 2025-11-07 07:05:24 -08:00
discountchubbs
0b54b86476 rounding 2025-11-06 15:47:31 -08:00
discountchubbs
fd675d29a9 no more nav-events 2025-11-06 12:25:27 -08:00
discountchubbs
d58bbda789 merge origin/nav-events into nav-commacon 2025-11-05 19:28:38 -08:00
discountchubbs
50773a62ed fine 2025-11-05 17:47:16 -08:00
discountchubbs
f5dbdc192e YOUR DESTINATION IS ON THE RIGHT 2025-11-05 15:28:52 -08:00
discountchubbs
f9987b2d7d quick don't look 2025-11-05 14:37:44 -08:00
discountchubbs
d51af1513b add to navigationd-service , but add here first because yessss 2025-11-05 14:30:02 -08:00
discountchubbs
2807c949a9 nayan! 2025-11-05 09:58:18 -08:00
nayan
0268d4384d Move Elements 2025-11-05 11:27:49 -05:00
nayan
0e60a56e56 Update idx to allow next maneuver 2025-11-05 11:02:21 -05:00
nayan
c5445d2a8b Make it bigger. BIGGER! 2025-11-05 11:02:21 -05:00
discountchubbs
6df1edbbaf offroad panel stuffs 2025-11-05 06:57:21 -08:00
discountchubbs
a098a0805a fix index access 2025-11-05 06:08:31 -08:00
nayan
469f1e01ef LOL, keep these 2025-11-04 16:28:23 -05:00
nayan
0fc5414f7c NAV DIRECTIONS UI 2025-11-04 16:22:37 -05:00
James Vecellio
d798288b2c more 2025-11-03 06:54:03 -08:00
James Vecellio
0e02fc2449 commacon stuff 2025-11-02 18:53:53 -08:00
James Vecellio-Grant
7d15afe5bc Merge branch 'nav-desires' into nav-events 2025-11-01 08:02:31 -07:00
James Vecellio-Grant
b6dd2d14db Merge branch 'navigationd-service' into nav-desires 2025-11-01 08:02:18 -07:00
James Vecellio-Grant
7d4e5bedaf Merge branch 'navigationd-init' into navigationd-service 2025-11-01 08:02:08 -07:00
James Vecellio-Grant
1063114408 Merge branch 'master' into navigationd-init 2025-11-01 08:01:57 -07:00
discountchubbs
958b4df69f give slightly more leniancy for offline routing 2025-11-01 08:01:30 -07:00
discountchubbs
f5953c5d8c Quarter mile is what apple maps uses 2025-11-01 07:57:55 -07:00
discountchubbs
72998034e6 copyright 2025-10-30 06:26:01 -07:00
discountchubbs
cefb344183 copyright 2025-10-30 06:22:34 -07:00
discountchubbs
c0da31abb6 mild clean 2025-10-30 06:18:47 -07:00
discountchubbs
bd759a56cf ehh no, 50 ft increments is better 2025-10-29 20:05:16 -07:00
discountchubbs
befc73c53e more 2025-10-29 20:03:19 -07:00
discountchubbs
8dbfc267ac Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-29 19:41:51 -07:00
discountchubbs
d17e80ad94 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-29 19:41:29 -07:00
discountchubbs
c2b7087723 Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-29 19:40:19 -07:00
James Vecellio-Grant
81b37712f1 Merge branch 'master' into navigationd-init 2025-10-29 19:39:41 -07:00
discountchubbs
68270a13a3 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-29 19:38:20 -07:00
discountchubbs
18cd3633e5 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-29 19:37:41 -07:00
discountchubbs
9c6a4d4a57 Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-29 19:37:10 -07:00
discountchubbs
1a4c48249b fix: handle empty maxspeed list in nav_instructions 2025-10-29 19:36:39 -07:00
James Vecellio-Grant
95d887a417 Update event_builder.py 2025-10-29 18:11:22 -07:00
James Vecellio-Grant
e297b4c03f Update event_builder.py 2025-10-29 18:06:28 -07:00
discountchubbs
1132377837 refactor event_builder into class with staticmethod for events.py call 2025-10-29 15:42:23 -07:00
discountchubbs
35f03ae001 Well im conflicted 2025-10-28 11:52:32 -07:00
discountchubbs
1c0b54a447 more 2025-10-28 06:55:28 -07:00
discountchubbs
8f0cdd514e weird 2025-10-28 06:54:50 -07:00
discountchubbs
3681caa717 Add nudge notif to event builder 2025-10-28 06:46:39 -07:00
discountchubbs
7446c43f69 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-28 06:44:01 -07:00
discountchubbs
5f5e3668eb Add steering pressed and torque to desire for non blindspot cars, and a note! 2025-10-28 06:43:00 -07:00
discountchubbs
8c07958f6f Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-28 06:28:33 -07:00
discountchubbs
ca1ce9bcc9 Clear route cache more gracefully to allow we based route connection to happen quick ish 2025-10-28 06:27:51 -07:00
discountchubbs
34a0819bc5 Read the description:
It's ideal that we should always use the next instruction ahead instead of our current instruction that we are driving on. Sure, current instruction is great for grepping maxspeed tags, but for upcoming instructions we want to use index 1.

I updated the test to reflect the two indexes.
2025-10-28 06:23:40 -07:00
James Vecellio-Grant
c68ea82a5d hahaha it’ll fail the test but that’s a later problem 2025-10-27 19:27:37 -07:00
discountchubbs
3157054100 a bit more readible 2025-10-26 13:02:09 -07:00
discountchubbs
2486ef1825 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-26 11:07:49 -07:00
discountchubbs
29f15dc8ed Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-26 11:07:16 -07:00
discountchubbs
31a5a3b3c0 assertion comparison operators 2025-10-26 11:06:00 -07:00
discountchubbs
d8fa3cfd04 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-26 08:45:24 -07:00
discountchubbs
2a4b348497 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-26 08:44:38 -07:00
discountchubbs
3ef3aceb4b Apply auto cancel route after 10 seconds off route. 2025-10-26 08:44:03 -07:00
James Vecellio-Grant
3d8763b3ce Merge branch 'master' into navigationd-init 2025-10-25 21:14:39 -07:00
James Vecellio-Grant
b2427a5f20 Merge branch 'master' into navigationd-init 2025-10-25 16:27:44 -07:00
discountchubbs
7ddafe62cd Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-23 19:21:25 -07:00
discountchubbs
ff4cc96a81 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-23 19:21:04 -07:00
discountchubbs
3b1ada64be sync 2025-10-23 19:20:15 -07:00
discountchubbs
6a08186434 Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-23 19:17:49 -07:00
discountchubbs
cf2b033c79 clean 2025-10-23 19:15:54 -07:00
discountchubbs
9fbef36c6b desire handling 2025-10-23 19:12:34 -07:00
discountchubbs
f5a38aa613 Add instruction field to maneuvers and update banner message logic 2025-10-23 19:08:52 -07:00
discountchubbs
25f5058430 Merge remote-tracking branch 'origin/nav-desires' into nav-events
# Conflicts:
#	sunnypilot/navd/navigation_helpers/nav_instructions.py
#	sunnypilot/navd/navigationd.py
2025-10-23 17:26:23 -07:00
discountchubbs
7b28c2f59a Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-23 17:24:16 -07:00
discountchubbs
99d954de10 sync 2025-10-23 17:23:47 -07:00
discountchubbs
b28f33481c Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-23 17:21:56 -07:00
discountchubbs
589e33f665 sum red diff 2025-10-23 17:20:23 -07:00
discountchubbs
39342d7b5e diff is red so == good? 2025-10-23 17:16:29 -07:00
discountchubbs
fe70650f73 100 meters 2025-10-23 16:56:58 -07:00
discountchubbs
e3f9fe892a more 2025-10-23 12:21:04 -07:00
discountchubbs
f4373fa244 Revert "dead"
This reverts commit 2376802589.
2025-10-23 12:20:52 -07:00
James Vecellio
2376802589 dead 2025-10-23 09:44:28 -07:00
James Vecellio
c3b51d7335 long maneuvers fix 2025-10-23 09:35:43 -07:00
discountchubbs
d3d8802402 no mid, yet 2025-10-22 19:34:24 -07:00
discountchubbs
d866500c92 daughter wants to take a bath 2025-10-22 19:27:59 -07:00
discountchubbs
23879836d9 return validity 2025-10-22 14:26:15 -07:00
discountchubbs
06add21971 sm shenanigans 2025-10-22 13:47:03 -07:00
discountchubbs
66fd3d1a01 smh 2025-10-22 09:51:33 -07:00
James Vecellio-Grant
71f7754f51 Didn’t like it 2025-10-22 07:45:44 -07:00
James Vecellio-Grant
b5591cbd62 Update plannerd.py 2025-10-22 07:36:56 -07:00
discountchubbs
7430c450c2 longitudinal maneuvers no subscribey 2025-10-22 06:15:52 -07:00
discountchubbs
e54a39cf43 publish dem events 👀 2025-10-21 21:13:33 -07:00
discountchubbs
3afe0bcdb3 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-21 16:27:46 -07:00
discountchubbs
b763f7aac1 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-21 16:27:02 -07:00
discountchubbs
450fcd4d55 oopsie part two lol 2025-10-21 16:25:30 -07:00
James Vecellio
551b4dea31 oopsie 2025-10-21 16:20:20 -07:00
James Vecellio
bd269defb3 oopsie 2025-10-21 16:19:49 -07:00
James Vecellio
8998f63a28 oopsie 2025-10-21 16:18:27 -07:00
James Vecellio-Grant
399ed08926 Merge branch 'master' into navigationd-init 2025-10-21 15:29:45 -07:00
James Vecellio-Grant
90f02040fe Merge branch 'master' into navigationd-service 2025-10-21 15:29:42 -07:00
James Vecellio-Grant
8423ecedb1 Merge branch 'master' into nav-desires 2025-10-21 15:29:39 -07:00
James Vecellio-Grant
34d1514e11 Merge branch 'master' into nav-events 2025-10-21 15:29:35 -07:00
discountchubbs
c50d511616 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-21 15:27:41 -07:00
discountchubbs
dd1479ed82 More macOS crap 🤠 2025-10-21 15:25:15 -07:00
discountchubbs
87ec262e39 Merge remote-tracking branch 'origin/nav-desires' into nav-events 2025-10-21 12:06:07 -07:00
discountchubbs
f82845ff42 Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-21 12:05:13 -07:00
discountchubbs
efcc5ccd15 Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-21 12:03:44 -07:00
James Vecellio-Grant
6aac50ab56 Merge branch 'master' into navigationd-init 2025-10-21 12:03:05 -07:00
discountchubbs
da0920cb60 feat: navigationd onroad events 2025-10-21 11:59:13 -07:00
discountchubbs
091bce4a3a Merge remote-tracking branch 'origin/navigationd-service' into nav-desires 2025-10-21 05:54:42 -07:00
discountchubbs
088f6aa407 sync 2025-10-21 05:53:12 -07:00
James Vecellio-Grant
211c8adcce Merge branch 'master' into navigationd-init 2025-10-21 05:52:06 -07:00
discountchubbs
fe5366e5b2 rm unused import 2025-10-19 20:34:11 -07:00
discountchubbs
1ecb0b0f66 dumb 2025-10-19 20:16:36 -07:00
discountchubbs
51e455db79 stupid msgq always breaking macOS 2025-10-19 20:15:14 -07:00
discountchubbs
dc6672fa80 Merge remote-tracking branch 'origin/navigationd-init' into navigationd-service 2025-10-19 20:02:36 -07:00
discountchubbs
07b8e7783d Do i really need a readme 2025-10-19 19:47:21 -07:00
discountchubbs
f17b0f200c non blocking polling
SOME attribute protection. kids crying, so need to stop for now!
2025-10-19 17:20:13 -07:00
discountchubbs
ad9bde8b1f non blocking polling
SOME attribute protection. kids crying, so need to stop for now!
2025-10-19 17:17:37 -07:00
discountchubbs
8cf9f9fe23 params!
maybe these should be protected attributes, but thats a tomorrow problem
2025-10-19 17:03:04 -07:00
discountchubbs
713985d823 feat: navigationd desire loop
Notes: Maybe I should add a param for this, so its not automatic when a route is set.
2025-10-19 09:38:29 -07:00
James Vecellio-Grant
088f9d0b59 Merge branch 'master' into navigationd-service 2025-10-18 18:22:34 -07:00
James Vecellio-Grant
53bf5b0d41 Merge branch 'master' into navigationd-init 2025-10-18 18:22:27 -07:00
discountchubbs
8c33592628 Revert "feat: navigationd" bc it was supposed to be a branch lol
This reverts commit 3bbb33f6bd.
2025-10-18 18:18:28 -07:00
James Vecellio
3bbb33f6bd feat: navigationd
I changed the reroute counter to 9 updates, which is every 3 seconds.  compared to 3, which is one second.
2025-10-18 18:15:22 -07:00
discountchubbs
5bd9549bd1 some clean up for production 2025-10-16 15:43:34 -07:00
discountchubbs
3481702715 some suggestions applied 2025-10-16 06:55:15 -07:00
discountchubbs
c9781ee31d feat: mapbox navigation helpers 2025-10-16 06:43:48 -07:00
56 changed files with 636 additions and 1683 deletions

View File

@@ -74,7 +74,7 @@ jobs:
env:
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
run: |
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
cd gitlab_docs
git checkout main
git sparse-checkout set --no-cone models/
@@ -191,7 +191,7 @@ jobs:
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
run: |
echo "Cloning GitLab"
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
cd gitlab_docs
echo "checkout models/${RECOMPILED_DIR}"
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}

View File

@@ -109,7 +109,7 @@ jobs:
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
run: |
echo "Cloning GitLab"
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
cd gitlab_docs
echo "checkout models/${RECOMPILED_DIR}"
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}

View File

@@ -156,8 +156,6 @@ jobs:
with:
name: models-${{ env.REF }}${{ inputs.artifact_suffix }}
path: ${{ github.workspace }}/selfdrive/modeld/models
- run: |
rm -f ${{ github.workspace }}/selfdrive/modeld/models/{dmonitoring_model,big_driving_policy,big_driving_vision}.onnx
- name: Build Model
run: |

1
.gitmodules vendored
View File

@@ -4,7 +4,6 @@
[submodule "opendbc"]
path = opendbc_repo
url = https://github.com/sunnypilot/opendbc.git
branch = tn
[submodule "msgq"]
path = msgq_repo
url = https://github.com/sunnypilot/msgq.git

View File

@@ -192,7 +192,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
aTarget @5 :Float32;
events @6 :List(OnroadEventSP.Event);
e2eAlerts @7 :E2eAlerts;
accelPersonality @8 :AccelerationPersonality;
struct DynamicExperimentalControl {
state @0 :DynamicExperimentalControlState;
@@ -204,11 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
blended @1;
}
}
enum AccelerationPersonality {
sport @0;
normal @1;
eco @2;
}
struct SmartCruiseControl {
vision @0 :Vision;
map @1 :Map;
@@ -345,7 +340,6 @@ struct OnroadEventSP @0xda96579883444c35 {
speedLimitChanged @21;
speedLimitPending @22;
e2eChime @23;
laneChangeRoadEdge @24;
}
}
@@ -452,8 +446,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
struct ModelDataV2SP @0xa1680744031fdb2d {
laneTurnDirection @0 :TurnDirection;
leftLaneChangeEdgeBlock @1 :Bool;
rightLaneChangeEdgeBlock @2 :Bool;
enum TurnDirection {
none @0;
@@ -496,84 +488,11 @@ struct CustomReserved15 @0xbd443b539493bc68 {
struct CustomReserved16 @0xfc6241ed8877b611 {
}
enum MapdExtendedOutType {
paths @0;
settings @1;
struct CustomReserved17 @0xa30662f84033036c {
}
struct MapdExtendedOut @0xa30662f84033036c {
type @0 :MapdExtendedOutType;
json @1 :Text;
struct CustomReserved18 @0xc86a3d38d13eb3ef {
}
enum MapdInputType {
download @0;
setTargetLateralAccel @1;
setSpeedLimitOffset @2;
setSpeedLimitControl @3;
setCurveSpeedControl @4;
setVisionCurveSpeedControl @5;
setLogLevel @6;
setVisionCurveTargetLatA @7;
setVisionCurveMinTargetV @8;
reloadSettings @9;
saveSettings @10;
setEnableSpeed @11;
setVisionCurveUseEnableSpeed @12;
setCurveUseEnableSpeed @13;
setSpeedLimitUseEnableSpeed @14;
setHoldLastSeenSpeedLimit @15;
setCurveTargetJerk @16;
setCurveTargetAccel @17;
setCurveTargetOffset @18;
setDefaultLaneWidth @19;
setCurveTargetLatA @20;
loadDefaultSettings @21;
loadRecommendedSettings @22;
setSlowDownForNextSpeedLimit @23;
setSpeedUpForNextSpeedLimit @24;
setHoldSpeedLimitWhileChangingSetSpeed @25;
}
enum SpeedLimitOffsetType {
static @0;
percent @1;
}
struct MapdIn @0xc86a3d38d13eb3ef {
type @0 :MapdInputType;
float @1 :Float32;
str @2 :Text;
bool @3 :Bool;
}
enum RoadContext {
freeway @0;
city @1;
unknown @2;
}
struct MapdOut @0xa4f1eb3323f5f582 {
wayName @0 :Text;
wayRef @1 :Text;
roadName @2 :Text;
speedLimit @3 :Float32;
nextSpeedLimit @4 :Float32;
nextSpeedLimitDistance @5 :Float32;
hazard @6 :Text;
nextHazard @7 :Text;
nextHazardDistance @8 :Float32;
advisorySpeed @9 :Float32;
nextAdvisorySpeed @10 :Float32;
nextAdvisorySpeedDistance @11 :Float32;
oneWay @12 :Bool;
lanes @13 :UInt8;
tileLoaded @14 :Bool;
speedLimitOffset @15 :Float32;
suggestedSpeed @16 :Float32;
estimatedRoadWidth @17 :Float32;
roadContext @18 :RoadContext;
distanceFromWayCenter @19 :Float32;
visionCurveSpeed @20 :Float32;
curveSpeed @21 :Float32;
struct CustomReserved19 @0xa4f1eb3323f5f582 {
}

View File

@@ -2639,9 +2639,9 @@ struct Event {
customReserved14 @140 :Custom.CustomReserved14;
customReserved15 @141 :Custom.CustomReserved15;
customReserved16 @142 :Custom.CustomReserved16;
mapdExtendedOut @143 :Custom.MapdExtendedOut;
mapdIn @144 :Custom.MapdIn;
mapdOut @145 :Custom.MapdOut;
customReserved17 @143 :Custom.CustomReserved17;
customReserved18 @144 :Custom.CustomReserved18;
customReserved19 @145 :Custom.CustomReserved19;
# *********** legacy + deprecated ***********
model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated

View File

@@ -91,7 +91,6 @@ _services: dict[str, tuple] = {
"modelDataV2SP": (True, 20.),
"navigationd": (True, 3.),
"liveLocationKalman": (True, 20.),
"mapdOut": (True, 20., 20),
# debug
"uiDebug": (True, 0., 1),

View File

@@ -130,8 +130,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"Version", {PERSISTENT, STRING}},
// --- sunnypilot params --- //
{"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
@@ -148,7 +146,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}},
{"DeviceBootMode", {PERSISTENT | BACKUP, INT, "0"}},
{"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
{"DynamicFollow", {PERSISTENT | BACKUP, BOOL, "0"}},
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -171,18 +168,18 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
// toyota specific params
{"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ToyotaStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualRadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualRadarTracksDelay", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
{"VisualWideCam", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualStyle", {PERSISTENT | BACKUP, INT, "0"}},
{"VisualStyleZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualStyleOverhead", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualStyleOverheadZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VisualStyleOverheadThreshold", {PERSISTENT | BACKUP, INT, "20"}},
// MADS params
{"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -239,9 +236,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
{"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
// mapd v020
{"MapdSettings", {PERSISTENT | BACKUP, JSON}},
// mapd
{"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},
{"MapdVersion", {PERSISTENT, STRING}},

View File

@@ -10,7 +10,7 @@ from cereal import car, log, custom
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog, ForwardingHandler
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.car import DT_CTRL, structs
from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from opendbc.car.carlog import carlog
@@ -123,13 +123,7 @@ class Car:
self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
self.RI = RI
# set alternative experiences from parameters
sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
self.CP.alternativeExperience = 0
if sp_toyota_auto_brake_hold:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
# mads
set_alternative_experience(self.CP, self.CP_SP, self.params)
set_car_specific_params(self.CP, self.CP_SP, self.params)

View File

@@ -58,7 +58,7 @@ class DesireHelper:
def get_lane_change_direction(CS):
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected, right_edge_detected):
def update(self, carstate, lateral_active, lane_change_prob):
self.alc.update_params()
self.lane_turn_controller.update_params()
v_ego = carstate.vEgo
@@ -90,8 +90,8 @@ class DesireHelper:
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)

View File

@@ -10,8 +10,6 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import index_function
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
@@ -230,8 +228,6 @@ class LongitudinalMpc:
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
self.source = SOURCES[2]
self.accel_controller = AccelPersonalityController()
self.dynamic_follow = FollowDistanceController()
def reset(self):
# self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -332,27 +328,10 @@ class LongitudinalMpc:
return lead_xv
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
if self.dynamic_follow.is_enabled():
t_follow = self.dynamic_follow.get_follow_distance_multiplier(v_ego)
#print(f"DEBUG: dynamic_follow enabled, t_follow={t_follow:.3f}, v_ego={v_ego:.2f}, v_cruise={v_cruise:.2f}")
else:
t_follow = get_T_FOLLOW(personality)
#print(f"DEBUG: dynamic_follow disabled, using personality t_follow={t_follow:.3f}, personality={personality}")
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
# Get acceleration limits
if self.accel_controller.is_enabled():
min_accel = self.accel_controller.get_min_accel(v_ego)
#print(f"DEBUG: accel_enabled=True, min_accel={min_accel:.3f}")
else:
min_accel = CRUISE_MIN_ACCEL
#print(f"DEBUG: accel_enabled=False, using stock min_accel={min_accel}")
a_cruise_min = min_accel
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -371,7 +350,7 @@ class LongitudinalMpc:
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
# TODO does this make sense when max_a is negative?
v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

View File

@@ -124,13 +124,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if mode == 'acc':
if self.accel_controller.is_enabled():
max_accel = self.accel_controller.get_max_accel(v_ego)
#print(f"Vibe personality active - max accel: {max_accel:.3f}")
accel_clip = [ACCEL_MIN, max_accel]
else:
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
else:
@@ -155,10 +149,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
if sm.valid['mapdOut']:
if sm['mapdOut'].suggestedSpeed > 0 and v_cruise > sm['mapdOut'].suggestedSpeed:
v_cruise = sm['mapdOut'].suggestedSpeed
if force_slow_decel:
v_cruise = 0.0

View File

@@ -27,12 +27,12 @@ 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', 'mapdOut', gps_location_service],
'liveMapDataSP', 'carStateSP', gps_location_service],
poll='carState')
while True:
sm.update()
#longitudinal_planner.sla.update_car_state(sm['carState'])
longitudinal_planner.sla.update_car_state(sm['carState'])
if sm.updated['modelV2']:
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)

Binary file not shown.

View File

@@ -51,8 +51,8 @@ def tg_compile(flags, model_name):
for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
flags = {
'larch64': 'DEV=QCOM',
'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0', # tinygrad calls brew which needs a $HOME in the env
}.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0')
'Darwin': 'DEV=CPU IMAGE=0',
}.get(arch, 'DEV=LLVM IMAGE=0')
tg_compile(flags, model_name)
# Compile BIG model if USB GPU is available

View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3
import os
from openpilot.system.hardware import TICI
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
from tinygrad.tensor import Tensor
from tinygrad.dtype import dtypes
import math

View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3
import os
from openpilot.system.hardware import TICI
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
USBGPU = "USBGPU" in os.environ
if USBGPU:
os.environ['DEV'] = 'AMD'
@@ -33,7 +33,7 @@ from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "selfdrive.modeld.modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -298,7 +298,6 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -396,10 +395,7 @@ def main(demo=False):
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
lane_change_prob = l_lane_change_prob + r_lane_change_prob
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

View File

@@ -230,8 +230,8 @@ class SelfdriveD(CruiseHelper):
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
# Create events for temperature, disk space, and memory
@@ -292,15 +292,9 @@ class SelfdriveD(CruiseHelper):
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
mdv2sp = self.sm['modelDataV2SP']
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
elif mdv2sp.leftLaneChangeEdgeBlock or mdv2sp.rightLaneChangeEdgeBlock:
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)

View File

@@ -33,42 +33,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"../assets/icons/experimental_white.svg",
false,
},
{
"ToyotaDriveMode",
tr("Enable drive mode btn link"),
tr("Links cars drive mode btn with accel personalities based on personality (i.e., relaxed, standard, sport)"),
"../assets/offroad/icon_blank.png",
false,
},
{
"ToyotaAutoHold",
tr("Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS"),
tr("As you may auto brake hold currently supported by openpilot, this feature will allow sunnypilot to automatically hold the vehicle at a stop when the lead car is stopped. (TSS2 Hybird only)"),
"../assets/offroad/icon_blank.png",
false,
},
{
"ToyotaEnhancedBsm",
tr("Toyota: Prius TSS2 BSM and some tssp"),
tr("Add support for BSM."),
"../assets/offroad/icon_blank.png",
false,
},
{
"ToyotaTSS2Long",
tr("Toyota: custom tune"),
tr("idk something gas and brake"),
"../assets/offroad/icon_blank.png",
false,
},
{
"ToyotaStockLongitudinal",
tr("Toyota: Stock Toyota Longitudinal"),
tr("This feature will allow sunnypilot to use the stock Toyota longitudinal control instead of the sunnypilot longitudinal control. "
""),
"../assets/offroad/icon_blank.png",
false,
},
{
"DisengageOnAccelerator",
tr("Disengage on Accelerator Pedal"),
@@ -121,15 +85,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"your steering wheel distance button."),
"../assets/icons/speed_limit.png",
longi_button_texts);
// accel controller
std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco")};
accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"),
tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. "
"In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these "
"acceleration personality within Onroad Settings on the driving screen."),
"",
accel_personality_texts);
accel_personality_setting->showDescription();
// set up uiState update for personality setting
QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
@@ -157,7 +113,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
// insert longitudinal personality after NDOG toggle
if (param == "DisengageOnAccelerator") {
addItem(long_personality_setting);
addItem(accel_personality_setting);
}
}
@@ -178,13 +133,6 @@ void TogglesPanel::updateState(const UIState &s) {
}
uiState()->scene.personality = personality;
}
if (sm.updated("longitudinalPlanSP")) {
auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality();
if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) {
accel_personality_setting->setCheckedButton(static_cast<int>(accel_personality));
}
uiState()->scene.accel_personality = accel_personality;
}
}
void TogglesPanel::expandToggleDescription(const QString &param) {
@@ -231,12 +179,10 @@ void TogglesPanel::updateToggles() {
experimental_mode_toggle->setEnabled(true);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(true);
accel_personality_setting->setEnabled(true);
} else {
// no long for now
experimental_mode_toggle->setEnabled(false);
long_personality_setting->setEnabled(false);
accel_personality_setting->setEnabled(true);
params.remove("ExperimentalMode");
const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.");

View File

@@ -88,7 +88,6 @@ protected:
Params params;
std::map<std::string, ParamControl*> toggles;
ButtonParamControl *long_personality_setting;
ButtonParamControl *accel_personality_setting;
virtual void updateToggles();
};

View File

@@ -25,6 +25,11 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
// update engageability/experimental mode button
experimental_btn->updateState(s);
dmon.updateState(s);
if (s.scene.visual_style == 0) {
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
} else {
setBackgroundColor(QColor(0, 0, 0));
}
}
void AnnotatedCameraWidget::initializeGL() {
@@ -35,7 +40,12 @@ void AnnotatedCameraWidget::initializeGL() {
qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));
prev_draw_t = millis_since_boot();
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
auto *s = uiState();
if (s->scene.visual_style == 0) {
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
} else {
setBackgroundColor(QColor(0, 0, 0));
}
}
mat4 AnnotatedCameraWidget::calcFrameMatrix() {
@@ -118,7 +128,13 @@ void AnnotatedCameraWidget::paintGL() {
} else if (v_ego > 15) {
wide_cam_requested = false;
}
wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
if (s->scene.visual_wide_cam == 1) {
wide_cam_requested = true;
} else if (s->scene.visual_wide_cam == 2) {
wide_cam_requested = false;
} else {
wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
}
}
CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId());

View File

@@ -1,4 +1,5 @@
#include "selfdrive/ui/qt/onroad/model.h"
#include <algorithm>
void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
auto *s = uiState();
@@ -22,7 +23,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
update_model(model, lead_one);
drawLaneLines(painter);
drawPath(painter, model, surface_rect.height(), surface_rect.width());
drawPath(painter, model, surface_rect.height());
if (longitudinal_control && sm.alive("radarState")) {
update_leads(radar_state, model.getPosition());
@@ -49,8 +50,14 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state,
}
void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
auto *s = uiState();
const auto &model_position = model.getPosition();
float max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
float max_distance;
if (s->scene.visual_style == 0) {
max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
} else {
max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
}
// update lane lines
const auto &lane_lines = model.getLaneLines();
@@ -58,7 +65,11 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
int max_idx = get_path_length_idx(lane_lines[0], max_distance);
for (int i = 0; i < std::size(lane_line_vertices); i++) {
lane_line_probs[i] = line_probs[i];
mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
if (s->scene.visual_style == 2) {
mapLineToPolygon(lane_lines[i], 0.075 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
} else {
mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
}
}
// update road edges
@@ -66,7 +77,11 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
const auto &edge_stds = model.getRoadEdgeStds();
for (int i = 0; i < std::size(road_edge_vertices); i++) {
road_edge_stds[i] = edge_stds[i];
mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
if (s->scene.visual_style == 2) {
mapLineToPolygon(road_edges[i], 0.1, 0, &road_edge_vertices[i], max_idx);
} else {
mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
}
}
// update path
@@ -79,20 +94,116 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
}
void ModelRenderer::drawLaneLines(QPainter &painter) {
// lanelines
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
painter.drawPolygon(lane_line_vertices[i]);
}
auto *s = uiState();
if (s->scene.visual_style == 2) {
QRectF r = clip_region;
// road edges
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
painter.drawPolygon(road_edge_vertices[i]);
qreal horizonY = r.bottom();
if (!road_edge_vertices[0].isEmpty() || !road_edge_vertices[1].isEmpty()) {
qreal leftH = r.top();
qreal rightH = r.top();
if (!road_edge_vertices[0].isEmpty()) {
leftH = std::numeric_limits<qreal>::max();
for (const QPointF &pt : road_edge_vertices[0]) {
if (pt.y() < leftH) leftH = pt.y();
}
}
if (!road_edge_vertices[1].isEmpty()) {
rightH = std::numeric_limits<qreal>::max();
for (const QPointF &pt : road_edge_vertices[1]) {
if (pt.y() < rightH) rightH = pt.y();
}
}
horizonY = std::max(leftH, rightH);
}
painter.fillRect(QRectF(r.left(), horizonY + 0, r.width(), r.bottom() - (horizonY + 0)), QColor("#111111"));
auto buildFill = [&](const QPolygonF &edgeRibbon, bool isLeftSide) -> QPolygonF {
if (edgeRibbon.isEmpty()) return {};
QMap<int, QPointF> byY;
for (const QPointF &pt : edgeRibbon) {
int yi = int(std::round(pt.y()));
if (!byY.contains(yi)) {
byY[yi] = pt;
} else {
if (isLeftSide) {
if (pt.x() > byY[yi].x()) byY[yi] = pt;
} else {
if (pt.x() < byY[yi].x()) byY[yi] = pt;
}
}
}
if (byY.isEmpty()) return {};
QPolygonF curve;
for (auto it = byY.cbegin(); it != byY.cend(); ++it) {
curve << it.value();
}
if (curve.size() < 2) return {};
const qreal topY = curve.first().y();
QPolygonF fill;
if (isLeftSide) {
fill << QPointF(r.left(), topY);
for (const QPointF &pt : curve) fill << pt;
fill << QPointF(r.left(), r.bottom());
} else {
fill << QPointF(r.right(), topY);
for (const QPointF &pt : curve) fill << pt;
fill << QPointF(r.right(), r.bottom());
}
return fill;
};
QPolygonF leftFill = buildFill(road_edge_vertices[0], true);
QPolygonF rightFill = buildFill(road_edge_vertices[1], false);
if (!leftFill.isEmpty()) {
painter.setBrush(QColor("#222222"));
painter.drawPolygon(leftFill);
}
if (!rightFill.isEmpty()) {
painter.setBrush(QColor("#222222"));
painter.drawPolygon(rightFill);
}
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
painter.setBrush(QColor::fromRgbF(0.902, 0.902, 0.902, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
painter.drawPolygon(lane_line_vertices[i]);
}
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
painter.setBrush(QColor(0x55, 0x55, 0x55, 255));
painter.drawPolygon(road_edge_vertices[i]);
}
QLinearGradient bgGrad(r.left(), horizonY - 100, r.left(), horizonY + 100);
bgGrad.setColorAt(0.0, QColor("#000000"));
bgGrad.setColorAt(0.5, QColor("#111111"));
bgGrad.setColorAt(1.0, QColor("#111111"));
painter.fillRect(QRectF(r.left(), horizonY - 200, r.width(), 200), bgGrad);
} else {
// lanelines
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
painter.drawPolygon(lane_line_vertices[i]);
}
// road edges
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
painter.drawPolygon(road_edge_vertices[i]);
}
}
}
void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height, int width) {
void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) {
QLinearGradient bg(0, height, 0, 0);
if (experimental_mode) {
// The first half of track_vertices are the points for the right side of the path
@@ -127,9 +238,6 @@ void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reade
painter.setBrush(bg);
painter.drawPolygon(track_vertices);
//LongFuel(painter,height, width);
//LateralFuel(painter, height, width);
}
void ModelRenderer::updatePathGradient(QLinearGradient &bg) {
@@ -176,197 +284,9 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
(1 - t) * start.alphaF() + t * end.alphaF());
}
void ModelRenderer::drawGaugeBackground(QPainter &painter, qreal centerX, qreal centerY) {
const qreal backgroundSize = GAUGE_SIZE * BACKGROUND_SIZE_MULTIPLIER;
// Draw circular background
painter.setPen(Qt::NoPen);
painter.setBrush(BACKGROUND_COLOR);
painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2, backgroundSize / 2);
// Draw border
QPen borderPen(BORDER_COLOR);
borderPen.setWidth(BORDER_PEN_WIDTH);
painter.setPen(borderPen);
painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2 + 1, backgroundSize / 2 + 1);
// Draw background semicircle
QPen semicirclePen(GAUGE_BACKGROUND_COLOR);
semicirclePen.setWidth(GAUGE_PEN_WIDTH);
semicirclePen.setCapStyle(Qt::RoundCap);
painter.setPen(semicirclePen);
painter.drawArc(QRectF(centerX - GAUGE_SIZE / 2, centerY - GAUGE_SIZE / 2,
GAUGE_SIZE, GAUGE_SIZE), 0, SEMICIRCLE_SPAN);
}
QColor ModelRenderer::getIndicatorColor(float absoluteValue, float lowThreshold, float highThreshold) {
if (absoluteValue < lowThreshold) {
return LOW_INDICATOR_COLOR;
} else if (absoluteValue < highThreshold) {
return MODERATE_INDICATOR_COLOR;
} else {
return HIGH_INDICATOR_COLOR;
}
}
int ModelRenderer::calculateSpanAngle(float absoluteValue, float maxValue) {
const int spanAngle = static_cast<int>(QUARTER_CIRCLE_SPAN * (absoluteValue / maxValue));
return std::clamp(spanAngle, 0, QUARTER_CIRCLE_SPAN);
}
void ModelRenderer::drawGaugeArc(QPainter &painter, qreal centerX, qreal centerY,
float value, bool isPositive, const QString &label) {
const float absoluteValue = std::abs(value);
if (absoluteValue <= MIN_THRESHOLD) {
return; // Skip drawing if value is too small
}
// Set up the arc rectangle
const QRectF arcRect(centerX - GAUGE_SIZE / 2, centerY - GAUGE_SIZE / 2,
GAUGE_SIZE, GAUGE_SIZE);
// Configure pen for the indicator arc
QPen indicatorPen;
indicatorPen.setWidth(GAUGE_PEN_WIDTH);
indicatorPen.setCapStyle(Qt::RoundCap);
painter.setPen(indicatorPen);
// Draw the arc based on direction
const int spanAngle = calculateSpanAngle(absoluteValue, 1.0f); // Adjust max value as needed
if (isPositive) {
painter.drawArc(arcRect, STARTING_ANGLE, spanAngle);
} else {
painter.drawArc(arcRect, STARTING_ANGLE, -spanAngle);
}
// Draw center label
painter.setPen(Qt::white);
QFont font = painter.font();
font.setPixelSize(20);
font.setBold(true);
painter.setFont(font);
painter.drawText(QRectF(centerX - 50, centerY + 10, 100, 20), Qt::AlignCenter, label);
}
void ModelRenderer::LongFuel(QPainter &painter, int height, int width) {
const qreal rectWidth = static_cast<qreal>(width);
const qreal rectHeight = static_cast<qreal>(height);
UIState *s = uiState();
if (!s || !s->sm) {
return; // Safety check
}
// Get current acceleration
const float currentAcceleration = (*s->sm)["carControl"].getCarControl().getActuators().getAccel();
const float absoluteAcceleration = std::abs(currentAcceleration);
// Calculate gauge position
const qreal centerX = rectWidth / 17;
const qreal centerY = rectHeight / 2 + 120;
// Draw gauge background
drawGaugeBackground(painter, centerX, centerY);
// Skip drawing arc if acceleration is too small
if (absoluteAcceleration <= MIN_THRESHOLD) {
drawGaugeArc(painter, centerX, centerY, 0.0f, true, "LONG");
return;
}
// Determine indicator color based on acceleration magnitude
const QColor indicatorColor = getIndicatorColor(absoluteAcceleration, 0.3f, 0.6f);
// Calculate span angle (scale for better visibility)
const int spanAngle = static_cast<int>(QUARTER_CIRCLE_SPAN * absoluteAcceleration);
const int clampedSpanAngle = std::clamp(spanAngle, 0, QUARTER_CIRCLE_SPAN);
// Draw the acceleration arc
QPen indicatorPen(indicatorColor);
indicatorPen.setWidth(GAUGE_PEN_WIDTH);
indicatorPen.setCapStyle(Qt::RoundCap);
painter.setPen(indicatorPen);
const QRectF arcRect(centerX - GAUGE_SIZE / 2, centerY - GAUGE_SIZE / 2,
GAUGE_SIZE, GAUGE_SIZE);
// Draw arc based on acceleration direction
if (currentAcceleration > 0) {
painter.drawArc(arcRect, STARTING_ANGLE, -clampedSpanAngle); // Left side for positive
} else {
painter.drawArc(arcRect, STARTING_ANGLE, clampedSpanAngle); // Right side for negative
}
// Draw center label
painter.setPen(Qt::white);
QFont font = painter.font();
font.setPixelSize(20);
font.setBold(true);
painter.setFont(font);
painter.drawText(QRectF(centerX - 50, centerY + 10, 100, 20), Qt::AlignCenter, "LONG");
}
void ModelRenderer::LateralFuel(QPainter &painter, int height, int width) {
const qreal rectWidth = static_cast<qreal>(width);
const qreal rectHeight = static_cast<qreal>(height);
UIState *s = uiState();
if (!s || !s->sm) {
return; // Safety check
}
// Get current steering angle
const float currentLateral = (*s->sm)["carState"].getCarState().getSteeringAngleDeg();
const float absoluteLateral = std::abs(currentLateral);
// Calculate gauge position
const qreal centerX = rectWidth / 17;
const qreal centerY = rectHeight / 2 - 120;
// Draw gauge background
drawGaugeBackground(painter, centerX, centerY);
// Skip drawing arc if lateral force is too small
if (absoluteLateral <= 0.1f) {
drawGaugeArc(painter, centerX, centerY, 0.0f, true, "LAT");
return;
}
// Determine indicator color based on lateral force magnitude
const QColor indicatorColor = getIndicatorColor(absoluteLateral, 5.0f, 15.0f);
// Calculate span angle (normalized to max expected steering angle)
const float maxSteeringAngle = 15.0f; // Adjust based on your vehicle's characteristics
const int spanAngle = static_cast<int>(QUARTER_CIRCLE_SPAN * (absoluteLateral / maxSteeringAngle));
const int clampedSpanAngle = std::clamp(spanAngle, 0, QUARTER_CIRCLE_SPAN);
// Draw the lateral arc
QPen indicatorPen(indicatorColor);
indicatorPen.setWidth(GAUGE_PEN_WIDTH);
indicatorPen.setCapStyle(Qt::RoundCap);
painter.setPen(indicatorPen);
const QRectF arcRect(centerX - GAUGE_SIZE / 2, centerY - GAUGE_SIZE / 2,
GAUGE_SIZE, GAUGE_SIZE);
// Draw arc based on steering direction
if (currentLateral < 0) {
painter.drawArc(arcRect, STARTING_ANGLE, -clampedSpanAngle); // Left turn
} else {
painter.drawArc(arcRect, STARTING_ANGLE, clampedSpanAngle); // Right turn
}
// Draw center label
painter.setPen(Qt::white);
QFont font = painter.font();
font.setPixelSize(20);
font.setBold(true);
painter.setFont(font);
painter.drawText(QRectF(centerX - 50, centerY + 10, 100, 20), Qt::AlignCenter, "LAT");
}
void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
const QPointF &vd, const QRect &surface_rect) {
auto *s = uiState();
const float speedBuff = 10.;
const float leadBuff = 40.;
const float d_rel = lead_data.getDRel();
@@ -389,20 +309,133 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa
float g_yo = sz / 10;
QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
painter.setBrush(QColor(218, 202, 37, 255));
if (s->scene.visual_style == 2) {
painter.setBrush(QColor(0xE6, 0xE6, 0xE6, 255));
} else {
painter.setBrush(QColor(218, 202, 37, 255));
}
painter.drawPolygon(glow, std::size(glow));
// chevron
QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
painter.setBrush(QColor(201, 34, 49, fillAlpha));
if (s->scene.visual_style == 2) {
painter.setBrush(QColor(0, 0, 0, fillAlpha));
} else {
painter.setBrush(QColor(201, 34, 49, fillAlpha));
}
painter.drawPolygon(chevron, std::size(chevron));
}
// Projects a point in car to space to the corresponding point in full frame image space.
float mapRange(float x, float in_min, float in_max, float out_min, float out_max) {
if (in_min < in_max) {
x = std::clamp(x, in_min, in_max);
} else {
x = std::clamp(x, in_max, in_min);
}
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
}
// Projects a point in car space to the corresponding point in full frame image space.
bool ModelRenderer::mapToScreen(float in_x, float in_y, float in_z, QPointF *out) {
auto *s = uiState();
auto &sm = *(s->sm);
float blend_speed_mph = fabsf(sm["carState"].getCarState().getVEgo() * 2.23694f);
Eigen::Vector3f input(in_x, in_y, in_z);
if ((s->scene.visual_style_zoom == 1 || s->scene.visual_style_zoom == 2) && s->scene.visual_style != 0) {
float zoom_start = 20.0f;
float zoom_end = 50.0f;
if (s->scene.visual_style_zoom == 2) {
std::swap(zoom_start, zoom_end);
}
float IN_X_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 24.0f);
float IN_Y_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 1.0f, 2.0f);
float IN_Z_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
float PITCH_DEG = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
input = Eigen::Vector3f(in_x + IN_X_OFFSET, in_y / IN_Y_OFFSET, in_z + IN_Z_OFFSET);
Eigen::AngleAxisf pitch_rot(PITCH_DEG * M_PI / 180.0f, Eigen::Vector3f::UnitY());
input = pitch_rot * input;
}
auto pt = car_space_transform * input;
*out = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
bool normal_valid = (pt.z() > 1e-3f &&
std::isfinite(pt.x()) && std::isfinite(pt.y()));
QPointF normal_view;
if (normal_valid) {
normal_view = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
}
const float base_scale_x = 20.0f;
const float base_scale_y = 15.0f;
const float y_offset = 450.0f;
float factor_scale_x = 0.0f;
if (blend_speed_mph > 0.0f) {
if (s->scene.visual_style_overhead_zoom == 1) {
factor_scale_x = mapRange(blend_speed_mph, 0.0f, 50.0f, 30.0f, 0.0f);
} else if (s->scene.visual_style_overhead_zoom == 2) {
factor_scale_x = mapRange(blend_speed_mph, 50.0f, 0.0f, 30.0f, 0.0f);
}
}
float scale_x = base_scale_x + factor_scale_x;
float scale_y = base_scale_y;
QPointF topdown_view(
clip_region.center().x() + in_y * scale_x,
(clip_region.bottom() - y_offset) - in_x * scale_y
);
if ((s->scene.visual_style_overhead == 1 || s->scene.visual_style_overhead == 2) && s->scene.visual_style != 0) {
static float blend = 0.0f;
static float target_blend = 0.0f;
static double last_t = millis_since_boot();
const bool inverted = (s->scene.visual_style_overhead == 2);
const float threshold = s->scene.visual_style_overhead_threshold;
const float hysteresis = 5.0f;
if (!inverted) {
if (target_blend < 0.5f && blend_speed_mph > threshold) {
target_blend = 1.0f;
} else if (target_blend > 0.5f && blend_speed_mph < threshold - hysteresis) {
target_blend = 0.0f;
}
} else {
if (target_blend < 0.5f && blend_speed_mph < threshold) {
target_blend = 1.0f;
} else if (target_blend > 0.5f && blend_speed_mph > threshold + hysteresis) {
target_blend = 0.0f;
}
}
double now = millis_since_boot();
double dt = (now - last_t) / 1000.0;
last_t = now;
const float transition_time = 1.50f;
float step = dt / transition_time;
if (blend < target_blend) {
blend = std::min(blend + step, target_blend);
} else if (blend > target_blend) {
blend = std::max(blend - step, target_blend);
}
if (!normal_valid) return false;
*out = QPointF(
(1 - blend) * normal_view.x() + blend * topdown_view.x(),
(1 - blend) * normal_view.y() + blend * topdown_view.y()
);
} else {
if (!normal_valid) return false;
*out = normal_view;
}
return clip_region.contains(*out);
}

View File

@@ -29,8 +29,6 @@ public:
ModelRenderer() {}
void setTransform(const Eigen::Matrix3f &transform) { car_space_transform = transform; }
void draw(QPainter &painter, const QRect &surface_rect);
void LongFuel(QPainter &p, int height, int width);
void LateralFuel(QPainter &p, int height, int width);
protected:
bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out);
@@ -40,16 +38,7 @@ protected:
void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
void drawLaneLines(QPainter &painter);
void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height, int width);
// Gauge helper methods
void drawGaugeBackground(QPainter &painter, qreal centerX, qreal centerY);
void drawGaugeArc(QPainter &painter, qreal centerX, qreal centerY,
float value, bool isPositive, const QString &label);
QColor getIndicatorColor(float absoluteValue, float lowThreshold, float highThreshold);
int calculateSpanAngle(float absoluteValue, float maxValue);
void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height);
void updatePathGradient(QLinearGradient &bg);
QColor blendColors(const QColor &start, const QColor &end, float t);
@@ -66,22 +55,4 @@ protected:
QPointF lead_vertices[2] = {};
Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero();
QRectF clip_region;
// Gauge configuration constants
static constexpr qreal GAUGE_SIZE = 140.0;
static constexpr qreal BACKGROUND_SIZE_MULTIPLIER = 1.4;
static constexpr qreal GAUGE_PEN_WIDTH = 30.0;
static constexpr qreal BORDER_PEN_WIDTH = 2.0;
static constexpr int SEMICIRCLE_SPAN = 180 * 16;
static constexpr int QUARTER_CIRCLE_SPAN = 90 * 16;
static constexpr int STARTING_ANGLE = 90 * 16;
static constexpr qreal MIN_THRESHOLD = 0.01;
// Color constants - Note: QColor cannot be constexpr, use inline static const instead
inline static const QColor BACKGROUND_COLOR = QColor(0, 0, 0, 80);
inline static const QColor BORDER_COLOR = QColor(0, 0, 0, 100);
inline static const QColor GAUGE_BACKGROUND_COLOR = QColor(50, 50, 50);
inline static const QColor LOW_INDICATOR_COLOR = QColor(23, 241, 66, 200);
inline static const QColor MODERATE_INDICATOR_COLOR = QColor(255, 166, 0, 200);
inline static const QColor HIGH_INDICATOR_COLOR = QColor(245, 0, 0, 200);
};

View File

@@ -196,6 +196,7 @@ mat4 CameraWidget::calcFrameMatrix() {
}
void CameraWidget::paintGL() {
auto *s = uiState();
glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
@@ -248,7 +249,9 @@ void CameraWidget::paintGL() {
glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
glEnableVertexAttribArray(0);
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
if (s->scene.visual_style == 0) {
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
}
glDisableVertexAttribArray(0);
glBindVertexArray(0);
glBindTexture(GL_TEXTURE_2D, 0);

View File

@@ -36,7 +36,7 @@ DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) {
interactivityTimeout = new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"),
tr("Apply a custom timeout for settings UI."
"\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."),
"", {0, 120}, 10, true, nullptr, false);
"", {999999, 1000000}, 1000000, true, nullptr, false);
connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() {
refresh();

View File

@@ -33,12 +33,6 @@ LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) {
tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
"../assets/offroad/icon_blank.png",
},
{
"RoadEdgeLaneChangeEnabled",
tr("Block Lane Change: Road Edge Detection"),
tr("Enable this toggle to block lane change when road edge is detected on the stalk actuated side."),
"../assets/offroad/icon_blank.png",
}
};
// Controls: Auto Lane Change Timer

View File

@@ -75,22 +75,6 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh);
// Acceleration Personality
AccelPersonalityControl = new ParamControlSP("AccelPersonalityEnabled",
tr("Acceleration Personality"),
tr("Controls acceleration behavior: Eco (efficient), Normal (balanced), Sport (responsive). "
"Adjust how aggressively the vehicle accelerates while maintaining smooth operation."),
"../assets/offroad/icon_shell.png");
list->addItem(AccelPersonalityControl);
// Dynamic Personality
DynamicPersonalityControl = new ParamControlSP("DynamicFollow",
tr("Following Distance Personality"),
tr("Controls following distance and braking behavior: Relaxed (longer distance, gentler braking), Standard (balanced), Aggressive (shorter distance, firmer braking). "
"Fine-tune your comfort level in traffic situations."),
"../assets/offroad/icon_shell.png");
list->addItem(DynamicPersonalityControl);
speedLimitSettings = new PushButtonSP(tr("Speed Limit"), 750, this);
connect(speedLimitSettings, &QPushButton::clicked, [&]() {
cruisePanelScroller->setLastScrollPosition();
@@ -180,10 +164,6 @@ void LongitudinalPanel::refresh(bool _offroad) {
dynamicExperimentalControl->refresh();
SmartCruiseControlVision->refresh();
SmartCruiseControlMap->refresh();
AccelPersonalityControl->setEnabled(true);
DynamicPersonalityControl->setEnabled(true);
AccelPersonalityControl->refresh();
DynamicPersonalityControl->refresh();
} else {
has_longitudinal_control = false;
is_pcm_cruise = false;

View File

@@ -36,9 +36,6 @@ private:
ParamControl *SmartCruiseControlMap;
ParamControl *intelligentCruiseButtonManagement = nullptr;
ParamControl *dynamicExperimentalControl = nullptr;
ParamControlSP *AccelPersonalityControl;
ParamControlSP *DynamicPersonalityControl;
SpeedLimitSettings *speedLimitScreen;
PushButtonSP *speedLimitSettings;
};

View File

@@ -11,6 +11,18 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
param_watcher = new ParamWatcher(this);
connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString &param_name, const QString &param_value) {
paramsRefresh();
if (param_name == "VisualStyle") {
visual_style_value = param_value.toInt();
} else if (param_name == "VisualStyleOverhead") {
visual_style_overhead_value = param_value.toInt();
} else if (param_name == "VisualRadarTracks") {
bool radar_tracks_enabled = param_value.toInt() != 0;
visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
}
visual_style_zoom_settings->setVisible(visual_style_value != 0);
visual_style_overhead_settings->setVisible(visual_style_value != 0);
visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
});
main_layout = new QStackedLayout(this);
@@ -90,6 +102,13 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
"",
false,
},
{
"VisualRadarTracks",
tr("Show Radar Tracks"),
tr("Shows what the cars radar sees."),
"",
false,
},
};
// Add regular toggles first
@@ -116,6 +135,111 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
param_watcher->addParam(param);
}
// Visuals: Radar Tracks Delay
visual_radar_tracks_delay_settings = new OptionControlSP("VisualRadarTracksDelay", tr("Adjust Visual Radar Tracks Delay"),
tr("Delays radar tracks to better match what you see through the camera."),
"", {0, 100}, 10, false, nullptr, true);
connect(visual_radar_tracks_delay_settings, &OptionControlSP::updateLabels, [=]() {
float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
});
float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
list->addItem(visual_radar_tracks_delay_settings);
// Wide Cam
std::vector<QString> visual_wide_cam_settings_texts{tr("Auto"), tr("On"), tr("Off")};
visual_wide_cam_settings = new ButtonParamControlSP(
"VisualWideCam", tr("Wide Cam"), tr("Override the wide cam view regardless of experimental mode status."),
"",
visual_wide_cam_settings_texts,
250);
list->addItem(visual_wide_cam_settings);
// Visual Style
std::vector<QString> visual_style_settings_texts{tr("Default"), tr("Minimal"), tr("Vision")};
visual_style_settings = new ButtonParamControlSP(
"VisualStyle", tr("Visual Style"),
tr(
"Switch between different on-road visualization layouts."
"<ul style='margin-left: 10px; margin-top: 4px;'>"
"<li><b>Default:</b> Standard OpenPilot layout with camera and path view.</li>"
"<li><b>Minimal:</b> Clean interface without camera feed or extra elements.</li>"
"<li><b>Vision:</b> Experimental layout that focuses on model perception and environment.</li>"
"</ul>"
),
"",
visual_style_settings_texts,
380);
list->addItem(visual_style_settings);
// Visual Style Zoom
std::vector<QString> visual_style_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
visual_style_zoom_settings = new ButtonParamControlSP(
"VisualStyleZoom", tr("Visual Style Zoom"),
tr(
"Enables dynamic zooming based on driving speed in the selected visual style."
"<ul style='margin-left: 10px; margin-top: 4px;'>"
"<li><b>Disabled:</b> Keeps the zoom fixed.</li>"
"<li><b>Enabled:</b> Zooms in at low speed and out at high speed.</li>"
"<li><b>Inverted:</b> Reverses the zoom behavior.</li>"
"</ul>"
),
"",
visual_style_zoom_settings_texts,
380);
list->addItem(visual_style_zoom_settings);
// Visual Style Overhead
std::vector<QString> visual_style_overhead_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
visual_style_overhead_settings = new ButtonParamControlSP(
"VisualStyleOverhead", tr("Visual Style Overhead"),
tr(
"Toggles an overhead (top-down) camera view for a 2D-style perspective."
"<ul style='margin-left: 10px; margin-top: 4px;'>"
"<li><b>Disabled:</b> Keeps the standard forward 3D view.</li>"
"<li><b>Enabled:</b> Switches to overhead view when active.</li>"
"<li><b>Inverted:</b> Reverses when the transition happens.</li>"
"</ul>"
),
"",
visual_style_overhead_settings_texts,
380);
list->addItem(visual_style_overhead_settings);
// Visual Style Overhead Zoom
std::vector<QString> visual_style_overhead_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
visual_style_overhead_zoom_settings = new ButtonParamControlSP(
"VisualStyleOverheadZoom", tr("Visual Style Overhead Zoom"),
tr(
"Controls zooming behavior while in overhead mode."
"<ul style='margin-left: 10px; margin-top: 4px;'>"
"<li><b>Disabled:</b> Keeps a fixed zoom level in overhead mode.</li>"
"<li><b>Enabled:</b> Zooms dynamically based on speed while overhead.</li>"
"<li><b>Inverted:</b> Opposite zoom direction.</li>"
"</ul>"
),
"",
visual_style_overhead_zoom_settings_texts,
380);
list->addItem(visual_style_overhead_zoom_settings);
// Visual Style Overhead Threshold
visual_style_overhead_threshold_settings = new OptionControlSP(
"VisualStyleOverheadThreshold", tr("Visual Style Overhead Threshold"),
tr("Sets the speed (in mph) where the display transitions between normal and overhead view."),
"", {10, 80}, 5, false, nullptr, false);
auto updateThresholdLabel = [=]() {
int mph = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
visual_style_overhead_threshold_settings->setLabel(QString("%1 mph").arg(mph));
};
connect(visual_style_overhead_threshold_settings, &OptionControlSP::updateLabels, updateThresholdLabel);
updateThresholdLabel();
list->addItem(visual_style_overhead_threshold_settings);
// Visuals: Display Metrics below Chevron
std::vector<QString> chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Time"), tr("All")};
chevron_info_settings = new ButtonParamControlSP(
@@ -136,6 +260,19 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
380);
list->addItem(dev_ui_settings);
bool radar_tracks_enabled = QString::fromStdString(params.get("VisualRadarTracks")).toInt() != 0;
visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
param_watcher->addParam("VisualRadarTracks");
visual_style_value = QString::fromStdString(params.get("VisualStyle")).toInt();
visual_style_overhead_value = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
visual_style_zoom_settings->setVisible(visual_style_value != 0);
visual_style_overhead_settings->setVisible(visual_style_value != 0);
visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
param_watcher->addParam("VisualStyle");
param_watcher->addParam("VisualStyleOverhead");
sunnypilotScroller = new ScrollViewSP(list, this);
vlayout->addWidget(sunnypilotScroller);
@@ -191,4 +328,19 @@ void VisualsPanel::paramsRefresh() {
if (dev_ui_settings) {
dev_ui_settings->refresh();
}
if (visual_wide_cam_settings) {
visual_wide_cam_settings->refresh();
}
if (visual_style_settings) {
visual_style_settings->refresh();
}
if (visual_style_zoom_settings) {
visual_style_zoom_settings->refresh();
}
if (visual_style_overhead_settings) {
visual_style_overhead_settings->refresh();
}
if (visual_style_overhead_zoom_settings) {
visual_style_overhead_zoom_settings->refresh();
}
}

View File

@@ -32,4 +32,14 @@ protected:
ButtonParamControlSP *dev_ui_settings;
bool has_longitudinal_control = false;
OptionControlSP *visual_radar_tracks_delay_settings;
ButtonParamControlSP *visual_wide_cam_settings;
int visual_style_value = 0;
int visual_style_overhead_value = 0;
ButtonParamControlSP *visual_style_settings;
ButtonParamControlSP *visual_style_zoom_settings;
ButtonParamControlSP *visual_style_overhead_settings;
ButtonParamControlSP *visual_style_overhead_zoom_settings;
OptionControlSP *visual_style_overhead_threshold_settings;
};

View File

@@ -44,6 +44,8 @@ void HudRendererSP::updateState(const UIState &s) {
const auto car_params = sm["carParams"].getCarParams();
const auto car_params_sp = sm["carParamsSP"].getCarParamsSP();
const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP();
if (sm.updated("carParams")) {
steerControlType = car_params.getSteerControlType();
}
@@ -52,80 +54,37 @@ void HudRendererSP::updateState(const UIState &s) {
pcmCruiseSpeed = car_params_sp.getPcmCruiseSpeed();
}
if (sm.alive("mapdOut") && sm.rcv_frame("mapdOut") > 0) {
const auto mapd = sm["mapdOut"].getMapdOut();
// Road name can come from wayName, wayRef, or roadName
wayName = QString::fromStdString(mapd.getWayName());
wayRef = QString::fromStdString(mapd.getWayRef());
QString mapdRoadName = QString::fromStdString(mapd.getRoadName());
if (!mapdRoadName.isEmpty()) {
roadNameStr = mapdRoadName;
} else if (!wayRef.isEmpty() && !wayName.isEmpty()) {
roadNameStr = wayRef + " - " + wayName;
} else if (!wayName.isEmpty()) {
roadNameStr = wayName;
} else if (!wayRef.isEmpty()) {
roadNameStr = wayRef;
} else {
roadNameStr = "";
}
if (sm.updated("longitudinalPlanSP")) {
speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv;
speedLimitLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLast() * speedConv;
speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv;
speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid();
speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid();
speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv;
speedLimitSource = lp_sp.getSpeedLimit().getResolver().getSource();
speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState();
speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive();
smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled();
smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive();
}
greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
tileLoaded = mapd.getTileLoaded();
float mapdSpeedLimitRaw = mapd.getSpeedLimit();
float mapdOffsetRaw = mapd.getSpeedLimitOffset();
mapdSpeedLimit = mapdSpeedLimitRaw * speedConv;
speedLimit = mapdSpeedLimit;
speedLimitLast = mapdSpeedLimit;
speedLimitOffset = mapdOffsetRaw * speedConv;
speedLimitValid = tileLoaded && mapdSpeedLimitRaw > 0;
speedLimitLastValid = speedLimitValid;
speedLimitFinalLast = mapdSpeedLimit + speedLimitOffset;
if (tileLoaded) {
speedLimitSource = 1; // MAP
} else {
speedLimitSource = 0; // NONE
}
float nextSpeedLimitRaw = mapd.getNextSpeedLimit();
speedLimitAheadValid = nextSpeedLimitRaw > 0 && tileLoaded;
speedLimitAhead = nextSpeedLimitRaw * speedConv;
speedLimitAheadDistance = mapd.getNextSpeedLimitDistance();
if (sm.updated("liveMapDataSP")) {
roadNameStr = QString::fromStdString(lmd.getRoadName());
speedLimitAheadValid = lmd.getSpeedLimitAheadValid();
speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv;
speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance();
if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD) {
speedLimitAheadValidFrame++;
} else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) {
speedLimitAheadValidFrame--;
}
// SCC data from mapd
suggestedSpeed = mapd.getSuggestedSpeed() * speedConv;
visionCurveSpeed = mapd.getVisionCurveSpeed() * speedConv;
curveSpeed = mapd.getCurveSpeed() * speedConv;
smartCruiseControlVisionEnabled = visionCurveSpeed > 0;
smartCruiseControlVisionActive = visionCurveSpeed > 0 && visionCurveSpeed < speedLimit;
smartCruiseControlMapEnabled = curveSpeed > 0;
smartCruiseControlMapActive = curveSpeed > 0 && curveSpeed < speedLimit;
advisorySpeed = mapd.getAdvisorySpeed() * speedConv;
nextAdvisorySpeed = mapd.getNextAdvisorySpeed() * speedConv;
nextAdvisorySpeedDistance = mapd.getNextAdvisorySpeedDistance();
}
speedLimitAheadDistancePrev = speedLimitAheadDistance;
speedLimitAssistState = 0;
speedLimitAssistActive = false;
static int reverse_delay = 0;
bool reverse_allowed = false;
if (car_state.getGearShifter() != cereal::CarState::GearShifter::REVERSE) {
@@ -149,7 +108,7 @@ void HudRendererSP::updateState(const UIState &s) {
}
if (sm.updated(gps_source)) {
gpsAccuracy = is_gps_location_external ? gpsLocation.getHorizontalAccuracy() : 1.0;
gpsAccuracy = is_gps_location_external ? gpsLocation.getHorizontalAccuracy() : 1.0; // External reports accuracy, internal does not.
altitude = gpsLocation.getAltitude();
bearingAccuracyDeg = gpsLocation.getBearingAccuracyDeg();
bearingDeg = gpsLocation.getBearingDeg();
@@ -323,7 +282,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
const int sign_height = 204;
QRect sign_rect(sign_x, sign_y, sign_width, sign_height);
if (speedLimitAssistState == 1) {
if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
speedLimitAssistFrame++;
showSpeedLimit = speed_limit_assist_pre_active_pulse;
drawSpeedLimitPreActiveArrow(p, sign_rect);
@@ -336,7 +295,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
drawSpeedLimitSigns(p, sign_rect);
// do not show during SLA's preActive state
if (speedLimitAssistState != 1) {
if (speedLimitAssistState != cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
drawUpcomingSpeedLimit(p);
}
}
@@ -676,7 +635,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p, QRect &sign_rect) {
void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0 &&
tileLoaded;
speedLimitSource == cereal::LongitudinalPlanSP::SpeedLimit::Source::MAP;
if (!speed_limit_ahead) {
return;
}

View File

@@ -85,7 +85,7 @@ private:
bool speedLimitValid;
bool speedLimitLastValid;
float speedLimitFinalLast;
int speedLimitSource; // 0=NONE, 1=MAP
cereal::LongitudinalPlanSP::SpeedLimit::Source speedLimitSource;
bool speedLimitAheadValid;
float speedLimitAhead;
float speedLimitAheadDistance;
@@ -94,7 +94,7 @@ private:
SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
bool roadName;
QString roadNameStr;
int speedLimitAssistState; // 0=NONE, 1=PRE_ACTIVE, etc.
cereal::LongitudinalPlanSP::SpeedLimit::AssistState speedLimitAssistState;
bool speedLimitAssistActive;
int speedLimitAssistFrame;
QPixmap plus_arrow_up_img;
@@ -131,15 +131,4 @@ private:
QString navigationNextModifier;
QString navigationNextManeuverType;
bool navigationHasNext;
QString wayName;
QString wayRef;
float mapdSpeedLimit;
float advisorySpeed;
float nextAdvisorySpeed;
float nextAdvisorySpeedDistance;
float suggestedSpeed;
float visionCurveSpeed;
float curveSpeed;
bool tileLoaded;
};

View File

@@ -8,6 +8,12 @@
#include "selfdrive/ui/sunnypilot/qt/onroad/model.h"
void ModelRendererSP::drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius) {
painter.setBrush(QColor(255, 255, 255, 200));
painter.setPen(Qt::NoPen);
painter.drawEllipse(pos, radius, radius);
}
void ModelRendererSP::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
ModelRenderer::update_model(model, lead);
const auto &model_position = model.getPosition();
@@ -48,7 +54,7 @@ void ModelRendererSP::draw(QPainter &painter, const QRect &surface_rect) {
if (s->scene.rainbow_mode) {
drawRainbowPath(painter, surface_rect);
} else {
ModelRenderer::drawPath(painter, model, surface_rect.height(), surface_rect.width());
ModelRenderer::drawPath(painter, model, surface_rect.height());
}
if (longitudinal_control && sm.alive("radarState")) {
@@ -67,6 +73,26 @@ void ModelRendererSP::draw(QPainter &painter, const QRect &surface_rect) {
const bool right_blindspot = car_state.getRightBlindspot();
drawBlindspot(painter, surface_rect, left_blindspot, right_blindspot);
}
if (s->scene.visual_radar_tracks) {
if (sm.alive("liveTracks") && sm.rcv_frame("liveTracks") >= s->scene.started_frame) {
const auto &tracks = sm["liveTracks"].getLiveTracks().getPoints();
for (const auto &track : tracks) {
if (!std::isfinite(track.getDRel()) || !std::isfinite(track.getYRel())) continue;
float t_lag = s->scene.visual_radar_tracks_delay;
float d_pred = track.getDRel();
float y_pred = track.getYRel();
if (t_lag > 0.0f) {
d_pred += track.getVRel() * t_lag + 0.5f * track.getARel() * t_lag * t_lag;
}
QPointF screen_pt;
if (mapToScreen(d_pred, -y_pred, path_offset_z, &screen_pt)) {
drawRadarPoint(painter, screen_pt, track.getVRel(), 10.0f);
}
}
}
}
drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
painter.restore();

View File

@@ -28,4 +28,6 @@ private:
// Lead status animation
float lead_status_alpha = 0.0f;
void drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius = 10.0f);
};

View File

@@ -29,8 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
"carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
"carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd",
"mapdOut"
"carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd", "liveTracks"
});
// update timer
@@ -45,6 +44,14 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
});
param_watcher->addParam("DevUIInfo");
param_watcher->addParam("StandstillTimer");
param_watcher->addParam("VisualRadarTracks");
param_watcher->addParam("VisualRadarTracksDelay");
param_watcher->addParam("VisualWideCam");
param_watcher->addParam("VisualStyle");
param_watcher->addParam("VisualStyleZoom");
param_watcher->addParam("VisualStyleOverhead");
param_watcher->addParam("VisualStyleOverheadZoom");
param_watcher->addParam("VisualStyleOverheadThreshold");
}
// This method overrides completely the update method from the parent class intentionally.
@@ -77,6 +84,17 @@ void ui_update_params_sp(UIStateSP *s) {
s->scene.chevron_info = std::atoi(params.get("ChevronInfo").c_str());
s->scene.blindspot_ui = params.getBool("BlindSpot");
s->scene.rainbow_mode = params.getBool("RainbowMode");
s->scene.visual_radar_tracks = QString::fromStdString(params.get("VisualRadarTracks")).toInt();
s->scene.visual_radar_tracks_delay = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
s->scene.visual_wide_cam = QString::fromStdString(params.get("VisualWideCam")).toInt();
s->scene.visual_style = QString::fromStdString(params.get("VisualStyle")).toInt();
s->scene.visual_style_zoom = QString::fromStdString(params.get("VisualStyleZoom")).toInt();
s->scene.visual_style_overhead = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
s->scene.visual_style_overhead_zoom = QString::fromStdString(params.get("VisualStyleOverheadZoom")).toInt();
s->scene.visual_style_overhead_threshold = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
}
void UIStateSP::reset_onroad_sleep_timer(OnroadTimerStatusToggle toggleTimerStatus) {

View File

@@ -21,4 +21,12 @@ typedef struct UISceneSP : UIScene {
int chevron_info;
bool blindspot_ui;
bool rainbow_mode;
int visual_radar_tracks = 0;
float visual_radar_tracks_delay = 0;
int visual_wide_cam = 0;
int visual_style = 0;
int visual_style_zoom = 0;
int visual_style_overhead = 0;
int visual_style_overhead_zoom = 0;
int visual_style_overhead_threshold = 20.0;
} UISceneSP;

View File

@@ -60,7 +60,6 @@ typedef struct UIScene {
cereal::PandaState::PandaType pandaType;
cereal::LongitudinalPersonality personality;
cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality;
float light_sensor = -1;
bool started, ignition, is_metric, recording_audio;

View File

@@ -28,7 +28,6 @@ from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
@@ -210,7 +209,6 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -316,10 +314,7 @@ def main(demo=False):
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
lane_change_prob = l_lane_change_prob + r_lane_change_prob
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

View File

@@ -26,7 +26,6 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
@@ -240,9 +239,6 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -344,10 +340,7 @@ def main(demo=False):
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
lane_change_prob = l_lane_change_prob + r_lane_change_prob
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

View File

@@ -116,7 +116,7 @@ class ModelCache:
class ModelFetcher:
"""Handles fetching and caching of model data from remote source"""
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-docs/refs/heads/gh-pages/docs/driving_models_v9.json"
MODEL_URL = "https://docs.sunnypilot.ai/driving_models_v8.json"
def __init__(self, params: Params):
self.params = params

View File

@@ -19,8 +19,8 @@ from openpilot.system.hardware.hw import Paths
from pathlib import Path
# see the README.md for more details on the model selector versioning
CURRENT_SELECTOR_VERSION = 11
REQUIRED_MIN_SELECTOR_VERSION = 11
CURRENT_SELECTOR_VERSION = 10
REQUIRED_MIN_SELECTOR_VERSION = 9
USE_ONNX = os.getenv('USE_ONNX', PC)

View File

@@ -14,10 +14,13 @@ CUSTOM_MODEL_PATH = Paths.model_root()
# Set QCOM environment variable for TICI devices, potentially enabling hardware acceleration
USBGPU = "USBGPU" in os.environ
if USBGPU:
os.environ['DEV'] = 'AMD'
os.environ['AMD'] = '1'
os.environ['AMD_IFACE'] = 'USB'
elif TICI:
os.environ['QCOM'] = '1'
else:
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
os.environ['LLVM'] = '1'
os.environ['JIT'] = '2' # TODO: This may cause issues
class ModelData:

View File

@@ -1,149 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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 cereal import custom
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
# Acceleration Profiles
MAX_ACCEL_PROFILES = {
AccelPersonality.eco: [1.8, 1.80, 1.40, .700, .410, .30, .22, .009],
AccelPersonality.normal: [1.9, 1.90, 1.50, .800, .530, .40, .26, .120],
AccelPersonality.sport: [2.0, 2.00, 1.60, .900, .680, .50, .35, .200],
}
MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.]
# Braking Profiles
MIN_ACCEL_PROFILES = {
AccelPersonality.eco: [-1.20, -1.20, -1.20],
AccelPersonality.normal: [-1.30, -1.30, -1.30],
AccelPersonality.sport: [-1.30, -1.40, -1.40],
}
MIN_ACCEL_BREAKPOINTS = [5., 10., 36.]
DECEL_SMOOTH_ALPHA = 0.02 # Very aggressive smoothing for decel (lower = smoother)
ACCEL_SMOOTH_ALPHA = 0.01 # Less aggressive for accel (higher = more responsive)
# Asymmetric rate limiting
MAX_DECEL_INCREASE_RATE = 0.1 # When braking harder (m/s² per second)
MAX_DECEL_DECREASE_RATE = 0.30 # When releasing brake (m/s² per second)
class AccelPersonalityController:
def __init__(self):
self.params = Params()
self.frame = 0
self.accel_personality = AccelPersonality.normal
self.last_max_accel = 2.0
self.last_min_accel = -0.01
self.first_run = True
self.param_keys = {
'personality': 'AccelPersonality',
'enabled': 'AccelPersonalityEnabled'
}
self._load_personality_from_params()
def _load_personality_from_params(self):
try:
saved = self.params.get(self.param_keys['personality'])
if saved is not None:
personality_value = int(saved)
if personality_value in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
self.accel_personality = personality_value
else:
self.accel_personality = AccelPersonality.normal
except (ValueError, TypeError):
self.accel_personality = AccelPersonality.normal
def _update_from_params(self):
if self.frame % int(1. / DT_MDL) != 0:
return
self._load_personality_from_params()
def get_accel_personality(self) -> int:
self._update_from_params()
return int(self.accel_personality)
def set_accel_personality(self, personality: int):
if personality not in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
return
self.accel_personality = personality
self.params.put(self.param_keys['personality'], str(personality))
def cycle_accel_personality(self) -> int:
personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
current_idx = personalities.index(self.accel_personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_accel_personality(next_personality)
return int(next_personality)
def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
v_ego = max(0.0, v_ego)
target_max_accel = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
target_min_accel = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality])
if self.first_run:
self.last_max_accel = target_max_accel
self.last_min_accel = target_min_accel
self.first_run = False
return float(target_min_accel), float(target_max_accel)
# exponential smoothing to max accel
self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max_accel + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel)
# VERY aggressive smoothing to min accel for ultra-smooth braking
smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min_accel + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel)
# asymmetric rate limiting
decel_change = smoothed_decel - self.last_min_accel
if decel_change < 0:
max_change_per_step = MAX_DECEL_INCREASE_RATE * DT_MDL
else:
max_change_per_step = MAX_DECEL_DECREASE_RATE * DT_MDL
decel_change = np.clip(decel_change, -max_change_per_step, max_change_per_step)
self.last_min_accel = self.last_min_accel + decel_change
if self.last_min_accel > self.last_max_accel:
self.last_min_accel = self.last_max_accel - 0.1
return float(self.last_min_accel), float(self.last_max_accel)
def get_min_accel(self, v_ego: float) -> float:
return self.get_accel_limits(v_ego)[0]
def get_max_accel(self, v_ego: float) -> float:
return self.get_accel_limits(v_ego)[1]
def is_enabled(self) -> bool:
return self.params.get_bool(self.param_keys['enabled'])
def set_enabled(self, enabled: bool):
self.params.put_bool(self.param_keys['enabled'], enabled)
def toggle_enabled(self) -> bool:
current = self.is_enabled()
self.set_enabled(not current)
return not current
def reset(self):
self.accel_personality = AccelPersonality.normal
self.frame = 0
self.last_max_accel = 2.0
self.last_min_accel = -0.01
self.first_run = True
def update(self):
self.frame += 1
self._update_from_params()

View File

@@ -1,128 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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 cereal import log
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
LongPersonality = log.LongitudinalPersonality
# Follow distance profiles mapped to LongPersonality
FOLLOW_PROFILES = {
LongPersonality.relaxed: [1.50, 1.50, 1.66, 1.66, 1.64, 1.89],
LongPersonality.standard: [1.30, 1.30, 1.45, 1.45, 1.44, 1.50],
LongPersonality.aggressive: [0.97, 0.97, 1.25, 1.25, 1.24, 1.28],
}
FOLLOW_BREAKPOINTS = [0., 3., 4, 11, 25., 36]
SMOOTHING_BASE = 0.99 # Base smoothing factor (higher = smoother)
SMOOTHING_RANGE = 0.50 # Additional smoothing at high speeds
SMOOTHING_SPEED_THRESHOLD = 36.0 # m/s (~80 mph) for max smoothing
PERSONALITY_CHANGE_COOLDOWN_S = 0.1
class FollowDistanceController:
def __init__(self):
self.params = Params()
self.frame = 0
self.personality = LongPersonality.standard
self.current_multiplier = None
self.first_run = True
self.personality_change_cooldown = 0
self.personality_cooldown_frames = int(PERSONALITY_CHANGE_COOLDOWN_S / DT_MDL)
self._load_personality()
def _load_personality(self):
try:
saved = self.params.get('LongitudinalPersonality')
if saved is not None:
val = int(saved)
if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
self.personality = val
except (ValueError, TypeError):
pass
def _update_from_params(self):
if self.frame % int(1. / DT_MDL) != 0:
return
if self.personality_change_cooldown > 0:
self.personality_change_cooldown -= 1
return
try:
param = self.params.get('LongitudinalPersonality')
if param is not None:
val = int(param)
if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
if val != self.personality:
self.personality = val
self.personality_change_cooldown = self.personality_cooldown_frames
except (ValueError, TypeError):
pass
def _get_smoothing_factor(self, v_ego: float) -> float:
speed_factor = np.clip(v_ego / SMOOTHING_SPEED_THRESHOLD, 0.3, 1.0)
return SMOOTHING_BASE + (SMOOTHING_RANGE * speed_factor)
def is_enabled(self) -> bool:
return self.params.get_bool('DynamicFollow')
def set_enabled(self, enabled: bool):
self.params.put_bool('DynamicFollow', enabled)
def toggle(self) -> bool:
enabled = self.is_enabled()
self.set_enabled(not enabled)
return not enabled
def get_personality(self) -> int:
self._update_from_params()
return int(self.personality)
def set_personality(self, personality: int):
if personality not in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
return
self.personality = personality
self.params.put('LongitudinalPersonality', str(personality))
self.personality_change_cooldown = self.personality_cooldown_frames
def cycle_personality(self) -> int:
personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
current_idx = personalities.index(self.personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_personality(next_personality)
return int(next_personality)
def get_follow_distance_multiplier(self, v_ego: float) -> float:
self._update_from_params()
v_ego = max(0.0, v_ego)
target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality]))
if self.first_run:
self.current_multiplier = target
self.first_run = False
return self.current_multiplier
#exponential smoothing with speedadaptive factor
alpha = self._get_smoothing_factor(v_ego)
self.current_multiplier = alpha * self.current_multiplier + (1.0 - alpha) * target
return self.current_multiplier
def reset(self):
self.personality = LongPersonality.standard
self.frame = 0
self.current_multiplier = None
self.first_run = True
self.personality_change_cooldown = 0
def update(self):
self.frame += 1
self._update_from_params()

View File

@@ -17,7 +17,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
@@ -25,27 +24,27 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
self.events_sp = EventsSP()
self.resolver = SpeedLimitResolver()
self.dec = DynamicExperimentalController(CP, mpc)
self.accel_controller = AccelPersonalityController()
self.scc = SmartCruiseControl()
self.resolver = SpeedLimitResolver()
self.sla = SpeedLimitAssist(CP, CP_SP)
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
self.source = LongitudinalPlanSource.cruise
self.e2e_alerts_helper = E2EAlertsHelper()
# Disabled controllers
self.scc = None
self.sla = None
self.resolver = None
self.output_v_target = 0.
self.output_a_target = 0.
@property
def mlsim(self) -> bool:
# If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
return bool(self.generation is None or self.generation >= 11)
def get_mpc_mode(self) -> str | None:
if not self.dec.active():
return None
return self.dec.mode()
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
@@ -53,20 +52,39 @@ class LongitudinalPlannerSP:
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
# Skip SCC and Speed Limit logic
self.source = LongitudinalPlanSource.cruise
self.output_v_target = v_cruise_cluster
self.output_a_target = a_ego
long_enabled = sm['carControl'].enabled
long_override = sm['carControl'].cruiseControl.override
# Smart Cruise Control
self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
# Speed Limit Resolver
self.resolver.update(v_ego, sm)
# Speed Limit Assist
has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp)
targets = {
LongitudinalPlanSource.cruise: (v_cruise, a_ego),
LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target),
}
self.source = min(targets, key=lambda k: targets[k][0])
self.output_v_target, self.output_a_target = targets[self.source]
return self.output_v_target, self.output_a_target
def update(self, sm: messaging.SubMaster) -> None:
self.events_sp.clear()
self.dec.update(sm)
self.e2e_alerts_helper.update(sm, self.events_sp)
self.accel_controller.update()
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
plan_sp_send = messaging.new_message('longitudinalPlanSP')
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
@@ -81,10 +99,43 @@ class LongitudinalPlannerSP:
dec.enabled = self.dec.enabled()
dec.active = self.dec.active()
# Skip SCC and Speed Limit fields (leave zeroed)
longitudinalPlanSP.smartCruiseControl.vision.enabled = False
longitudinalPlanSP.smartCruiseControl.map.enabled = False
longitudinalPlanSP.speedLimit.assist.enabled = False
# Smart Cruise Control
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
# Vision Control
sccVision = smartCruiseControl.vision
sccVision.state = self.scc.vision.state
sccVision.vTarget = float(self.scc.vision.output_v_target)
sccVision.aTarget = float(self.scc.vision.output_a_target)
sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc)
sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
sccVision.enabled = self.scc.vision.is_enabled
sccVision.active = self.scc.vision.is_active
# Map Control
sccMap = smartCruiseControl.map
sccMap.state = self.scc.map.state
sccMap.vTarget = float(self.scc.map.output_v_target)
sccMap.aTarget = float(self.scc.map.output_a_target)
sccMap.enabled = self.scc.map.is_enabled
sccMap.active = self.scc.map.is_active
# Speed Limit
speedLimit = longitudinalPlanSP.speedLimit
resolver = speedLimit.resolver
resolver.speedLimit = float(self.resolver.speed_limit)
resolver.speedLimitLast = float(self.resolver.speed_limit_last)
resolver.speedLimitFinal = float(self.resolver.speed_limit_final)
resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last)
resolver.speedLimitValid = self.resolver.speed_limit_valid
resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid
resolver.speedLimitOffset = float(self.resolver.speed_limit_offset)
resolver.distToSpeedLimit = float(self.resolver.distance)
resolver.source = self.resolver.source
assist = speedLimit.assist
assist.state = self.sla.state
assist.enabled = self.sla.is_enabled
assist.active = self.sla.is_active
assist.vTarget = float(self.sla.output_v_target)
assist.aTarget = float(self.sla.output_a_target)
# E2E Alerts
e2eAlerts = longitudinalPlanSP.e2eAlerts

View File

@@ -1,113 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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 numpy as np
from cereal import log
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
NEARSIDE_PROB = 0.2
EDGE_PROB = 0.35
EDGE_REACTION_TIME = 1.0
class RoadEdgeLaneChangeController:
def __init__(self, desire_helper):
self.desire_helper = desire_helper
self.params = Params()
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
self._frame = 0
def set_enabled(self, enabled):
self.enabled = enabled
if not enabled:
self._reset_state()
def _read_params(self) -> None:
if self._frame % int(1. / DT_MDL) == 0:
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
def _reset_state(self):
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
def _update_edge_detection(self, road_edge_stds, lane_line_probs):
if not self.enabled:
return
left_road_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
right_road_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
# Lane line probabilities: [left_outer, left_inner, right_inner, right_outer]
left_lane_nearside_prob = lane_line_probs[0] if len(lane_line_probs) > 0 else 0.0
right_lane_nearside_prob = lane_line_probs[3] if len(lane_line_probs) > 3 else 0.0
left_edge_conditions = (
left_road_edge_prob > EDGE_PROB and
left_lane_nearside_prob < NEARSIDE_PROB and
(len(lane_line_probs) <= 3 or right_lane_nearside_prob >= left_lane_nearside_prob)
)
right_edge_conditions = (
right_road_edge_prob > EDGE_PROB and
right_lane_nearside_prob < NEARSIDE_PROB and
(len(lane_line_probs) <= 0 or left_lane_nearside_prob >= right_lane_nearside_prob)
)
if left_edge_conditions:
self.left_edge_timer += DT_MDL
self.left_edge_detected = self.left_edge_timer > EDGE_REACTION_TIME
else:
self.left_edge_timer = 0.0
self.left_edge_detected = False
if right_edge_conditions:
self.right_edge_timer += DT_MDL
self.right_edge_detected = self.right_edge_timer > EDGE_REACTION_TIME
else:
self.right_edge_timer = 0.0
self.right_edge_detected = False
def update(self, road_edge_stds, lane_line_probs):
self._read_params()
if not self.enabled:
self._frame += 1
return
self._update_edge_detection(road_edge_stds, lane_line_probs)
self._frame += 1
def should_trigger_lane_change(self, carstate, lateral_active):
if not self.enabled:
return False, log.LaneChangeDirection.none
return False, log.LaneChangeDirection.none
def is_lane_change_blocked(self, direction):
if not self.enabled:
return False
if direction == log.LaneChangeDirection.left:
return self.left_edge_detected
elif direction == log.LaneChangeDirection.right:
return self.right_edge_detected
return False
def can_change_lane_left(self):
return not self.left_edge_detected if self.enabled else True
def can_change_lane_right(self):
return not self.right_edge_detected if self.enabled else True
@property
def edge_detected(self):
return self.left_edge_detected or self.right_edge_detected

View File

@@ -6,9 +6,6 @@ from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
TurnDirection = custom.ModelDataV2SP.TurnDirection
@@ -124,10 +121,7 @@ def set_lane_turn_params():
(DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none), # No blinkers? no desire!
])
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
dh = DesireHelper()
relc = RoadEdgeLaneChangeController(dh)
relc.set_enabled(True)
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
for _ in range(10):
dh.update(carstate, lateral_active, lane_change_prob, left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
dh = DesireHelper()
for _ in range(10):
dh.update(carstate, lateral_active, lane_change_prob)
assert dh.desire == expected_desire

View File

@@ -1,190 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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 pytest
from cereal import log
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController, EDGE_REACTION_TIME
@pytest.fixture
def relc_controller(mocker):
mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
mock_params.return_value.get_bool.return_value = True
DH = DesireHelper()
relc = RoadEdgeLaneChangeController(DH)
relc.set_enabled(True)
return relc
def test_disable_resets_state(relc_controller):
relc = relc_controller
relc.left_edge_detected = True
relc.right_edge_detected = True
relc.left_edge_timer = 5.0
relc.right_edge_timer = 5.0
relc.set_enabled(False)
assert not relc.left_edge_detected
assert not relc.right_edge_detected
assert relc.left_edge_timer == 0.0
assert relc.right_edge_timer == 0.0
def test_lane_change_blocked_left(relc_controller):
relc = relc_controller
relc.left_edge_detected = True
assert relc.is_lane_change_blocked(log.LaneChangeDirection.left)
def test_lane_change_blocked_right(relc_controller):
relc = relc_controller
relc.right_edge_detected = True
assert relc.is_lane_change_blocked(log.LaneChangeDirection.right)
def test_lane_change_not_blocked_opposite_side(relc_controller):
relc = relc_controller
relc.left_edge_detected = True
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
relc.left_edge_detected = False
relc.right_edge_detected = True
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
def test_lane_change_not_blocked_when_disabled(relc_controller):
relc = relc_controller
relc.set_enabled(False)
relc.left_edge_detected = True
relc.right_edge_detected = True
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
def test_can_change_lane_left(relc_controller):
relc = relc_controller
assert relc.can_change_lane_left()
relc.left_edge_detected = True
assert not relc.can_change_lane_left()
def test_can_change_lane_right(relc_controller):
relc = relc_controller
assert relc.can_change_lane_right()
relc.right_edge_detected = True
assert not relc.can_change_lane_right()
def test_can_change_lane_when_disabled(relc_controller):
relc = relc_controller
relc.set_enabled(False)
relc.left_edge_detected = True
relc.right_edge_detected = True
assert relc.can_change_lane_left()
assert relc.can_change_lane_right()
def test_edge_detected_property(relc_controller):
relc = relc_controller
assert not relc.edge_detected
relc.left_edge_detected = True
assert relc.edge_detected
relc.left_edge_detected = False
relc.right_edge_detected = True
assert relc.edge_detected
relc.left_edge_detected = True
assert relc.edge_detected
def test_should_trigger_lane_change(relc_controller):
relc = relc_controller
should_trigger, direction = relc.should_trigger_lane_change(None, True)
assert not should_trigger
assert direction == log.LaneChangeDirection.none
def test_update_increments_frame(relc_controller):
relc = relc_controller
initial = relc._frame
relc.update([0.5, 0.5], [0.5, 0.5, 0.5, 0.5])
assert relc._frame == initial + 1
def test_left_edge_detection(relc_controller):
relc = relc_controller
road_edge_stds = [0.0, 0.9]
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
for _ in range(num_updates):
relc.update(road_edge_stds, lane_line_probs)
assert relc.left_edge_detected
def test_right_edge_detection(relc_controller):
relc = relc_controller
road_edge_stds = [0.9, 0.0]
lane_line_probs = [0.8, 0.8, 0.8, 0.0]
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
for _ in range(num_updates):
relc.update(road_edge_stds, lane_line_probs)
assert relc.right_edge_detected
def test_edge_detection_requires_time(relc_controller):
relc = relc_controller
road_edge_stds = [0.0, 0.9]
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
num_updates = int(EDGE_REACTION_TIME / DT_MDL) - 1
for _ in range(num_updates):
relc.update(road_edge_stds, lane_line_probs)
assert not relc.left_edge_detected
def test_edge_detection_clears(relc_controller):
relc = relc_controller
road_edge_stds = [0.0, 0.9]
lane_line_probs = [0.0, 0.8, 0.8, 0.8]
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
for _ in range(num_updates):
relc.update(road_edge_stds, lane_line_probs)
assert relc.left_edge_detected
road_edge_stds = [0.9, 0.9]
relc.update(road_edge_stds, lane_line_probs)
assert not relc.left_edge_detected
assert relc.left_edge_timer == 0.0
def test_both_edges_detected(relc_controller):
relc = relc_controller
road_edge_stds = [0.0, 0.0]
lane_line_probs = [0.0, 0.8, 0.8, 0.0]
num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
for _ in range(num_updates):
relc.update(road_edge_stds, lane_line_probs)
assert relc.left_edge_detected
assert relc.right_edge_detected

View File

@@ -1,314 +0,0 @@
import pytest
# Import the actual modules
from cereal import log, custom
from openpilot.common.realtime import DT_MDL
# Import the enums we need for testing
LongPersonality = log.LongitudinalPersonality
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
class MockParams:
"""Simple mock for Params class"""
def __init__(self):
self.data = {}
self.bool_data = {
'VibePersonalityEnabled': True,
'VibeAccelPersonalityEnabled': True,
'VibeFollowPersonalityEnabled': True
}
def get(self, key, encoding=None):
return self.data.get(key)
def get_bool(self, key):
return self.bool_data.get(key, True)
def put(self, key, value):
self.data[key] = value
def put_bool(self, key, value):
self.bool_data[key] = value
def reset_mock(self):
self.call_count = 0
@property
def call_count(self):
return getattr(self, '_call_count', 0)
@call_count.setter
def call_count(self, value):
self._call_count = value
@pytest.fixture
def mock_params():
"""Create mock params instance"""
return MockParams()
@pytest.fixture
def controller(mock_params, monkeypatch):
"""Create controller instance with mocked Params"""
# Patch the Params import in the controller module
monkeypatch.setattr('openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality.Params',
lambda: mock_params)
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
return VibePersonalityController()
class TestVibePersonalityController:
def test_initialization(self, controller):
"""Test controller initializes with correct defaults"""
assert controller.frame == 0
assert controller.accel_personality == AccelPersonality.normal
assert controller.long_personality == LongPersonality.standard
assert 'accel_personality' in controller.param_keys
assert 'long_personality' in controller.param_keys
def test_frame_increment(self, controller):
"""Test frame counter increments correctly"""
initial_frame = controller.frame
controller.update()
assert controller.frame == initial_frame + 1
controller.update()
assert controller.frame == initial_frame + 2
def test_parameter_reading_throttled(self, controller, mock_params):
"""Test parameters are only read every DT_MDL frames"""
# Track calls manually
original_get = mock_params.get
call_count = 0
def counting_get(*args, **kwargs):
nonlocal call_count
call_count += 1
return original_get(*args, **kwargs)
mock_params.get = counting_get
# First call should read params (frame 0)
controller._update_from_params()
# Reset counter
call_count = 0
# Advance frame but not to threshold
controller.frame = 5 # Less than int(1/DT_MDL)
controller._update_from_params()
assert call_count == 0 # Should not read params
# Advance to threshold
controller.frame = int(1. / DT_MDL) # Equal to threshold
controller._update_from_params()
assert call_count >= 2 # Should read both personality params
def test_accel_personality_management(self, controller, mock_params):
"""Test acceleration personality setting and cycling"""
# Test setting valid personality
assert controller.set_accel_personality(AccelPersonality.eco)
assert controller.accel_personality == AccelPersonality.eco
assert controller.set_accel_personality(AccelPersonality.sport)
assert controller.accel_personality == AccelPersonality.sport
# Test setting invalid personality
assert not controller.set_accel_personality(999)
assert controller.accel_personality == AccelPersonality.sport # Should remain unchanged
# Test cycling
controller.accel_personality = AccelPersonality.eco
next_personality = controller.cycle_accel_personality()
assert next_personality == AccelPersonality.normal # should cycle to normal
assert controller.accel_personality == AccelPersonality.normal
next_personality = controller.cycle_accel_personality()
assert next_personality == AccelPersonality.sport # should cycle to sport
next_personality = controller.cycle_accel_personality()
assert next_personality == AccelPersonality.eco # should cycle back to eco
def test_long_personality_management(self, controller, mock_params):
"""Test longitudinal personality setting and cycling"""
# Test setting valid personality
assert controller.set_long_personality(LongPersonality.relaxed)
assert controller.long_personality == LongPersonality.relaxed
assert controller.set_long_personality(LongPersonality.aggressive)
assert controller.long_personality == LongPersonality.aggressive
# Test setting invalid personality
assert not controller.set_long_personality(999)
assert controller.long_personality == LongPersonality.aggressive # Should remain unchanged
# Test cycling
controller.long_personality = LongPersonality.standard
next_personality = controller.cycle_long_personality()
assert next_personality == LongPersonality.aggressive # should cycle to aggressive
assert controller.long_personality == LongPersonality.aggressive
next_personality = controller.cycle_long_personality()
assert next_personality == LongPersonality.relaxed # should cycle to relaxed
next_personality = controller.cycle_long_personality()
assert next_personality == LongPersonality.standard # should cycle back to standard
def test_toggle_functions(self, controller, mock_params):
"""Test toggle functionality"""
# Set initial state to False
mock_params.bool_data['VibePersonalityEnabled'] = False
result = controller.toggle_personality()
assert result # Should toggle to True
assert mock_params.bool_data['VibePersonalityEnabled']
# Set initial state to True
mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
result = controller.toggle_accel_personality()
assert not result # Should toggle to False
assert not mock_params.bool_data['VibeAccelPersonalityEnabled']
def test_enable_checks(self, controller, mock_params):
"""Test various enable state checks"""
# All enabled
mock_params.bool_data = {
'VibePersonalityEnabled': True,
'VibeAccelPersonalityEnabled': True,
'VibeFollowPersonalityEnabled': True
}
assert controller.is_enabled()
assert controller.is_accel_enabled()
assert controller.is_follow_enabled()
# Main toggle disabled
mock_params.bool_data['VibePersonalityEnabled'] = False
assert not controller.is_enabled()
assert not controller.is_accel_enabled()
assert not controller.is_follow_enabled()
def test_accel_limits_calculation(self, controller, mock_params):
"""Test acceleration limits calculation"""
# Enable all features through mock_params bool_data
mock_params.bool_data = {
'VibePersonalityEnabled': True,
'VibeAccelPersonalityEnabled': True,
'VibeFollowPersonalityEnabled': True
}
# Test with different speeds and personalities
controller.accel_personality = 1 # normal
controller.long_personality = 1 # standard
limits = controller.get_accel_limits(10.0) # 10 m/s
assert limits is not None
min_a, max_a = limits
assert isinstance(min_a, float)
assert isinstance(max_a, float)
assert min_a < 0 # Should be negative (braking)
assert max_a > 0 # Should be positive (acceleration)
# Test with disabled controller
mock_params.bool_data['VibePersonalityEnabled'] = False
limits = controller.get_accel_limits(10.0)
assert limits is None
def test_follow_distance_multiplier(self, controller, mock_params):
"""Test following distance multiplier calculation"""
# Enable controller
mock_params.bool_data['VibePersonalityEnabled'] = True
mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
# Test with different speeds and personalities
controller.long_personality = LongPersonality.relaxed
multiplier = controller.get_follow_distance_multiplier(15.0) # 15 m/s
assert multiplier is not None
assert isinstance(multiplier, float)
assert multiplier > 0
# Test with different personality - aggressive should have shorter distance
controller.long_personality = LongPersonality.aggressive
aggressive_multiplier = controller.get_follow_distance_multiplier(15.0)
assert aggressive_multiplier is not None
assert aggressive_multiplier < multiplier # Aggressive should have shorter distance
# Test with disabled controller
mock_params.bool_data['VibeFollowPersonalityEnabled'] = False
multiplier = controller.get_follow_distance_multiplier(15.0)
assert multiplier is None
def test_personality_differences(self, controller, mock_params):
"""Test that different personalities actually produce different values"""
# Enable controller
mock_params.bool_data['VibePersonalityEnabled'] = True
mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
# Test acceleration differences - sport should have higher max acceleration than eco
controller.accel_personality = AccelPersonality.eco
eco_limits = controller.get_accel_limits(20.0)
controller.accel_personality = AccelPersonality.sport
sport_limits = controller.get_accel_limits(20.0)
assert sport_limits[1] > eco_limits[1] # Sport should have higher max acceleration
# Test following distance differences - relaxed should have longer distance than aggressive
controller.long_personality = LongPersonality.relaxed
relaxed_dist = controller.get_follow_distance_multiplier(20.0)
controller.long_personality = LongPersonality.aggressive
aggressive_dist = controller.get_follow_distance_multiplier(20.0)
assert relaxed_dist > aggressive_dist # Relaxed should have longer following distance
def test_reset(self, controller):
"""Test reset functionality"""
# Change some values
controller.accel_personality = AccelPersonality.sport
controller.long_personality = LongPersonality.relaxed
controller.frame = 100
# Reset
controller.reset()
# Check defaults are restored
assert controller.accel_personality == AccelPersonality.normal
assert controller.long_personality == LongPersonality.standard
assert controller.frame == 0
def test_edge_cases(self, controller, mock_params):
"""Test edge cases and error handling"""
# Enable all features
mock_params.bool_data = {
'VibePersonalityEnabled': True,
'VibeAccelPersonalityEnabled': True,
'VibeFollowPersonalityEnabled': True
}
# Test with zero speed
limits = controller.get_accel_limits(0.0)
assert limits is not None
multiplier = controller.get_follow_distance_multiplier(0.0)
assert multiplier is not None
# Test with very high speed
limits = controller.get_accel_limits(100.0)
assert limits is not None
multiplier = controller.get_follow_distance_multiplier(100.0)
assert multiplier is not None
# Test interpolation works correctly
low_speed_limits = controller.get_accel_limits(5.0)
high_speed_limits = controller.get_accel_limits(50.0)
assert low_speed_limits[1] > high_speed_limits[1] # Max accel should decrease with speed

View File

@@ -1,144 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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 cereal import log, custom
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
LongPersonality = log.LongitudinalPersonality
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport)
MAX_ACCEL_PROFILES = {
AccelPersonality.eco: [2.0, 1.99, 1.88, 1.10, .500, .292, .15, .10], # eco
AccelPersonality.normal: [2.0, 2.00, 1.94, 1.22, .635, .33, .22, .16], # normal
AccelPersonality.sport: [2.0, 2.00, 2.00, 1.85, .800, .54, .32, .22], # sport
}
MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.]
# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive)
MIN_ACCEL_PROFILES = {
LongPersonality.relaxed: [-.0006, -.0006, -.010, -.30, -1.20], # gentler braking
LongPersonality.standard: [-.0007, -.0007, -.012, -.35, -1.20], # normal braking
LongPersonality.aggressive: [-.0020, -.0008, -.014, -.40, -1.20], # more aggressive braking
}
MIN_ACCEL_BREAKPOINTS = [0., 3.0, 11., 14, 50.]
# Follow distance profiles mapped to LongPersonality (relaxed/standard/aggressive)
FOLLOW_PROFILES = {
LongPersonality.relaxed: [1.55, 1.65, 1.65, 1.80], # more spread out
LongPersonality.standard: [1.45, 1.45, 1.45, 1.55], # balanced
LongPersonality.aggressive: [1.20, 1.25, 1.28, 1.35], # tighter
}
FOLLOW_BREAKPOINTS = [0., 6., 18., 36.]
class VibePersonalityController:
"""Controller for acceleration and distance personalities"""
def __init__(self):
self.params = Params()
self.frame = 0
self.accel_personality = AccelPersonality.normal
self.long_personality = LongPersonality.standard
self.param_keys = {
'accel_personality': 'AccelPersonality',
'long_personality': 'LongitudinalPersonality',
'enabled': 'VibePersonalityEnabled',
'accel_enabled': 'VibeAccelPersonalityEnabled',
'follow_enabled': 'VibeFollowPersonalityEnabled'
}
def _update_from_params(self):
"""Update personalities from params"""
if self.frame % int(1. / DT_MDL) != 0:
return
accel_personality_int = int(self.params.get(self.param_keys['accel_personality']))
self.accel_personality = accel_personality_int
long_personality_int = int(self.params.get(self.param_keys['long_personality']))
self.long_personality = long_personality_int
def _get_toggle_state(self, key: str) -> bool:
return self.params.get_bool(self.param_keys[key])
def _set_toggle_state(self, key: str, value: bool):
self.params.put_bool(self.param_keys[key], value)
def set_accel_personality(self, personality: int) -> bool:
self.accel_personality = personality
self.params.put(self.param_keys['accel_personality'], str(personality))
return True
def cycle_accel_personality(self) -> int:
personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
current_idx = personalities.index(self.accel_personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_accel_personality(next_personality)
return int(next_personality)
def get_accel_personality(self) -> int:
self._update_from_params()
return int(self.accel_personality)
def set_long_personality(self, personality: int) -> bool:
self.long_personality = personality
self.params.put(self.param_keys['long_personality'], str(personality))
return True
def cycle_long_personality(self) -> int:
personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
current_idx = personalities.index(self.long_personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_long_personality(next_personality)
return int(next_personality)
def get_long_personality(self) -> int:
self._update_from_params()
return int(self.long_personality)
def toggle_personality(self): return self._toggle_flag('enabled')
def toggle_accel_personality(self): return self._toggle_flag('accel_enabled')
def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled')
def _toggle_flag(self, key):
current = self._get_toggle_state(key)
self._set_toggle_state(key, not current)
return not current
def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled)
def is_accel_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled')
def is_follow_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled')
def is_enabled(self) -> bool: return self._get_toggle_state('enabled') and (self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled'))
def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
"""Get acceleration limits based on current personalities."""
self._update_from_params()
max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality])
return float(min_a), float(max_a)
def get_follow_distance_multiplier(self, v_ego: float) -> float:
"""Get dynamic following distance based on speed and personality"""
self._update_from_params()
return float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.long_personality]))
def get_min_accel(self, v_ego: float) -> float:
return self.get_accel_limits(v_ego)[0]
def get_max_accel(self, v_ego: float) -> float:
return self.get_accel_limits(v_ego)[1]
def reset(self):
self.accel_personality = AccelPersonality.normal
self.long_personality = LongPersonality.standard
self.frame = 0
def update(self):
self.frame += 1

View File

@@ -226,12 +226,4 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
AlertStatus.normal, AlertSize.none,
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
},
EventNameSP.laneChangeRoadEdge: {
ET.WARNING: Alert(
"Lane Change Unavailable: Road Edge",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
},
}

View File

@@ -178,15 +178,14 @@ procs += [
PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),
# mapd
#NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
#PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
NativeProcess("mapd", "selfdrive", ["./mapd"], always_run),
NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
# navigationd
PythonProcess("navigationd", "sunnypilot.navd.navigationd", only_onroad),
# locationd
#NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
]
if os.path.exists("./github_runner.sh"):