mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-14 16:44:38 +08:00
Compare commits
129 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
672053db65 | ||
|
|
774710e95e | ||
|
|
8cdc236585 | ||
|
|
ec031ffeb6 | ||
|
|
9c5ee2e12a | ||
|
|
65386da6d4 | ||
|
|
00799154ef | ||
|
|
d6731c30da | ||
|
|
0b54b86476 | ||
|
|
fd675d29a9 | ||
|
|
d58bbda789 | ||
|
|
50773a62ed | ||
|
|
f5dbdc192e | ||
|
|
f9987b2d7d | ||
|
|
d51af1513b | ||
|
|
2807c949a9 | ||
|
|
0268d4384d | ||
|
|
0e60a56e56 | ||
|
|
c5445d2a8b | ||
|
|
6df1edbbaf | ||
|
|
a098a0805a | ||
|
|
469f1e01ef | ||
|
|
0fc5414f7c | ||
|
|
d798288b2c | ||
|
|
0e02fc2449 | ||
|
|
7d15afe5bc | ||
|
|
b6dd2d14db | ||
|
|
7d4e5bedaf | ||
|
|
1063114408 | ||
|
|
958b4df69f | ||
|
|
f5953c5d8c | ||
|
|
72998034e6 | ||
|
|
cefb344183 | ||
|
|
c0da31abb6 | ||
|
|
bd759a56cf | ||
|
|
befc73c53e | ||
|
|
8dbfc267ac | ||
|
|
d17e80ad94 | ||
|
|
c2b7087723 | ||
|
|
81b37712f1 | ||
|
|
68270a13a3 | ||
|
|
18cd3633e5 | ||
|
|
9c6a4d4a57 | ||
|
|
1a4c48249b | ||
|
|
95d887a417 | ||
|
|
e297b4c03f | ||
|
|
1132377837 | ||
|
|
35f03ae001 | ||
|
|
1c0b54a447 | ||
|
|
8f0cdd514e | ||
|
|
3681caa717 | ||
|
|
7446c43f69 | ||
|
|
5f5e3668eb | ||
|
|
8c07958f6f | ||
|
|
ca1ce9bcc9 | ||
|
|
34a0819bc5 | ||
|
|
c68ea82a5d | ||
|
|
3157054100 | ||
|
|
2486ef1825 | ||
|
|
29f15dc8ed | ||
|
|
31a5a3b3c0 | ||
|
|
d8fa3cfd04 | ||
|
|
2a4b348497 | ||
|
|
3ef3aceb4b | ||
|
|
3d8763b3ce | ||
|
|
b2427a5f20 | ||
|
|
7ddafe62cd | ||
|
|
ff4cc96a81 | ||
|
|
3b1ada64be | ||
|
|
6a08186434 | ||
|
|
cf2b033c79 | ||
|
|
9fbef36c6b | ||
|
|
f5a38aa613 | ||
|
|
25f5058430 | ||
|
|
7b28c2f59a | ||
|
|
99d954de10 | ||
|
|
b28f33481c | ||
|
|
589e33f665 | ||
|
|
39342d7b5e | ||
|
|
fe70650f73 | ||
|
|
e3f9fe892a | ||
|
|
f4373fa244 | ||
|
|
2376802589 | ||
|
|
c3b51d7335 | ||
|
|
d3d8802402 | ||
|
|
d866500c92 | ||
|
|
23879836d9 | ||
|
|
06add21971 | ||
|
|
66fd3d1a01 | ||
|
|
71f7754f51 | ||
|
|
b5591cbd62 | ||
|
|
7430c450c2 | ||
|
|
e54a39cf43 | ||
|
|
3afe0bcdb3 | ||
|
|
b763f7aac1 | ||
|
|
450fcd4d55 | ||
|
|
551b4dea31 | ||
|
|
bd269defb3 | ||
|
|
8998f63a28 | ||
|
|
399ed08926 | ||
|
|
90f02040fe | ||
|
|
8423ecedb1 | ||
|
|
34d1514e11 | ||
|
|
c50d511616 | ||
|
|
dd1479ed82 | ||
|
|
87ec262e39 | ||
|
|
f82845ff42 | ||
|
|
efcc5ccd15 | ||
|
|
6aac50ab56 | ||
|
|
da0920cb60 | ||
|
|
091bce4a3a | ||
|
|
088f6aa407 | ||
|
|
211c8adcce | ||
|
|
fe5366e5b2 | ||
|
|
1ecb0b0f66 | ||
|
|
51e455db79 | ||
|
|
dc6672fa80 | ||
|
|
07b8e7783d | ||
|
|
f17b0f200c | ||
|
|
ad9bde8b1f | ||
|
|
8cf9f9fe23 | ||
|
|
713985d823 | ||
|
|
088f9d0b59 | ||
|
|
53bf5b0d41 | ||
|
|
8c33592628 | ||
|
|
3bbb33f6bd | ||
|
|
5bd9549bd1 | ||
|
|
3481702715 | ||
|
|
c9781ee31d |
@@ -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}
|
||||
|
||||
@@ -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}
|
||||
|
||||
@@ -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
1
.gitmodules
vendored
@@ -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
|
||||
|
||||
@@ -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 {
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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}},
|
||||
|
||||
Submodule opendbc_repo updated: 2bcc263692...c32e79f3c6
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
BIN
selfdrive/mapd
BIN
selfdrive/mapd
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 ¶m) {
|
||||
@@ -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.");
|
||||
|
||||
@@ -88,7 +88,6 @@ protected:
|
||||
Params params;
|
||||
std::map<std::string, ParamControl*> toggles;
|
||||
ButtonParamControl *long_personality_setting;
|
||||
ButtonParamControl *accel_personality_setting;
|
||||
|
||||
virtual void updateToggles();
|
||||
};
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -36,9 +36,6 @@ private:
|
||||
ParamControl *SmartCruiseControlMap;
|
||||
ParamControl *intelligentCruiseButtonManagement = nullptr;
|
||||
ParamControl *dynamicExperimentalControl = nullptr;
|
||||
|
||||
ParamControlSP *AccelPersonalityControl;
|
||||
ParamControlSP *DynamicPersonalityControl;
|
||||
SpeedLimitSettings *speedLimitScreen;
|
||||
PushButtonSP *speedLimitSettings;
|
||||
};
|
||||
|
||||
@@ -11,6 +11,18 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
|
||||
param_watcher = new ParamWatcher(this);
|
||||
connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString ¶m_name, const QString ¶m_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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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),
|
||||
},
|
||||
}
|
||||
|
||||
@@ -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"):
|
||||
|
||||
Submodule tinygrad_repo updated: 7296c74cbd...d2bb1bcb97
Reference in New Issue
Block a user