diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 44409b9c5..029709a26 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -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 { diff --git a/cereal/gen/cpp/custom.capnp.c++ b/cereal/gen/cpp/custom.capnp.c++ index 0c5a6201c..5d751d44b 100644 --- a/cereal/gen/cpp/custom.capnp.c++ +++ b/cereal/gen/cpp/custom.capnp.c++ @@ -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 = { diff --git a/cereal/gen/cpp/custom.capnp.h b/cereal/gen/cpp/custom.capnp.h index 4feabc907..043621b64 100644 --- a/cereal/gen/cpp/custom.capnp.h +++ b/cereal/gen/cpp/custom.capnp.h @@ -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( ::capnp::bounded<66>() * ::capnp::ELEMENTS); } -inline bool FrogPilotPlan::Builder::getExperimentalMode() { +inline bool FrogPilotPlan::Builder::getDisableThrottle() { return _builder.getDataField( ::capnp::bounded<66>() * ::capnp::ELEMENTS); } -inline void FrogPilotPlan::Builder::setExperimentalMode(bool value) { +inline void FrogPilotPlan::Builder::setDisableThrottle(bool value) { _builder.setDataField( ::capnp::bounded<66>() * ::capnp::ELEMENTS, value); } -inline bool FrogPilotPlan::Reader::getForcingStop() const { +inline bool FrogPilotPlan::Reader::getExperimentalMode() const { return _reader.getDataField( ::capnp::bounded<67>() * ::capnp::ELEMENTS); } -inline bool FrogPilotPlan::Builder::getForcingStop() { +inline bool FrogPilotPlan::Builder::getExperimentalMode() { return _builder.getDataField( ::capnp::bounded<67>() * ::capnp::ELEMENTS); } -inline void FrogPilotPlan::Builder::setForcingStop(bool value) { +inline void FrogPilotPlan::Builder::setExperimentalMode(bool value) { _builder.setDataField( ::capnp::bounded<67>() * ::capnp::ELEMENTS, value); } +inline bool FrogPilotPlan::Reader::getForcingStop() const { + return _reader.getDataField( + ::capnp::bounded<68>() * ::capnp::ELEMENTS); +} + +inline bool FrogPilotPlan::Builder::getForcingStop() { + return _builder.getDataField( + ::capnp::bounded<68>() * ::capnp::ELEMENTS); +} +inline void FrogPilotPlan::Builder::setForcingStop(bool value) { + _builder.setDataField( + ::capnp::bounded<68>() * ::capnp::ELEMENTS, value); +} + inline float FrogPilotPlan::Reader::getForcingStopLength() const { return _reader.getDataField( ::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( - ::capnp::bounded<68>() * ::capnp::ELEMENTS); + ::capnp::bounded<69>() * ::capnp::ELEMENTS); } inline bool FrogPilotPlan::Builder::getLateralCheck() { return _builder.getDataField( - ::capnp::bounded<68>() * ::capnp::ELEMENTS); + ::capnp::bounded<69>() * ::capnp::ELEMENTS); } inline void FrogPilotPlan::Builder::setLateralCheck(bool value) { _builder.setDataField( - ::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( - ::capnp::bounded<69>() * ::capnp::ELEMENTS); + ::capnp::bounded<70>() * ::capnp::ELEMENTS); } inline bool FrogPilotPlan::Builder::getRedLight() { return _builder.getDataField( - ::capnp::bounded<69>() * ::capnp::ELEMENTS); + ::capnp::bounded<70>() * ::capnp::ELEMENTS); } inline void FrogPilotPlan::Builder::setRedLight(bool value) { _builder.setDataField( - ::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( - ::capnp::bounded<70>() * ::capnp::ELEMENTS); + ::capnp::bounded<71>() * ::capnp::ELEMENTS); } inline bool FrogPilotPlan::Builder::getSpeedLimitChanged() { return _builder.getDataField( - ::capnp::bounded<70>() * ::capnp::ELEMENTS); + ::capnp::bounded<71>() * ::capnp::ELEMENTS); } inline void FrogPilotPlan::Builder::setSpeedLimitChanged(bool value) { _builder.setDataField( - ::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( - ::capnp::bounded<71>() * ::capnp::ELEMENTS); + ::capnp::bounded<72>() * ::capnp::ELEMENTS); } inline bool FrogPilotPlan::Builder::getThemeUpdated() { return _builder.getDataField( - ::capnp::bounded<71>() * ::capnp::ELEMENTS); + ::capnp::bounded<72>() * ::capnp::ELEMENTS); } inline void FrogPilotPlan::Builder::setThemeUpdated(bool value) { - _builder.setDataField( - ::capnp::bounded<71>() * ::capnp::ELEMENTS, value); -} - -inline bool FrogPilotPlan::Reader::getTogglesUpdated() const { - return _reader.getDataField( - ::capnp::bounded<72>() * ::capnp::ELEMENTS); -} - -inline bool FrogPilotPlan::Builder::getTogglesUpdated() { - return _builder.getDataField( - ::capnp::bounded<72>() * ::capnp::ELEMENTS); -} -inline void FrogPilotPlan::Builder::setTogglesUpdated(bool value) { _builder.setDataField( ::capnp::bounded<72>() * ::capnp::ELEMENTS, value); } -inline bool FrogPilotPlan::Reader::getTrackingLead() const { +inline bool FrogPilotPlan::Reader::getTogglesUpdated() const { return _reader.getDataField( ::capnp::bounded<73>() * ::capnp::ELEMENTS); } -inline bool FrogPilotPlan::Builder::getTrackingLead() { +inline bool FrogPilotPlan::Builder::getTogglesUpdated() { return _builder.getDataField( ::capnp::bounded<73>() * ::capnp::ELEMENTS); } -inline void FrogPilotPlan::Builder::setTrackingLead(bool value) { +inline void FrogPilotPlan::Builder::setTogglesUpdated(bool value) { _builder.setDataField( ::capnp::bounded<73>() * ::capnp::ELEMENTS, value); } +inline bool FrogPilotPlan::Reader::getTrackingLead() const { + return _reader.getDataField( + ::capnp::bounded<74>() * ::capnp::ELEMENTS); +} + +inline bool FrogPilotPlan::Builder::getTrackingLead() { + return _builder.getDataField( + ::capnp::bounded<74>() * ::capnp::ELEMENTS); +} +inline void FrogPilotPlan::Builder::setTrackingLead(bool value) { + _builder.setDataField( + ::capnp::bounded<74>() * ::capnp::ELEMENTS, value); +} + inline float FrogPilotPlan::Reader::getUnconfirmedSlcSpeedLimit() const { return _reader.getDataField( ::capnp::bounded<23>() * ::capnp::ELEMENTS); diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index 52e9984af..a3be5241e 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 99e66778b..d0b854778 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -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 diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index c2c1b6eff..35aae36b5 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 20db0f0f8..419a53c1c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cf161471c..78266927d 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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')