mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-04 15:13:58 +08:00
New Following
This commit is contained in:
firestar5683
+29
-28
@@ -193,34 +193,35 @@ struct FrogPilotPlan @0xa1680744031fdb2d {
|
||||
cscTraining @4 :Bool;
|
||||
dangerJerk @5 :Float32;
|
||||
desiredFollowDistance @6 :Int64;
|
||||
experimentalMode @7 :Bool;
|
||||
forcingStop @8 :Bool;
|
||||
forcingStopLength @9 :Float32;
|
||||
frogpilotEvents @10 :List(FrogPilotCarEvent);
|
||||
increasedStoppedDistance @11 :Float32;
|
||||
lateralCheck @12 :Bool;
|
||||
laneWidthLeft @13 :Float32;
|
||||
laneWidthRight @14 :Float32;
|
||||
maxAcceleration @15 :Float32;
|
||||
minAcceleration @16 :Float32;
|
||||
redLight @17 :Bool;
|
||||
roadCurvature @18 :Float32;
|
||||
slcMapSpeedLimit @19 :Float32;
|
||||
slcMapboxSpeedLimit @20 :Float32;
|
||||
slcNextSpeedLimit @21 :Float32;
|
||||
slcOverriddenSpeed @22 :Float32;
|
||||
slcSpeedLimit @23 :Float32;
|
||||
slcSpeedLimitOffset @24 :Float32;
|
||||
slcSpeedLimitSource @25 :Text;
|
||||
speedJerk @26 :Float32;
|
||||
speedJerkStock @27 :Float32;
|
||||
speedLimitChanged @28 :Bool;
|
||||
tFollow @29 :Float32;
|
||||
themeUpdated @30 :Bool;
|
||||
togglesUpdated @31 :Bool;
|
||||
trackingLead @32 :Bool;
|
||||
unconfirmedSlcSpeedLimit @33 :Float32;
|
||||
vCruise @34 :Float32;
|
||||
disableThrottle @7 :Bool;
|
||||
experimentalMode @8 :Bool;
|
||||
forcingStop @9 :Bool;
|
||||
forcingStopLength @10 :Float32;
|
||||
frogpilotEvents @11 :List(FrogPilotCarEvent);
|
||||
increasedStoppedDistance @12 :Float32;
|
||||
lateralCheck @13 :Bool;
|
||||
laneWidthLeft @14 :Float32;
|
||||
laneWidthRight @15 :Float32;
|
||||
maxAcceleration @16 :Float32;
|
||||
minAcceleration @17 :Float32;
|
||||
redLight @18 :Bool;
|
||||
roadCurvature @19 :Float32;
|
||||
slcMapSpeedLimit @20 :Float32;
|
||||
slcMapboxSpeedLimit @21 :Float32;
|
||||
slcNextSpeedLimit @22 :Float32;
|
||||
slcOverriddenSpeed @23 :Float32;
|
||||
slcSpeedLimit @24 :Float32;
|
||||
slcSpeedLimitOffset @25 :Float32;
|
||||
slcSpeedLimitSource @26 :Text;
|
||||
speedJerk @27 :Float32;
|
||||
speedJerkStock @28 :Float32;
|
||||
speedLimitChanged @29 :Bool;
|
||||
tFollow @30 :Float32;
|
||||
themeUpdated @31 :Bool;
|
||||
togglesUpdated @32 :Bool;
|
||||
trackingLead @33 :Bool;
|
||||
unconfirmedSlcSpeedLimit @34 :Float32;
|
||||
vCruise @35 :Float32;
|
||||
}
|
||||
|
||||
struct FrogPilotRadarState @0xcb9fd56c7057593a {
|
||||
|
||||
+178
-162
@@ -1739,7 +1739,7 @@ const ::capnp::_::RawSchema s_f416ec09499d9d19 = {
|
||||
0, 3, i_f416ec09499d9d19, nullptr, nullptr, { &s_f416ec09499d9d19, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<597> b_a1680744031fdb2d = {
|
||||
static const ::capnp::_::AlignedData<613> b_a1680744031fdb2d = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
45, 219, 31, 3, 68, 7, 104, 161,
|
||||
13, 0, 0, 0, 1, 0, 13, 0,
|
||||
@@ -1749,7 +1749,7 @@ static const ::capnp::_::AlignedData<597> b_a1680744031fdb2d = {
|
||||
21, 0, 0, 0, 218, 0, 0, 0,
|
||||
33, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
29, 0, 0, 0, 175, 7, 0, 0,
|
||||
29, 0, 0, 0, 231, 7, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
@@ -1757,252 +1757,259 @@ static const ::capnp::_::AlignedData<597> b_a1680744031fdb2d = {
|
||||
103, 80, 105, 108, 111, 116, 80, 108,
|
||||
97, 110, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 1, 0, 1, 0,
|
||||
140, 0, 0, 0, 3, 0, 4, 0,
|
||||
144, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
197, 3, 0, 0, 138, 0, 0, 0,
|
||||
225, 3, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
200, 3, 0, 0, 3, 0, 1, 0,
|
||||
212, 3, 0, 0, 2, 0, 1, 0,
|
||||
228, 3, 0, 0, 3, 0, 1, 0,
|
||||
240, 3, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
209, 3, 0, 0, 178, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
212, 3, 0, 0, 3, 0, 1, 0,
|
||||
224, 3, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 64, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
221, 3, 0, 0, 162, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
224, 3, 0, 0, 3, 0, 1, 0,
|
||||
236, 3, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
233, 3, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
232, 3, 0, 0, 3, 0, 1, 0,
|
||||
244, 3, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 65, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
241, 3, 0, 0, 98, 0, 0, 0,
|
||||
237, 3, 0, 0, 178, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
240, 3, 0, 0, 3, 0, 1, 0,
|
||||
252, 3, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
2, 0, 0, 0, 64, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
249, 3, 0, 0, 90, 0, 0, 0,
|
||||
249, 3, 0, 0, 162, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
248, 3, 0, 0, 3, 0, 1, 0,
|
||||
4, 4, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
252, 3, 0, 0, 3, 0, 1, 0,
|
||||
8, 4, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 4, 0, 0, 178, 0, 0, 0,
|
||||
5, 4, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
4, 4, 0, 0, 3, 0, 1, 0,
|
||||
16, 4, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 65, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
13, 4, 0, 0, 98, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
12, 4, 0, 0, 3, 0, 1, 0,
|
||||
24, 4, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 4, 0, 0, 90, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
20, 4, 0, 0, 3, 0, 1, 0,
|
||||
32, 4, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
29, 4, 0, 0, 178, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
32, 4, 0, 0, 3, 0, 1, 0,
|
||||
44, 4, 0, 0, 2, 0, 1, 0,
|
||||
7, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 1, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
13, 4, 0, 0, 138, 0, 0, 0,
|
||||
41, 4, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
16, 4, 0, 0, 3, 0, 1, 0,
|
||||
28, 4, 0, 0, 2, 0, 1, 0,
|
||||
40, 4, 0, 0, 3, 0, 1, 0,
|
||||
52, 4, 0, 0, 2, 0, 1, 0,
|
||||
8, 0, 0, 0, 67, 0, 0, 0,
|
||||
0, 0, 1, 0, 8, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
25, 4, 0, 0, 98, 0, 0, 0,
|
||||
49, 4, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
24, 4, 0, 0, 3, 0, 1, 0,
|
||||
36, 4, 0, 0, 2, 0, 1, 0,
|
||||
9, 0, 0, 0, 5, 0, 0, 0,
|
||||
52, 4, 0, 0, 3, 0, 1, 0,
|
||||
64, 4, 0, 0, 2, 0, 1, 0,
|
||||
9, 0, 0, 0, 68, 0, 0, 0,
|
||||
0, 0, 1, 0, 9, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
33, 4, 0, 0, 146, 0, 0, 0,
|
||||
61, 4, 0, 0, 98, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
36, 4, 0, 0, 3, 0, 1, 0,
|
||||
48, 4, 0, 0, 2, 0, 1, 0,
|
||||
10, 0, 0, 0, 0, 0, 0, 0,
|
||||
60, 4, 0, 0, 3, 0, 1, 0,
|
||||
72, 4, 0, 0, 2, 0, 1, 0,
|
||||
10, 0, 0, 0, 5, 0, 0, 0,
|
||||
0, 0, 1, 0, 10, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
45, 4, 0, 0, 130, 0, 0, 0,
|
||||
69, 4, 0, 0, 146, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
44, 4, 0, 0, 3, 0, 1, 0,
|
||||
72, 4, 0, 0, 2, 0, 1, 0,
|
||||
11, 0, 0, 0, 8, 0, 0, 0,
|
||||
72, 4, 0, 0, 3, 0, 1, 0,
|
||||
84, 4, 0, 0, 2, 0, 1, 0,
|
||||
11, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 11, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
69, 4, 0, 0, 202, 0, 0, 0,
|
||||
81, 4, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
76, 4, 0, 0, 3, 0, 1, 0,
|
||||
88, 4, 0, 0, 2, 0, 1, 0,
|
||||
12, 0, 0, 0, 68, 0, 0, 0,
|
||||
80, 4, 0, 0, 3, 0, 1, 0,
|
||||
108, 4, 0, 0, 2, 0, 1, 0,
|
||||
12, 0, 0, 0, 8, 0, 0, 0,
|
||||
0, 0, 1, 0, 12, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
85, 4, 0, 0, 106, 0, 0, 0,
|
||||
105, 4, 0, 0, 202, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
84, 4, 0, 0, 3, 0, 1, 0,
|
||||
96, 4, 0, 0, 2, 0, 1, 0,
|
||||
13, 0, 0, 0, 9, 0, 0, 0,
|
||||
112, 4, 0, 0, 3, 0, 1, 0,
|
||||
124, 4, 0, 0, 2, 0, 1, 0,
|
||||
13, 0, 0, 0, 69, 0, 0, 0,
|
||||
0, 0, 1, 0, 13, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
93, 4, 0, 0, 114, 0, 0, 0,
|
||||
121, 4, 0, 0, 106, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
92, 4, 0, 0, 3, 0, 1, 0,
|
||||
104, 4, 0, 0, 2, 0, 1, 0,
|
||||
14, 0, 0, 0, 10, 0, 0, 0,
|
||||
120, 4, 0, 0, 3, 0, 1, 0,
|
||||
132, 4, 0, 0, 2, 0, 1, 0,
|
||||
14, 0, 0, 0, 9, 0, 0, 0,
|
||||
0, 0, 1, 0, 14, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
101, 4, 0, 0, 122, 0, 0, 0,
|
||||
129, 4, 0, 0, 114, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
100, 4, 0, 0, 3, 0, 1, 0,
|
||||
112, 4, 0, 0, 2, 0, 1, 0,
|
||||
15, 0, 0, 0, 11, 0, 0, 0,
|
||||
128, 4, 0, 0, 3, 0, 1, 0,
|
||||
140, 4, 0, 0, 2, 0, 1, 0,
|
||||
15, 0, 0, 0, 10, 0, 0, 0,
|
||||
0, 0, 1, 0, 15, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
109, 4, 0, 0, 130, 0, 0, 0,
|
||||
137, 4, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
108, 4, 0, 0, 3, 0, 1, 0,
|
||||
120, 4, 0, 0, 2, 0, 1, 0,
|
||||
16, 0, 0, 0, 12, 0, 0, 0,
|
||||
136, 4, 0, 0, 3, 0, 1, 0,
|
||||
148, 4, 0, 0, 2, 0, 1, 0,
|
||||
16, 0, 0, 0, 11, 0, 0, 0,
|
||||
0, 0, 1, 0, 16, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
117, 4, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
116, 4, 0, 0, 3, 0, 1, 0,
|
||||
128, 4, 0, 0, 2, 0, 1, 0,
|
||||
17, 0, 0, 0, 69, 0, 0, 0,
|
||||
0, 0, 1, 0, 17, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
125, 4, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
124, 4, 0, 0, 3, 0, 1, 0,
|
||||
136, 4, 0, 0, 2, 0, 1, 0,
|
||||
18, 0, 0, 0, 13, 0, 0, 0,
|
||||
0, 0, 1, 0, 18, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
133, 4, 0, 0, 114, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
132, 4, 0, 0, 3, 0, 1, 0,
|
||||
144, 4, 0, 0, 2, 0, 1, 0,
|
||||
19, 0, 0, 0, 14, 0, 0, 0,
|
||||
0, 0, 1, 0, 19, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
141, 4, 0, 0, 138, 0, 0, 0,
|
||||
145, 4, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
144, 4, 0, 0, 3, 0, 1, 0,
|
||||
156, 4, 0, 0, 2, 0, 1, 0,
|
||||
20, 0, 0, 0, 15, 0, 0, 0,
|
||||
0, 0, 1, 0, 20, 0, 0, 0,
|
||||
17, 0, 0, 0, 12, 0, 0, 0,
|
||||
0, 0, 1, 0, 17, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
153, 4, 0, 0, 162, 0, 0, 0,
|
||||
153, 4, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
156, 4, 0, 0, 3, 0, 1, 0,
|
||||
168, 4, 0, 0, 2, 0, 1, 0,
|
||||
21, 0, 0, 0, 16, 0, 0, 0,
|
||||
0, 0, 1, 0, 21, 0, 0, 0,
|
||||
152, 4, 0, 0, 3, 0, 1, 0,
|
||||
164, 4, 0, 0, 2, 0, 1, 0,
|
||||
18, 0, 0, 0, 70, 0, 0, 0,
|
||||
0, 0, 1, 0, 18, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
165, 4, 0, 0, 146, 0, 0, 0,
|
||||
161, 4, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
160, 4, 0, 0, 3, 0, 1, 0,
|
||||
172, 4, 0, 0, 2, 0, 1, 0,
|
||||
19, 0, 0, 0, 13, 0, 0, 0,
|
||||
0, 0, 1, 0, 19, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
169, 4, 0, 0, 114, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
168, 4, 0, 0, 3, 0, 1, 0,
|
||||
180, 4, 0, 0, 2, 0, 1, 0,
|
||||
22, 0, 0, 0, 17, 0, 0, 0,
|
||||
0, 0, 1, 0, 22, 0, 0, 0,
|
||||
20, 0, 0, 0, 14, 0, 0, 0,
|
||||
0, 0, 1, 0, 20, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
177, 4, 0, 0, 154, 0, 0, 0,
|
||||
177, 4, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
180, 4, 0, 0, 3, 0, 1, 0,
|
||||
192, 4, 0, 0, 2, 0, 1, 0,
|
||||
23, 0, 0, 0, 18, 0, 0, 0,
|
||||
21, 0, 0, 0, 15, 0, 0, 0,
|
||||
0, 0, 1, 0, 21, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
189, 4, 0, 0, 162, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
192, 4, 0, 0, 3, 0, 1, 0,
|
||||
204, 4, 0, 0, 2, 0, 1, 0,
|
||||
22, 0, 0, 0, 16, 0, 0, 0,
|
||||
0, 0, 1, 0, 22, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
201, 4, 0, 0, 146, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
204, 4, 0, 0, 3, 0, 1, 0,
|
||||
216, 4, 0, 0, 2, 0, 1, 0,
|
||||
23, 0, 0, 0, 17, 0, 0, 0,
|
||||
0, 0, 1, 0, 23, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
189, 4, 0, 0, 114, 0, 0, 0,
|
||||
213, 4, 0, 0, 154, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
188, 4, 0, 0, 3, 0, 1, 0,
|
||||
200, 4, 0, 0, 2, 0, 1, 0,
|
||||
24, 0, 0, 0, 19, 0, 0, 0,
|
||||
216, 4, 0, 0, 3, 0, 1, 0,
|
||||
228, 4, 0, 0, 2, 0, 1, 0,
|
||||
24, 0, 0, 0, 18, 0, 0, 0,
|
||||
0, 0, 1, 0, 24, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
197, 4, 0, 0, 162, 0, 0, 0,
|
||||
225, 4, 0, 0, 114, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
200, 4, 0, 0, 3, 0, 1, 0,
|
||||
212, 4, 0, 0, 2, 0, 1, 0,
|
||||
25, 0, 0, 0, 1, 0, 0, 0,
|
||||
224, 4, 0, 0, 3, 0, 1, 0,
|
||||
236, 4, 0, 0, 2, 0, 1, 0,
|
||||
25, 0, 0, 0, 19, 0, 0, 0,
|
||||
0, 0, 1, 0, 25, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
209, 4, 0, 0, 162, 0, 0, 0,
|
||||
233, 4, 0, 0, 162, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
212, 4, 0, 0, 3, 0, 1, 0,
|
||||
224, 4, 0, 0, 2, 0, 1, 0,
|
||||
26, 0, 0, 0, 20, 0, 0, 0,
|
||||
236, 4, 0, 0, 3, 0, 1, 0,
|
||||
248, 4, 0, 0, 2, 0, 1, 0,
|
||||
26, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
221, 4, 0, 0, 82, 0, 0, 0,
|
||||
245, 4, 0, 0, 162, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
220, 4, 0, 0, 3, 0, 1, 0,
|
||||
232, 4, 0, 0, 2, 0, 1, 0,
|
||||
27, 0, 0, 0, 21, 0, 0, 0,
|
||||
248, 4, 0, 0, 3, 0, 1, 0,
|
||||
4, 5, 0, 0, 2, 0, 1, 0,
|
||||
27, 0, 0, 0, 20, 0, 0, 0,
|
||||
0, 0, 1, 0, 27, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
229, 4, 0, 0, 122, 0, 0, 0,
|
||||
1, 5, 0, 0, 82, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
228, 4, 0, 0, 3, 0, 1, 0,
|
||||
240, 4, 0, 0, 2, 0, 1, 0,
|
||||
28, 0, 0, 0, 70, 0, 0, 0,
|
||||
0, 5, 0, 0, 3, 0, 1, 0,
|
||||
12, 5, 0, 0, 2, 0, 1, 0,
|
||||
28, 0, 0, 0, 21, 0, 0, 0,
|
||||
0, 0, 1, 0, 28, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
237, 4, 0, 0, 146, 0, 0, 0,
|
||||
9, 5, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
240, 4, 0, 0, 3, 0, 1, 0,
|
||||
252, 4, 0, 0, 2, 0, 1, 0,
|
||||
29, 0, 0, 0, 22, 0, 0, 0,
|
||||
8, 5, 0, 0, 3, 0, 1, 0,
|
||||
20, 5, 0, 0, 2, 0, 1, 0,
|
||||
29, 0, 0, 0, 71, 0, 0, 0,
|
||||
0, 0, 1, 0, 29, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
249, 4, 0, 0, 66, 0, 0, 0,
|
||||
17, 5, 0, 0, 146, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
244, 4, 0, 0, 3, 0, 1, 0,
|
||||
0, 5, 0, 0, 2, 0, 1, 0,
|
||||
30, 0, 0, 0, 71, 0, 0, 0,
|
||||
20, 5, 0, 0, 3, 0, 1, 0,
|
||||
32, 5, 0, 0, 2, 0, 1, 0,
|
||||
30, 0, 0, 0, 22, 0, 0, 0,
|
||||
0, 0, 1, 0, 30, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
253, 4, 0, 0, 106, 0, 0, 0,
|
||||
29, 5, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
252, 4, 0, 0, 3, 0, 1, 0,
|
||||
8, 5, 0, 0, 2, 0, 1, 0,
|
||||
24, 5, 0, 0, 3, 0, 1, 0,
|
||||
36, 5, 0, 0, 2, 0, 1, 0,
|
||||
31, 0, 0, 0, 72, 0, 0, 0,
|
||||
0, 0, 1, 0, 31, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
5, 5, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
4, 5, 0, 0, 3, 0, 1, 0,
|
||||
16, 5, 0, 0, 2, 0, 1, 0,
|
||||
32, 0, 0, 0, 73, 0, 0, 0,
|
||||
0, 0, 1, 0, 32, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
13, 5, 0, 0, 106, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
12, 5, 0, 0, 3, 0, 1, 0,
|
||||
24, 5, 0, 0, 2, 0, 1, 0,
|
||||
33, 0, 0, 0, 23, 0, 0, 0,
|
||||
0, 0, 1, 0, 33, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 5, 0, 0, 202, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
28, 5, 0, 0, 3, 0, 1, 0,
|
||||
40, 5, 0, 0, 2, 0, 1, 0,
|
||||
34, 0, 0, 0, 24, 0, 0, 0,
|
||||
0, 0, 1, 0, 34, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
37, 5, 0, 0, 66, 0, 0, 0,
|
||||
33, 5, 0, 0, 106, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
32, 5, 0, 0, 3, 0, 1, 0,
|
||||
44, 5, 0, 0, 2, 0, 1, 0,
|
||||
32, 0, 0, 0, 73, 0, 0, 0,
|
||||
0, 0, 1, 0, 32, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
41, 5, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
40, 5, 0, 0, 3, 0, 1, 0,
|
||||
52, 5, 0, 0, 2, 0, 1, 0,
|
||||
33, 0, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 1, 0, 33, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
49, 5, 0, 0, 106, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
48, 5, 0, 0, 3, 0, 1, 0,
|
||||
60, 5, 0, 0, 2, 0, 1, 0,
|
||||
34, 0, 0, 0, 23, 0, 0, 0,
|
||||
0, 0, 1, 0, 34, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
57, 5, 0, 0, 202, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
64, 5, 0, 0, 3, 0, 1, 0,
|
||||
76, 5, 0, 0, 2, 0, 1, 0,
|
||||
35, 0, 0, 0, 24, 0, 0, 0,
|
||||
0, 0, 1, 0, 35, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
73, 5, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
68, 5, 0, 0, 3, 0, 1, 0,
|
||||
80, 5, 0, 0, 2, 0, 1, 0,
|
||||
97, 99, 99, 101, 108, 101, 114, 97,
|
||||
116, 105, 111, 110, 74, 101, 114, 107,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -2070,6 +2077,15 @@ static const ::capnp::_::AlignedData<597> b_a1680744031fdb2d = {
|
||||
5, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
100, 105, 115, 97, 98, 108, 101, 84,
|
||||
104, 114, 111, 116, 116, 108, 101, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
101, 120, 112, 101, 114, 105, 109, 101,
|
||||
110, 116, 97, 108, 77, 111, 100, 101,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -2343,11 +2359,11 @@ static const ::capnp::_::AlignedData<597> b_a1680744031fdb2d = {
|
||||
static const ::capnp::_::RawSchema* const d_a1680744031fdb2d[] = {
|
||||
&s_81c2f05a394cf4af,
|
||||
};
|
||||
static const uint16_t m_a1680744031fdb2d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 13, 14, 12, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34};
|
||||
static const uint16_t i_a1680744031fdb2d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34};
|
||||
static const uint16_t m_a1680744031fdb2d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 14, 15, 13, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35};
|
||||
static const uint16_t i_a1680744031fdb2d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35};
|
||||
const ::capnp::_::RawSchema s_a1680744031fdb2d = {
|
||||
0xa1680744031fdb2d, b_a1680744031fdb2d.words, 597, d_a1680744031fdb2d, m_a1680744031fdb2d,
|
||||
1, 35, i_a1680744031fdb2d, nullptr, nullptr, { &s_a1680744031fdb2d, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
0xa1680744031fdb2d, b_a1680744031fdb2d.words, 613, d_a1680744031fdb2d, m_a1680744031fdb2d,
|
||||
1, 36, i_a1680744031fdb2d, nullptr, nullptr, { &s_a1680744031fdb2d, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<55> b_cb9fd56c7057593a = {
|
||||
|
||||
@@ -1557,6 +1557,8 @@ public:
|
||||
|
||||
inline ::int64_t getDesiredFollowDistance() const;
|
||||
|
||||
inline bool getDisableThrottle() const;
|
||||
|
||||
inline bool getExperimentalMode() const;
|
||||
|
||||
inline bool getForcingStop() const;
|
||||
@@ -1664,6 +1666,9 @@ public:
|
||||
inline ::int64_t getDesiredFollowDistance();
|
||||
inline void setDesiredFollowDistance( ::int64_t value);
|
||||
|
||||
inline bool getDisableThrottle();
|
||||
inline void setDisableThrottle(bool value);
|
||||
|
||||
inline bool getExperimentalMode();
|
||||
inline void setExperimentalMode(bool value);
|
||||
|
||||
@@ -3036,34 +3041,48 @@ inline void FrogPilotPlan::Builder::setDesiredFollowDistance( ::int64_t value) {
|
||||
::capnp::bounded<3>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getExperimentalMode() const {
|
||||
inline bool FrogPilotPlan::Reader::getDisableThrottle() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<66>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getExperimentalMode() {
|
||||
inline bool FrogPilotPlan::Builder::getDisableThrottle() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<66>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setExperimentalMode(bool value) {
|
||||
inline void FrogPilotPlan::Builder::setDisableThrottle(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<66>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getForcingStop() const {
|
||||
inline bool FrogPilotPlan::Reader::getExperimentalMode() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<67>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getForcingStop() {
|
||||
inline bool FrogPilotPlan::Builder::getExperimentalMode() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<67>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setForcingStop(bool value) {
|
||||
inline void FrogPilotPlan::Builder::setExperimentalMode(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<67>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getForcingStop() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getForcingStop() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setForcingStop(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float FrogPilotPlan::Reader::getForcingStopLength() const {
|
||||
return _reader.getDataField<float>(
|
||||
::capnp::bounded<5>() * ::capnp::ELEMENTS);
|
||||
@@ -3128,16 +3147,16 @@ inline void FrogPilotPlan::Builder::setIncreasedStoppedDistance(float value) {
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getLateralCheck() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getLateralCheck() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setLateralCheck(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<68>() * ::capnp::ELEMENTS, value);
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float FrogPilotPlan::Reader::getLaneWidthLeft() const {
|
||||
@@ -3198,16 +3217,16 @@ inline void FrogPilotPlan::Builder::setMinAcceleration(float value) {
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getRedLight() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getRedLight() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setRedLight(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<69>() * ::capnp::ELEMENTS, value);
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float FrogPilotPlan::Reader::getRoadCurvature() const {
|
||||
@@ -3372,16 +3391,16 @@ inline void FrogPilotPlan::Builder::setSpeedJerkStock(float value) {
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getSpeedLimitChanged() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getSpeedLimitChanged() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setSpeedLimitChanged(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<70>() * ::capnp::ELEMENTS, value);
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float FrogPilotPlan::Reader::getTFollow() const {
|
||||
@@ -3400,46 +3419,46 @@ inline void FrogPilotPlan::Builder::setTFollow(float value) {
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getThemeUpdated() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<72>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getThemeUpdated() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS);
|
||||
::capnp::bounded<72>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setThemeUpdated(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<71>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getTogglesUpdated() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<72>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getTogglesUpdated() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<72>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setTogglesUpdated(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<72>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getTrackingLead() const {
|
||||
inline bool FrogPilotPlan::Reader::getTogglesUpdated() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<73>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getTrackingLead() {
|
||||
inline bool FrogPilotPlan::Builder::getTogglesUpdated() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<73>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setTrackingLead(bool value) {
|
||||
inline void FrogPilotPlan::Builder::setTogglesUpdated(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<73>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Reader::getTrackingLead() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<74>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool FrogPilotPlan::Builder::getTrackingLead() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<74>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void FrogPilotPlan::Builder::setTrackingLead(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<74>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float FrogPilotPlan::Reader::getUnconfirmedSlcSpeedLimit() const {
|
||||
return _reader.getDataField<float>(
|
||||
::capnp::bounded<23>() * ::capnp::ELEMENTS);
|
||||
|
||||
Binary file not shown.
@@ -9,7 +9,7 @@ from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, COMFORT_BRAKE, DANGER_ZONE_COST, J_EGO_COST
|
||||
|
||||
from openpilot.frogpilot.common.frogpilot_utilities import calculate_lane_width, calculate_road_curvature
|
||||
from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD, params, params_memory
|
||||
@@ -32,6 +32,7 @@ class FrogPilotPlanner:
|
||||
|
||||
self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL)
|
||||
|
||||
self.disable_throttle = False
|
||||
self.driving_in_curve = False
|
||||
self.lateral_check = False
|
||||
self.model_stopped = False
|
||||
@@ -65,6 +66,11 @@ class FrogPilotPlanner:
|
||||
self.cem.curve_detected = False
|
||||
self.cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
|
||||
|
||||
self.disable_throttle = self.tracking_lead and not self.frogpilot_following.following_lead
|
||||
self.disable_throttle &= self.lead_one.vLead + COMFORT_BRAKE < v_ego > CRUISING_SPEED
|
||||
self.disable_throttle &= sm["controlsState"].enabled
|
||||
|
||||
|
||||
self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
|
||||
|
||||
self.frogpilot_events.update(v_cruise, sm, frogpilot_toggles)
|
||||
@@ -140,6 +146,8 @@ class FrogPilotPlanner:
|
||||
|
||||
frogpilotPlan.desiredFollowDistance = self.frogpilot_following.desired_follow_distance
|
||||
|
||||
frogpilotPlan.disableThrottle = self.disable_throttle
|
||||
|
||||
frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode
|
||||
|
||||
frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop
|
||||
|
||||
@@ -18,6 +18,7 @@ class FrogPilotVCruise:
|
||||
self.override_force_stop = False
|
||||
|
||||
self.override_force_stop_timer = 0
|
||||
self.force_stop_timer = 0.0
|
||||
|
||||
def update(self, gps_position, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles):
|
||||
force_stop = self.frogpilot_planner.cem.stop_light_detected and sm["controlsState"].enabled and frogpilot_toggles.force_stops
|
||||
@@ -58,16 +59,6 @@ class FrogPilotVCruise:
|
||||
|
||||
self.csc_target = v_cruise
|
||||
|
||||
# Mike's extended lead linear braking
|
||||
if self.frogpilot_planner.lead_one.vLead < v_ego > CRUISING_SPEED and sm["controlsState"].enabled and self.frogpilot_planner.tracking_lead and frogpilot_toggles.human_following:
|
||||
if not self.frogpilot_planner.frogpilot_following.following_lead:
|
||||
decel_rate = (v_ego - self.frogpilot_planner.lead_one.vLead)**2 / self.frogpilot_planner.lead_one.dRel
|
||||
self.braking_target = max(v_ego - (decel_rate * DT_MDL), self.frogpilot_planner.lead_one.vLead + CRUISING_SPEED)
|
||||
else:
|
||||
self.braking_target = v_cruise
|
||||
else:
|
||||
self.braking_target = v_cruise
|
||||
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
self.slc.frogpilot_toggles = frogpilot_toggles
|
||||
|
||||
@@ -97,7 +88,7 @@ class FrogPilotVCruise:
|
||||
|
||||
self.tracked_model_length = self.frogpilot_planner.model_length
|
||||
|
||||
targets = [self.braking_target, self.csc_target, v_cruise]
|
||||
targets = [self.csc_target, v_cruise]
|
||||
if frogpilot_toggles.speed_limit_controller:
|
||||
targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff)
|
||||
|
||||
|
||||
@@ -302,6 +302,9 @@ class LongitudinalMpc:
|
||||
self.current_filter_time = LEAD_FILTER_TIME_LOW
|
||||
self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
|
||||
self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
|
||||
# Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps
|
||||
self.filter_time_factor = 1.0
|
||||
self.slew_per_sec = 1.0
|
||||
# Instance variables to avoid global modifications
|
||||
self.current_x_ego_cost = X_EGO_OBSTACLE_COSTS[0]
|
||||
self.current_j_ego_cost = J_EGO_COSTS[0]
|
||||
@@ -357,7 +360,9 @@ class LongitudinalMpc:
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, 'Zl', Zl)
|
||||
|
||||
def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0, uncertainty=0.0):
|
||||
def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True,
|
||||
personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0,
|
||||
uncertainty=0.0, accel_reengage=False, panic_bypass=False):
|
||||
# Update parameters based on current speed with interpolation for smooth scaling
|
||||
speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph
|
||||
|
||||
@@ -390,12 +395,35 @@ class LongitudinalMpc:
|
||||
speed_jerk *= dist_factor
|
||||
|
||||
# Scene complexity adjustment based on model uncertainty
|
||||
complexity_factor = 1.0
|
||||
filter_time_factor = 1.0
|
||||
prev_filter_time_factor = getattr(self, 'prev_filter_time_factor', 1.0)
|
||||
# Target factor from uncertainty
|
||||
if uncertainty <= 0.45:
|
||||
tgt_factor = 1.0
|
||||
elif uncertainty >= 0.70:
|
||||
tgt_factor = 0.0
|
||||
else:
|
||||
tgt_factor = float(np.interp(uncertainty, [0.45, 0.70], [1.0, 0.30]))
|
||||
|
||||
if uncertainty > 1.0: # High uncertainty indicates complex scene
|
||||
complexity_factor = 1.5 # Boost responsiveness
|
||||
filter_time_factor = 0.0 # Disable smoothing for immediate response
|
||||
if accel_reengage:
|
||||
tgt_factor = min(tgt_factor, 0.5)
|
||||
|
||||
# Hard bypass of smoothing when approaching fast or magnitude trips
|
||||
if panic_bypass:
|
||||
tgt_factor = 0.0
|
||||
|
||||
# Slew-limit changes to avoid step-wise filter jumps
|
||||
max_step = self.slew_per_sec * self.dt
|
||||
delta = np.clip(tgt_factor - self.filter_time_factor, -max_step, max_step)
|
||||
self.filter_time_factor += float(delta)
|
||||
filter_time_factor = float(self.filter_time_factor)
|
||||
|
||||
# When uncertainty is moderately elevated, allow accel but cap jerk by increasing jerk cost
|
||||
if 0.45 <= uncertainty < 0.60:
|
||||
scale = float(np.interp(uncertainty, [0.45, 0.60], [1.2, 1.5]))
|
||||
speed_jerk *= scale
|
||||
|
||||
if abs(filter_time_factor - prev_filter_time_factor) > 1e-3:
|
||||
cloudlog.error(f"LON_FILTER; filter_time_factor={filter_time_factor:.2f}; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f} mps; lead_dist={lead_dist:.2f} m; accel_reengage={accel_reengage}")
|
||||
|
||||
if self.mode == 'acc':
|
||||
a_change_cost = acceleration_jerk if prev_accel_constraint else 0
|
||||
@@ -410,7 +438,7 @@ class LongitudinalMpc:
|
||||
self.set_cost_weights(cost_weights, constraint_cost_weights)
|
||||
|
||||
# Adjust filter time constants for complex scenes
|
||||
if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.1:
|
||||
if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05:
|
||||
current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0
|
||||
current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0
|
||||
new_filter_time = self.current_filter_time * filter_time_factor
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import numpy as np
|
||||
import time
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
@@ -23,6 +24,10 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
ALLOW_THROTTLE_THRESHOLD = 0.5
|
||||
MIN_ALLOW_THROTTLE_SPEED = 2.5
|
||||
|
||||
# Uncertainty-based filter disable thresholds
|
||||
UNCERT_SLOPE_TRIG = 0.12 # per second
|
||||
UNCERT_MAG_TRIG = 0.50
|
||||
|
||||
# Lookup table for turns
|
||||
_A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
_A_TOTAL_MAX_BP = [20., 40.]
|
||||
@@ -106,6 +111,30 @@ class LongitudinalPlanner:
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.j_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.solverExecutionTime = 0.0
|
||||
# logging cadence & state
|
||||
self.last_uncert_log_t = 0.0
|
||||
self.prev_uncert_over = False
|
||||
|
||||
# ---- Rubberband mitigation state ----
|
||||
# Two uncertainty tracks (slow/fast) for asymmetric gating
|
||||
self.uncert_slow = FirstOrderFilter(0.0, 1.6, self.dt) # ~lam=0.6
|
||||
self.uncert_fast = FirstOrderFilter(0.0, 0.9, self.dt) # faster cool-down for accel decisions
|
||||
# Lead stability tracking
|
||||
self.prev_lead_dist = None
|
||||
self.last_big_brake_t = 0.0
|
||||
self.stable_lead = False
|
||||
# Temporary accel nudge window
|
||||
self.accel_nudge_until = 0.0
|
||||
|
||||
# Hysteresis gate + dwell for accel re-engage and smoothed lead distance
|
||||
self.accel_gate = False
|
||||
self._t_arm = 0.0
|
||||
self._t_disarm = 0.0
|
||||
self.lead_dist_f = None
|
||||
|
||||
# Uncertainty slope tracking
|
||||
self._uncert_last = 0.0
|
||||
self._uncert_last_t = None
|
||||
|
||||
@property
|
||||
def mlsim(self):
|
||||
@@ -199,6 +228,7 @@ class LongitudinalPlanner:
|
||||
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune)
|
||||
# Don't clip at low speeds since throttle_prob doesn't account for creep
|
||||
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
self.allow_throttle &= not sm['frogpilotPlan'].disableThrottle
|
||||
|
||||
if not self.allow_throttle:
|
||||
clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
|
||||
@@ -216,31 +246,157 @@ class LongitudinalPlanner:
|
||||
|
||||
lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0
|
||||
|
||||
# Smooth lead distance (EMA) to avoid chatter in thresholds
|
||||
alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego))
|
||||
if self.lead_dist_f is None:
|
||||
self.lead_dist_f = float(lead_dist)
|
||||
else:
|
||||
self.lead_dist_f += alpha * (float(lead_dist) - self.lead_dist_f)
|
||||
|
||||
# Lead stability estimation and recent-brake timer
|
||||
now_t = time.monotonic()
|
||||
# relative speed (ego - lead) positive when closing
|
||||
v_rel = (v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0
|
||||
if self.prev_lead_dist is None:
|
||||
d_rel_dot = 0.0
|
||||
else:
|
||||
d_rel_dot = (lead_dist - self.prev_lead_dist) / max(self.dt, 1e-3)
|
||||
self.prev_lead_dist = lead_dist
|
||||
|
||||
# Remember time of last non-trivial model brake risk
|
||||
if 'raw_brake_max' in locals() and raw_brake_max is not None and raw_brake_max > 0.02:
|
||||
self.last_big_brake_t = now_t
|
||||
|
||||
# Stable lead heuristic (short window, cheap to compute)
|
||||
recently_braked = (now_t - self.last_big_brake_t) < 0.7
|
||||
self.stable_lead = (
|
||||
self.lead_one.status and
|
||||
abs(v_rel) < 0.5 and
|
||||
abs(d_rel_dot) < 0.5 and
|
||||
not recently_braked
|
||||
)
|
||||
|
||||
# Calculate scene uncertainty from model desire prediction entropy and disengage predictions
|
||||
uncertainty = 0.0
|
||||
if hasattr(sm['modelV2'], 'meta'):
|
||||
# Desire prediction entropy (maneuver uncertainty)
|
||||
# Desire prediction entropy (maneuver uncertainty), normalized to [0, 1]
|
||||
desire_entropy = 0.0
|
||||
if hasattr(sm['modelV2'].meta, 'desirePrediction'):
|
||||
desire_probs = sm['modelV2'].meta.desirePrediction
|
||||
if len(desire_probs) > 1:
|
||||
desire_probs = np.array(desire_probs)
|
||||
desire_probs = desire_probs / np.sum(desire_probs) # Normalize
|
||||
desire_entropy = -np.sum(desire_probs * np.log(desire_probs + 1e-10))
|
||||
probs = np.asarray(desire_probs, dtype=float)
|
||||
total = float(np.sum(probs))
|
||||
if total > 1e-6:
|
||||
p = probs / total
|
||||
entropy = -np.sum(p * np.log(p + 1e-10))
|
||||
max_entropy = np.log(len(p))
|
||||
desire_entropy = float(entropy / max(max_entropy, 1e-6)) # normalized entropy in [0,1]
|
||||
else:
|
||||
desire_entropy = 0.0 # guard against all-zero vector
|
||||
|
||||
# Disengage prediction risk (intervention likelihood)
|
||||
disengage_risk = 0.0
|
||||
raw_brake_max = -1.0
|
||||
lam = -1.0
|
||||
if hasattr(sm['modelV2'].meta, 'disengagePredictions'):
|
||||
# Use brake press probabilities as primary risk indicator
|
||||
brake_probs = sm['modelV2'].meta.disengagePredictions.brakePressProbs
|
||||
if len(brake_probs) > 0:
|
||||
disengage_risk = np.max(brake_probs) # Peak risk over time horizon
|
||||
# Exponentially decayed max over the full horizon
|
||||
probs = np.asarray(brake_probs, dtype=float)
|
||||
# Clip tiny brake blips so they don't inflate uncertainty
|
||||
if float(np.max(probs)) < 0.015:
|
||||
probs = probs * 0.5
|
||||
raw_brake_max = float(np.max(probs))
|
||||
# Time vector assuming model horizon step = DT_MDL
|
||||
t = np.arange(len(probs), dtype=float) * DT_MDL
|
||||
lam = 0.6 # decay rate per second (tunable: 0.5–0.9 typical)
|
||||
weights = np.exp(-lam * t)
|
||||
disengage_risk = float(np.max(probs * weights))
|
||||
|
||||
# Combined uncertainty metric
|
||||
uncertainty = desire_entropy + disengage_risk
|
||||
# Combined uncertainty metric (range roughly 0..2), with dual-track filtering
|
||||
raw_uncertainty = desire_entropy + disengage_risk
|
||||
# Update filters
|
||||
self.uncert_slow.update(raw_uncertainty)
|
||||
self.uncert_fast.update(raw_uncertainty)
|
||||
# Use a more permissive track for accel decisions
|
||||
uncertainty = self.uncert_slow.x
|
||||
uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x)
|
||||
|
||||
self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint,
|
||||
personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist, uncertainty=uncertainty)
|
||||
# --- Slope-based panic bypass ---
|
||||
if self._uncert_last_t is None:
|
||||
uncert_slope = 0.0
|
||||
else:
|
||||
dt_u = max(1e-3, now_t - self._uncert_last_t)
|
||||
uncert_slope = (uncertainty - self._uncert_last) / dt_u
|
||||
self._uncert_last = uncertainty
|
||||
self._uncert_last_t = now_t
|
||||
|
||||
closing_fast = (self.lead_one.status and (v_ego - self.lead_one.vLead) > 0.5)
|
||||
# Trigger if either slope is high or magnitude is high; require a valid lead and closing
|
||||
panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG)
|
||||
|
||||
if panic_bypass:
|
||||
try:
|
||||
cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# now_t defined earlier
|
||||
over = uncertainty > 1.0
|
||||
# Log on threshold edge or at ~1 Hz
|
||||
if over != self.prev_uncert_over or (now_t - self.last_uncert_log_t) > 1.0:
|
||||
try:
|
||||
cloudlog.error(
|
||||
f"LON_UNCERT; v_ego={v_ego:.2f} mps; desireEntropy={desire_entropy:.3f}; "
|
||||
f"brakeRawMax={(raw_brake_max if 'raw_brake_max' in locals() else -1.0):.3f}; "
|
||||
f"brakeDecayed={(disengage_risk if 'disengage_risk' in locals() else -1.0):.3f}; "
|
||||
f"lam={(lam if 'lam' in locals() else -1.0):.2f}; uncertainty={uncertainty:.3f}; over={over}"
|
||||
)
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"LON_UNCERT log error: {e}")
|
||||
self.prev_uncert_over = over
|
||||
self.last_uncert_log_t = now_t
|
||||
|
||||
# Asymmetric accel release with hysteresis + dwell to prevent on/off pulsing
|
||||
rise_dwell_s, fall_dwell_s = 0.6, 0.4
|
||||
good = (
|
||||
(self.a_desired > 0.0) and
|
||||
self.stable_lead and
|
||||
(uncertainty <= 0.425) and
|
||||
(desire_entropy < 0.41)
|
||||
)
|
||||
|
||||
# dwell timers for robust gating
|
||||
if good and not self.accel_gate:
|
||||
if now_t - self._t_arm >= rise_dwell_s:
|
||||
self.accel_gate = True
|
||||
else:
|
||||
self._t_arm = now_t
|
||||
|
||||
if (not good) and self.accel_gate:
|
||||
if now_t - self._t_disarm >= fall_dwell_s:
|
||||
self.accel_gate = False
|
||||
else:
|
||||
self._t_disarm = now_t
|
||||
|
||||
if self.accel_gate:
|
||||
# Ensure some positive headroom for MPC to exit coasting
|
||||
accel_limits_turns[1] = max(accel_limits_turns[1], 0.2)
|
||||
# Short self-canceling nudge to unstick (applied post-MPC)
|
||||
if now_t > self.accel_nudge_until:
|
||||
self.accel_nudge_until = now_t + 0.45
|
||||
|
||||
self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk,
|
||||
sm['frogpilotPlan'].dangerJerk,
|
||||
sm['frogpilotPlan'].speedJerk,
|
||||
prev_accel_constraint,
|
||||
personality=sm['controlsState'].personality,
|
||||
v_ego=v_ego,
|
||||
lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist,
|
||||
uncertainty=uncertainty,
|
||||
accel_reengage=self.accel_gate,
|
||||
panic_bypass=panic_bypass)
|
||||
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
# After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim
|
||||
@@ -273,6 +429,53 @@ class LongitudinalPlanner:
|
||||
self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
# Anticipatory pre-brake to avoid "coming in hot" when closing on a lead
|
||||
if self.lead_one.status:
|
||||
rel_v = max(0.0, v_ego - self.lead_one.vLead)
|
||||
# dynamic time headway adds a small buffer when uncertainty is elevated
|
||||
base_th = 1.6
|
||||
th = base_th + 0.6 * max(0.0, uncertainty - 0.42)
|
||||
desired_gap = th * v_ego
|
||||
if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5):
|
||||
k_rel, k_unc = 0.04, 0.20
|
||||
pre_brake = k_rel * rel_v + k_unc * max(0.0, uncertainty - 0.42)
|
||||
pre_brake = min(pre_brake, 0.06)
|
||||
self.a_desired = float(self.a_desired - pre_brake)
|
||||
|
||||
# Apply tiny feed-forward nudge when released and safe
|
||||
if now_t < self.accel_nudge_until and self.a_desired > -0.1:
|
||||
self.a_desired = float(min(self.a_desired + 0.12, get_max_accel(v_ego)))
|
||||
|
||||
# --- Gap clamp (minimize rubber-banding when we drift too far behind) ---
|
||||
# If we are following a valid lead and our actual gap exceeds the desired by +0.2s,
|
||||
# add a small accel bias to gently close the gap. This only applies when we're not braking.
|
||||
if self.lead_one.status and v_ego > 1.0 and self.a_desired > -0.1:
|
||||
# Desired headway from current setting
|
||||
t_follow_goal = float(sm['frogpilotPlan'].tFollow)
|
||||
desired_gap_m = t_follow_goal * v_ego
|
||||
# Use filtered lead distance if available
|
||||
actual_gap_m = float(self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel)
|
||||
# Allow +0.2s slack before biasing acceleration
|
||||
slack_m = 0.2 * v_ego
|
||||
gap_err_m = actual_gap_m - (desired_gap_m + slack_m)
|
||||
|
||||
if gap_err_m > 0.0:
|
||||
# Convert extra meters into a small accel bias.
|
||||
# k_gap chosen conservatively to avoid dramatic behavior.
|
||||
k_gap = 0.005 # (m/s^2) per meter of extra gap
|
||||
accel_bias = k_gap * gap_err_m
|
||||
# Cap the bias tightly so it only trims long gaps, not cause surging
|
||||
accel_bias = float(clip(accel_bias, 0.0, 0.18))
|
||||
self.a_desired = float(min(self.a_desired + accel_bias, get_max_accel(v_ego)))
|
||||
try:
|
||||
cloudlog.error(f"LON_GAPCLAMP; v_ego={v_ego:.2f}; desired_gap={desired_gap_m:.2f}; actual_gap={actual_gap_m:.2f}; bias={accel_bias:.3f}")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Small deadzone around zero accel to kill micro-dithers
|
||||
if -0.05 < self.a_desired < 0.05:
|
||||
self.a_desired = 0.0
|
||||
|
||||
def publish(self, classic_model, tinygrad_model, sm, pm, frogpilot_toggles):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
|
||||
Reference in New Issue
Block a user