mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-01 08:33:44 +08:00
Compare commits
311 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| cfb38312db | |||
| 6fd4278e02 | |||
| 451cc3445a | |||
| 1daa2d9081 | |||
| 7a9099818f | |||
| ef4fa48f33 | |||
| e426ced4c0 | |||
| 7681ba763b | |||
| b04d666a68 | |||
| fb026b86a3 | |||
| cff4c5e589 | |||
| e0cca951f3 | |||
| 2439c048ed | |||
| 7b54d7d7a0 | |||
| b2bdb5787f | |||
| 282f517b78 | |||
| 1779144ff9 | |||
| 54fb2750ba | |||
| 14431ab77f | |||
| 9d45db41b3 | |||
| b57c593d92 | |||
| 0674c42866 | |||
| 1f7bcf246a | |||
| b0512dc523 | |||
| cd88fd3850 | |||
| ff78eaeba1 | |||
| 92c8aeeb92 | |||
| ecaa20e7be | |||
| 859c98c9d8 | |||
| 69cbe8c6ed | |||
| 1aafe92fc0 | |||
| adb6b9fb12 | |||
| f67a9f4624 | |||
| 094e834ac4 | |||
| e4067060b9 | |||
| 86a14640b5 | |||
| 59fb84f0d5 | |||
| 78de201ce7 | |||
| 17e49f69cc | |||
| bec75ebdd8 | |||
| 473832efa7 | |||
| 2cceeb3552 | |||
| 923a47f713 | |||
| 790a762a05 | |||
| 221c219fca | |||
| 8b60649eed | |||
| ded0b506d6 | |||
| b51d9af9a0 | |||
| 4460ce8166 | |||
| f1aa0c7f78 | |||
| e7c8126fd9 | |||
| 125999c364 | |||
| 5b25ea7f99 | |||
| b7e2631286 | |||
| a838871189 | |||
| 1827331599 | |||
| 5c777bbe01 | |||
| fb97f993d1 | |||
| 94b67077e3 | |||
| 14f17699b9 | |||
| 80e27d5cbb | |||
| 039dbcd877 | |||
| 8c134ae555 | |||
| 276c7a2b34 | |||
| 0e2dbcebfa | |||
| bbb7760a95 | |||
| 23a27b2fbf | |||
| 8b78107a40 | |||
| c27b6007de | |||
| 36c2dce247 | |||
| 8035039731 | |||
| 8aa6c9440f | |||
| 34ef40fd81 | |||
| 4d044d7618 | |||
| 6247e3dc84 | |||
| b1131289b7 | |||
| 35dc7d661e | |||
| c39b2dad94 | |||
| e6b769245c | |||
| 8b94f8b2f8 | |||
| dec014cd17 | |||
| 7b40272866 | |||
| 506456e7f0 | |||
| 4ea4b9d177 | |||
| 0e2313dc31 | |||
| 79ea7db103 | |||
| f9ae9192fa | |||
| 7ec23006c6 | |||
| 2be9447a6c | |||
| cfd926778e | |||
| a97a67e3d0 | |||
| 518b6de08d | |||
| 6933e3bcdb | |||
| 05e0ca8bee | |||
| 410614fcf3 | |||
| e2bc0996ef | |||
| 1be0c20cf5 | |||
| 839143b9ed | |||
| bce86637ae | |||
| b833d3ee89 | |||
| e0441dfb4b | |||
| 62ec40bba6 | |||
| 15c6d38028 | |||
| 56eb9f555c | |||
| 2f9951df02 | |||
| 6030bf4da3 | |||
| 074694d660 | |||
| df35f48f3b | |||
| 4fb9704540 | |||
| 48cbe266fc | |||
| 21aa7ff367 | |||
| 7caf05dd51 | |||
| 2d779f5db9 | |||
| 18208f1da0 | |||
| 5a3c6ddf57 | |||
| eaa8732ab0 | |||
| 6f0284c84f | |||
| 999ea03f23 | |||
| 0975db3ff1 | |||
| 8d70a8b80a | |||
| da93f92887 | |||
| 9117f6c071 | |||
| 3567ff9691 | |||
| f44ae2ced9 | |||
| 376e0ca615 | |||
| 32b7686468 | |||
| b9e0f52ea9 | |||
| a3163b680f | |||
| 8a927d808f | |||
| bc7d5e474d | |||
| 36f192b5fe | |||
| ffd5cd4ac2 | |||
| e4a00fcd6c | |||
| 0bdcb41103 | |||
| 78051085ca | |||
| 4910d5809a | |||
| 8dd862ff28 | |||
| 1b57497da9 | |||
| 8d8d1ffc7a | |||
| c5919d5495 | |||
| da71951c95 | |||
| 25a152cd8b | |||
| 9077a1082a | |||
| 7ae7000254 | |||
| 7029455706 | |||
| 3a71a62215 | |||
| 00622e8c33 | |||
| 549da3ee92 | |||
| e038a65ef8 | |||
| 8d0513c657 | |||
| 2cea48f4cd | |||
| 8855a9ab65 | |||
| 32321c01cc | |||
| 52cd65fefb | |||
| 1fcdeccd40 | |||
| 3517c36978 | |||
| fb30c3c1e8 | |||
| 1c25e568d5 | |||
| f172122b7c | |||
| 67d6cdc7cd | |||
| 6724085cfd | |||
| 6774f34eee | |||
| 6d0402896d | |||
| 344021a3d9 | |||
| a9b85ab27d | |||
| 17204a46e4 | |||
| 7c4d415462 | |||
| b8985b6d72 | |||
| c669473f88 | |||
| 8751435bf5 | |||
| 30ae210761 | |||
| 3eb693f58b | |||
| 7b4a31c5ac | |||
| 1527c8cf88 | |||
| ab0a7ae666 | |||
| 11ec2f1f21 | |||
| 2c6808d37e | |||
| 2e96382c49 | |||
| 0239e440ca | |||
| 76b972daff | |||
| bc3ef3e7dd | |||
| 9839291dd0 | |||
| 17cba328d6 | |||
| 86093765d8 | |||
| 05fa1c8ae8 | |||
| e8a40d6b85 | |||
| c265e0bb85 | |||
| c6b118788b | |||
| b004f6dbdc | |||
| cd4930b680 | |||
| f861aca628 | |||
| 1ff0d8e2ee | |||
| d76d70764e | |||
| dc73ce0b71 | |||
| 3f666748af | |||
| 8de8a8838c | |||
| f563b7eb71 | |||
| 3c18b83708 | |||
| bd35f5904b | |||
| 474f2737f6 | |||
| ea1ac4a212 | |||
| 165d7c7b36 | |||
| b53cbb2e18 | |||
| b95f8c5929 | |||
| e564bb0b85 | |||
| e2ec8a7b13 | |||
| 75c6f0f10e | |||
| af38044b42 | |||
| 684fa846d8 | |||
| 416e722855 | |||
| 9fd4613bbb | |||
| a0362e3c5f | |||
| e32ef1cdc0 | |||
| 20673ec8a6 | |||
| 53fbdf7329 | |||
| 6f72b74fac | |||
| e83705a32e | |||
| 95d36b9ba2 | |||
| b9e74254bd | |||
| b4405b200d | |||
| a8a3fdac54 | |||
| 4eddc622a7 | |||
| 4e42ada240 | |||
| 6266217655 | |||
| ea09d32e98 | |||
| 15958c88d3 | |||
| b80d7fb5ea | |||
| 1c3d25c6ff | |||
| c9f22b32c7 | |||
| c0524985bb | |||
| 83839c7ea7 | |||
| c1a1d4b4c3 | |||
| b5af7a905a | |||
| 96b1b2f55f | |||
| 9361ba5d70 | |||
| 4e9014311e | |||
| 232873fc70 | |||
| 0a61fca9c9 | |||
| 480bdc34dc | |||
| 716b475a13 | |||
| b1ec5ec034 | |||
| 470613c2b7 | |||
| 336c5b4154 | |||
| abdb9dc750 | |||
| ab98683973 | |||
| 186c24dbe6 | |||
| 9cdf6340a1 | |||
| 2855b1341c | |||
| cf28f99976 | |||
| a39d67dc47 | |||
| 7e75257f12 | |||
| df38449553 | |||
| 1385ef3bc5 | |||
| 7c23c11c51 | |||
| aeff2e12ec | |||
| 7274899671 | |||
| 6c00fd608f | |||
| df6a034c11 | |||
| acb109c290 | |||
| b227b00249 | |||
| 3ec9d6c18a | |||
| 86db8b95f0 | |||
| 4cfff8a35f | |||
| 962fedf48c | |||
| 04494414d1 | |||
| 0b83576e9b | |||
| ce4ef0f817 | |||
| f0b15c1c56 | |||
| f898e9fdfe | |||
| 8ee7804b0e | |||
| 923228194e | |||
| 8837b2e3f6 | |||
| f48c9dc1c2 | |||
| 74aa07a8cd | |||
| 5236e4860f | |||
| 3d174da1c3 | |||
| 8faa40f3a3 | |||
| 3e03275f28 | |||
| 7595cf8a25 | |||
| 2675d43adb | |||
| d9f4ce82e6 | |||
| 648a1845d8 | |||
| a87eff6d1c | |||
| dd6ad37e23 | |||
| c5e778b939 | |||
| a9ab81a77a | |||
| 2d40e1d8e5 | |||
| 7c8f367a5d | |||
| ff4cf558aa | |||
| 0e151e51bc | |||
| 60cc0031b0 | |||
| d24cac0998 | |||
| 45d110830c | |||
| ea3a9ae911 | |||
| 6bff8c0e7c | |||
| bc6b8802b8 | |||
| 252ef572d3 | |||
| 414d397e3f | |||
| 11b7b3789d | |||
| 871ac53717 | |||
| 64ea66b6e6 | |||
| 6d7c6759b3 | |||
| 4cea013570 | |||
| eb375c0587 | |||
| 7fd8a5a4bd | |||
| b3c90216bb | |||
| 10f345f956 | |||
| 956d2c36d0 | |||
| 55e688b6f2 | |||
| f017954027 | |||
| 7e992d11b1 |
@@ -2,5 +2,6 @@ Wen
|
||||
REGIST
|
||||
PullRequest
|
||||
cancelled
|
||||
indeces
|
||||
FOF
|
||||
NoO
|
||||
|
||||
@@ -9,7 +9,6 @@
|
||||
*.ttf filter=lfs diff=lfs merge=lfs -text
|
||||
*.otf filter=lfs diff=lfs merge=lfs -text
|
||||
*.wav filter=lfs diff=lfs merge=lfs -text
|
||||
openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
|
||||
|
||||
openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
|
||||
openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text
|
||||
|
||||
Generated
+7
@@ -21,5 +21,12 @@
|
||||
</clean>
|
||||
</configuration>
|
||||
</target>
|
||||
<target id="f2590b2b-9b93-49f9-8510-da3f3724a2ae" name="replay" defaultType="TOOL">
|
||||
<configuration id="d475264f-6f4c-4092-9b4e-6773309f38b7" name="replay" toolchainName="Default">
|
||||
<build type="TOOL">
|
||||
<tool actionId="Tool_External Tools_uv build tools replay" />
|
||||
</build>
|
||||
</configuration>
|
||||
</target>
|
||||
</component>
|
||||
</project>
|
||||
Generated
+7
@@ -20,4 +20,11 @@
|
||||
<option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
|
||||
</exec>
|
||||
</tool>
|
||||
<tool name="uv build tools replay" showInMainMenu="false" showInEditor="false" showInProject="false" showInSearchPopup="false" disabled="false" useConsole="true" showConsoleOnStdOut="false" showConsoleOnStdErr="false" synchronizeAfterRun="true">
|
||||
<exec>
|
||||
<option name="COMMAND" value="bash" />
|
||||
<option name="PARAMETERS" value="-c "source .venv/bin/activate && scons -u -j$(nproc) tools/replay/"" />
|
||||
<option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
|
||||
</exec>
|
||||
</tool>
|
||||
</toolSet>
|
||||
@@ -1,5 +1,5 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="sunnypilot" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
|
||||
<configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
|
||||
<envs>
|
||||
<env name="QT_DBL_CLICK_DIST" value="150" />
|
||||
</envs>
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Debug Route Controls" type="PythonConfigurationType" factoryName="Python">
|
||||
<module name="openpilot-special" />
|
||||
<option name="ENV_FILES" value="" />
|
||||
<option name="INTERPRETER_OPTIONS" value="" />
|
||||
<option name="PARENT_ENVS" value="true" />
|
||||
<envs>
|
||||
<env name="PYTHONUNBUFFERED" value="1" />
|
||||
<env name="FINGERPRINT" value="KIA_EV9" />
|
||||
<env name="SKIP_FW_QUERY" value="1" />
|
||||
</envs>
|
||||
<option name="SDK_HOME" value="" />
|
||||
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/selfdrive/car" />
|
||||
<option name="IS_MODULE_SDK" value="true" />
|
||||
<option name="ADD_CONTENT_ROOTS" value="true" />
|
||||
<option name="ADD_SOURCE_ROOTS" value="true" />
|
||||
<EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
|
||||
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/selfdrive/car/card.py" />
|
||||
<option name="PARAMETERS" value="" />
|
||||
<option name="SHOW_COMMAND_LINE" value="false" />
|
||||
<option name="EMULATE_TERMINAL" value="true" />
|
||||
<option name="MODULE_MODE" value="false" />
|
||||
<option name="REDIRECT_INPUT" value="false" />
|
||||
<option name="INPUT_FILE" value="" />
|
||||
<method v="2" />
|
||||
</configuration>
|
||||
</component>
|
||||
@@ -0,0 +1,7 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Replay for controls + ui" type="Multirun" separateTabs="false" reuseTabsWithFailures="false" startOneByOne="true" markFailedProcess="true" hideSuccessProcess="false" delayTime="0.0">
|
||||
<runConfiguration name="replay for controls" type="Native Application" />
|
||||
<runConfiguration name="Build Debug" type="Custom Build Application" />
|
||||
<method v="2" />
|
||||
</configuration>
|
||||
</component>
|
||||
@@ -0,0 +1,7 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="replay for controls" type="CLionNativeAppRunConfigurationType" focusToolWindowBeforeRun="true" PROGRAM_PARAMS=""$Prompt$" --block "sendcan,carState,carParams,carOutput,liveTracks,carParamsSP,carStateSP,bookmarkButton"" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="true" WORKING_DIR="file://$ProjectFileDir$/tools/replay" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="replay" CONFIG_NAME="replay" version="1" RUN_PATH="replay">
|
||||
<method v="2">
|
||||
<option name="CLION.COMPOUND.BUILD" enabled="true" />
|
||||
</method>
|
||||
</configuration>
|
||||
</component>
|
||||
+1
-1
Submodule opendbc_repo updated: f6b7f24174...cc4b08625a
@@ -382,7 +382,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
|
||||
leadOne @2 :LeadData;
|
||||
leadTwo @3 :LeadData;
|
||||
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
|
||||
fordLateralPath @5 :FordLateralPath;
|
||||
|
||||
struct Param {
|
||||
key @0 :Text;
|
||||
@@ -403,14 +402,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
|
||||
}
|
||||
}
|
||||
|
||||
struct FordLateralPath {
|
||||
pathOffset @0 :Float32; # c0 [m]
|
||||
pathAngle @1 :Float32; # c1 [rad]
|
||||
curvature @2 :Float32; # c2 [1/m]
|
||||
curvatureRate @3 :Float32; # c3 [1/m^2]
|
||||
valid @4 :Bool;
|
||||
}
|
||||
|
||||
struct BackupManagerSP @0xf98d843bfd7004a3 {
|
||||
backupStatus @0 :Status;
|
||||
restoreStatus @1 :Status;
|
||||
@@ -478,30 +469,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||
}
|
||||
}
|
||||
|
||||
struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
|
||||
enabled @0 :Bool;
|
||||
madsDistanceMeters @1 :Float64;
|
||||
fullAssistDistanceMeters @2 :Float64;
|
||||
event @3 :Event;
|
||||
|
||||
struct Event {
|
||||
id @0 :UInt64;
|
||||
category @1 :Category;
|
||||
distanceMeters @2 :Float64;
|
||||
previousDistanceMeters @3 :Float64;
|
||||
unit @4 :Unit;
|
||||
}
|
||||
|
||||
enum Category {
|
||||
none @0;
|
||||
mads @1;
|
||||
fullAssist @2;
|
||||
}
|
||||
|
||||
enum Unit {
|
||||
imperial @0;
|
||||
metric @1;
|
||||
}
|
||||
struct CustomReserved10 @0xcb9fd56c7057593a {
|
||||
}
|
||||
|
||||
struct CustomReserved11 @0xc2243c65e0340384 {
|
||||
|
||||
@@ -2640,7 +2640,7 @@ struct Event {
|
||||
carStateSP @114 :Custom.CarStateSP;
|
||||
liveMapDataSP @115 :Custom.LiveMapDataSP;
|
||||
modelDataV2SP @116 :Custom.ModelDataV2SP;
|
||||
assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
|
||||
customReserved10 @136 :Custom.CustomReserved10;
|
||||
customReserved11 @137 :Custom.CustomReserved11;
|
||||
customReserved12 @138 :Custom.CustomReserved12;
|
||||
customReserved13 @139 :Custom.CustomReserved13;
|
||||
|
||||
@@ -90,7 +90,6 @@ _services: dict[str, tuple] = {
|
||||
"carParamsSP": (True, 0.02, 1),
|
||||
"carControlSP": (True, 100., 10),
|
||||
"carStateSP": (True, 100., 10),
|
||||
"assistedDrivingMilestoneState": (True, 10., 1),
|
||||
"liveMapDataSP": (True, 1., 1),
|
||||
"modelDataV2SP": (True, 20., None, QueueSize.BIG),
|
||||
"liveLocationKalman": (True, 20.),
|
||||
|
||||
@@ -97,10 +97,6 @@ Params::Params(const std::string &path) {
|
||||
}
|
||||
|
||||
Params::~Params() {
|
||||
flushNonBlockingWrites();
|
||||
}
|
||||
|
||||
void Params::flushNonBlockingWrites() {
|
||||
if (future.valid()) {
|
||||
future.wait();
|
||||
}
|
||||
|
||||
@@ -75,7 +75,6 @@ public:
|
||||
return put(key.c_str(), val ? "1" : "0", 1);
|
||||
}
|
||||
void putNonBlocking(const std::string &key, const std::string &val);
|
||||
void flushNonBlockingWrites();
|
||||
inline void putBoolNonBlocking(const std::string &key, bool val) {
|
||||
putNonBlocking(key, val ? "1" : "0");
|
||||
}
|
||||
|
||||
@@ -73,7 +73,6 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
|
||||
params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
|
||||
params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
|
||||
params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
|
||||
params_flush = _bind("params_flush", [ParamsHandle])
|
||||
params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
|
||||
params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
|
||||
params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
|
||||
@@ -179,10 +178,6 @@ class Params:
|
||||
def put_bool(self, key, val, block=False):
|
||||
params_put_bool(self.p, self.check_key(key), val, block)
|
||||
|
||||
def flush(self):
|
||||
"""Wait for all prior nonblocking writes from this Params instance."""
|
||||
params_flush(self.p)
|
||||
|
||||
def remove(self, key):
|
||||
params_remove(self.p, self.check_key(key))
|
||||
|
||||
|
||||
@@ -133,12 +133,6 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
|
||||
});
|
||||
}
|
||||
|
||||
void params_flush(ParamsHandle *handle) noexcept {
|
||||
translate_exceptions([&]() {
|
||||
handle->params.flushNonBlockingWrites();
|
||||
});
|
||||
}
|
||||
|
||||
int params_remove(ParamsHandle *handle, const char *key) noexcept {
|
||||
return translate_exceptions(-1, [&]() {
|
||||
return handle->params.remove(key);
|
||||
|
||||
@@ -136,8 +136,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||
{"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
|
||||
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}}, // seconds
|
||||
@@ -158,7 +156,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
|
||||
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
|
||||
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
|
||||
@@ -168,9 +165,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
|
||||
{"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
|
||||
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
|
||||
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
|
||||
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
|
||||
{"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
|
||||
@@ -106,13 +106,6 @@ class TestParams(OpenpilotTestCase):
|
||||
assert q.get("CarParams") is None
|
||||
assert q.get("CarParams", True) == b"1"
|
||||
|
||||
def test_flush_non_blocking_writes(self):
|
||||
self.params.put("DongleId", "first")
|
||||
self.params.put("DongleId", "last")
|
||||
self.params.flush()
|
||||
|
||||
assert self.params.get("DongleId") == "last"
|
||||
|
||||
def test_params_all_keys(self):
|
||||
keys = Params().all_keys()
|
||||
|
||||
|
||||
@@ -37,6 +37,8 @@ SP_BRANCH_MIGRATIONS = {
|
||||
("tizi", "release3-staging"): "release-tizi-staging",
|
||||
("mici", "release3"): "release-mici",
|
||||
("mici", "release3-staging"): "release-mici-staging",
|
||||
("tici", "hkg-angle-steering-2025"): "hkg-angle-steering-2025-tici",
|
||||
("tici", "hkg-angle-steering-2025-prebuilt"): "hkg-angle-steering-2025-tici-prebuilt"
|
||||
}
|
||||
|
||||
BUILD_METADATA_FILENAME = "build.json"
|
||||
|
||||
Binary file not shown.
@@ -63,6 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
|
||||
struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
|
||||
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
|
||||
)
|
||||
struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
|
||||
|
||||
return struct_dataclass
|
||||
|
||||
@@ -13,7 +13,6 @@ from openpilot.common.swaglog import cloudlog
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
@@ -53,8 +52,6 @@ class Controls(ControlsExt):
|
||||
self.steer_limited_by_safety = False
|
||||
self.curvature = 0.0
|
||||
self.desired_curvature = 0.0
|
||||
self.ford_path_controller = FordPathController()
|
||||
self.ford_path = FordPath()
|
||||
|
||||
self.pose_calibrator = PoseCalibrator()
|
||||
self.calibrated_pose: Pose | None = None
|
||||
@@ -158,11 +155,6 @@ class Controls(ControlsExt):
|
||||
actuators.curvature = float(lateral_output)
|
||||
else:
|
||||
actuators.steeringAngleDeg = float(lateral_output)
|
||||
if self.CP.brand == "ford":
|
||||
self.ford_path = self.ford_path_controller.update(model_v2 if self.sm.valid['modelV2'] else None,
|
||||
self.desired_curvature, v_ego=CS.vEgo, active=CC.latActive,
|
||||
current_curvature=self.curvature)
|
||||
actuators.curvature = float(self.ford_path.curvature)
|
||||
# Ensure no NaNs/Infs
|
||||
for p in ACTUATOR_FIELDS:
|
||||
attr = getattr(actuators, p)
|
||||
|
||||
@@ -1,111 +0,0 @@
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, fields
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
|
||||
|
||||
|
||||
DBC_OFFSET = (-5.12, 5.11)
|
||||
DBC_ANGLE = (-0.5, 0.5235)
|
||||
DBC_CURVATURE = (-0.02, 0.02)
|
||||
DBC_CURVATURE_RATE = (-0.001024, 0.001023)
|
||||
|
||||
_PATH_HORIZON = 7.0
|
||||
_C2_DELAY = 0.05
|
||||
_PATH_RATES = (4.0, 1.0, math.inf, math.inf)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FordPath:
|
||||
valid: bool = False
|
||||
path_offset: float = 0.0
|
||||
path_angle: float = 0.0
|
||||
curvature: float = 0.0
|
||||
curvature_rate: float = 0.0
|
||||
|
||||
|
||||
def _finite(value: float) -> float:
|
||||
return float(value) if math.isfinite(value) else 0.0
|
||||
|
||||
|
||||
def _model_path(model) -> tuple[list[float], list[float], list[float]] | None:
|
||||
try:
|
||||
x = [float(value) for value in model.position.x]
|
||||
y = [float(value) for value in model.position.y]
|
||||
heading = [float(value) for value in model.orientation.z]
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
return None
|
||||
if len(x) < 2 or len(x) != len(y) or len(x) != len(heading):
|
||||
return None
|
||||
if not all(math.isfinite(value) for values in (x, y, heading) for value in values):
|
||||
return None
|
||||
|
||||
distance = [0.0]
|
||||
for i in range(1, len(x)):
|
||||
distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]))
|
||||
if distance[-1] <= 0.0:
|
||||
return None
|
||||
|
||||
unwrapped_heading = [heading[0]]
|
||||
for value in heading[1:]:
|
||||
delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi
|
||||
unwrapped_heading.append(unwrapped_heading[-1] + delta)
|
||||
return distance, y, unwrapped_heading
|
||||
|
||||
|
||||
def _encode_path(desired_curvature: float, current_curvature: float | None, centering_curvature: float) -> FordPath:
|
||||
action_curvature = _finite(desired_curvature)
|
||||
measured_curvature = float(np.clip(_finite(current_curvature), -MAX_CURVATURE, MAX_CURVATURE)) \
|
||||
if current_curvature is not None else 0.0
|
||||
|
||||
fast_curvature = (action_curvature - centering_curvature) + (action_curvature - measured_curvature)
|
||||
path_offset = 0.5 * fast_curvature * _PATH_HORIZON ** 2
|
||||
path_angle = fast_curvature * _PATH_HORIZON
|
||||
|
||||
return FordPath(
|
||||
valid=True,
|
||||
path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
|
||||
path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
|
||||
curvature=float(np.clip(centering_curvature, *DBC_CURVATURE)),
|
||||
curvature_rate=0.0,
|
||||
)
|
||||
|
||||
|
||||
class FordPathController:
|
||||
"""Split each Ford path command into one-model-frame fast and steady components."""
|
||||
|
||||
def __init__(self, dt: float = 0.01):
|
||||
self.dt = dt
|
||||
self._last_path = FordPath(valid=True)
|
||||
delay_steps = max(round(_C2_DELAY / dt), 1)
|
||||
self._desired_history: deque[float] = deque(maxlen=delay_steps + 1)
|
||||
|
||||
def _delayed_curvature(self, desired_curvature: float) -> float:
|
||||
curvature = float(np.clip(_finite(desired_curvature), *DBC_CURVATURE))
|
||||
if not self._desired_history:
|
||||
self._desired_history.extend([curvature] * self._desired_history.maxlen)
|
||||
self._desired_history.append(curvature)
|
||||
return self._desired_history[0]
|
||||
|
||||
def _limit(self, target: FordPath) -> FordPath:
|
||||
values = []
|
||||
for field, rate in zip(fields(FordPath)[1:], _PATH_RATES, strict=True):
|
||||
previous = getattr(self._last_path, field.name)
|
||||
value = getattr(target, field.name)
|
||||
values.append(float(np.clip(value, previous - rate * self.dt, previous + rate * self.dt)))
|
||||
self._last_path = FordPath(True, *values)
|
||||
return self._last_path
|
||||
|
||||
def update(self, model, desired_curvature: float, *, v_ego: float = 0.0, active: bool = True,
|
||||
current_curvature: float | None = None) -> FordPath:
|
||||
if not active:
|
||||
self._last_path = FordPath(valid=True)
|
||||
self._desired_history.clear()
|
||||
return FordPath()
|
||||
if model is None or _model_path(model) is None:
|
||||
self._desired_history.clear()
|
||||
return self._limit(FordPath(valid=True))
|
||||
centering_curvature = self._delayed_curvature(desired_curvature)
|
||||
return self._limit(_encode_path(desired_curvature, current_curvature, centering_curvature))
|
||||
@@ -1,471 +0,0 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.car.helpers import convert_carControlSP
|
||||
from openpilot.selfdrive.controls.lib.ford_path import DBC_CURVATURE, FordPathController
|
||||
|
||||
|
||||
def _path(curvature: float, curvature_rate: float = 0.0, speed: float = 8.0):
|
||||
t = np.linspace(0.0, 3.0, 61)
|
||||
distance = speed * t
|
||||
heading = curvature * distance + 0.5 * curvature_rate * distance ** 2
|
||||
x = np.zeros_like(distance)
|
||||
y = np.zeros_like(distance)
|
||||
for i in range(1, len(distance)):
|
||||
ds = distance[i] - distance[i - 1]
|
||||
average_heading = 0.5 * (heading[i] + heading[i - 1])
|
||||
x[i] = x[i - 1] + ds * math.cos(average_heading)
|
||||
y[i] = y[i - 1] + ds * math.sin(average_heading)
|
||||
return SimpleNamespace(
|
||||
position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
|
||||
orientation=SimpleNamespace(z=heading.tolist()),
|
||||
)
|
||||
|
||||
|
||||
def _equivalent_curvature(path, distance: float = 7.0) -> float:
|
||||
offset = path.path_offset + path.path_angle * distance + 0.5 * path.curvature * distance ** 2 + \
|
||||
path.curvature_rate * distance ** 3 / 6.0
|
||||
return 2.0 * offset / distance ** 2
|
||||
|
||||
|
||||
def _fast_curvatures(path, distance: float = 7.0) -> tuple[float, float]:
|
||||
return 2.0 * path.path_offset / distance ** 2, path.path_angle / distance
|
||||
|
||||
|
||||
def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_curvature: float | None = None):
|
||||
return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature)
|
||||
|
||||
|
||||
def test_steady_gentle_arc_uses_full_c2_without_fast_pose():
|
||||
path = _command(_path(0.008), 0.008, v_ego=8.0, current_curvature=0.008)
|
||||
|
||||
assert path.valid
|
||||
assert np.isclose(_fast_curvatures(path)[0], 0.0)
|
||||
assert np.isclose(_fast_curvatures(path)[1], 0.0)
|
||||
assert np.isclose(path.curvature, 0.008, atol=5e-5)
|
||||
assert abs(path.curvature_rate) < 1e-5
|
||||
|
||||
|
||||
def test_upstream_c2_remains_primary_for_tight_curve_when_tracking():
|
||||
path = _command(_path(0.015), 0.015, v_ego=20.0, current_curvature=0.015)
|
||||
|
||||
assert np.isclose(path.curvature, 0.015)
|
||||
assert np.allclose(_fast_curvatures(path), (0.0, 0.0))
|
||||
|
||||
|
||||
def test_fast_fields_only_encode_c2_excess_and_tracking_error():
|
||||
tracking = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.04)
|
||||
behind = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.03)
|
||||
|
||||
assert np.isclose(tracking.curvature, DBC_CURVATURE[1])
|
||||
assert np.allclose(_fast_curvatures(tracking), (0.04 - DBC_CURVATURE[1],) * 2)
|
||||
assert np.allclose(_fast_curvatures(behind), (0.05 - DBC_CURVATURE[1],) * 2)
|
||||
|
||||
|
||||
def test_new_request_uses_fast_fields_for_one_model_frame_before_c2():
|
||||
controller = FordPathController()
|
||||
for _ in range(10):
|
||||
controller.update(_path(0.004), 0.004, current_curvature=0.004)
|
||||
|
||||
changing = [controller.update(_path(0.0045), 0.0045, current_curvature=0.0045) for _ in range(6)]
|
||||
|
||||
assert all(np.isclose(command.curvature, 0.004) for command in changing[:5])
|
||||
assert all(np.allclose(_fast_curvatures(command), (0.0005, 0.0005)) for command in changing[:5])
|
||||
assert all(np.isclose(command.curvature + _fast_curvatures(command)[0], 0.0045) for command in changing)
|
||||
assert all(np.isclose(command.curvature + _fast_curvatures(command)[1], 0.0045) for command in changing)
|
||||
assert np.isclose(changing[5].curvature, 0.0045)
|
||||
assert np.allclose(_fast_curvatures(changing[5]), (0.0, 0.0))
|
||||
|
||||
|
||||
def test_one_frame_c2_history_clears_when_control_inactive():
|
||||
controller = FordPathController()
|
||||
for _ in range(10):
|
||||
controller.update(_path(0.01), 0.01, current_curvature=0.01)
|
||||
|
||||
assert not controller.update(_path(0.0), 0.0, active=False).valid
|
||||
resumed = controller.update(_path(-0.002), -0.002, current_curvature=-0.002)
|
||||
|
||||
assert np.isclose(resumed.curvature, -0.002)
|
||||
assert np.allclose(_fast_curvatures(resumed), (0.0, 0.0))
|
||||
|
||||
|
||||
def test_gentle_changing_curve_stays_on_full_c2_when_tracking():
|
||||
path = _command(_path(0.004, 0.00015), 0.004, v_ego=8.0, current_curvature=0.004)
|
||||
|
||||
assert path.path_offset == 0.0
|
||||
assert path.path_angle == 0.0
|
||||
assert np.isclose(path.curvature, 0.004)
|
||||
|
||||
|
||||
def test_c2_follows_action_instead_of_independent_model_unwind():
|
||||
path = _command(_path(0.004, -0.0005), 0.004, v_ego=8.0, current_curvature=0.004)
|
||||
|
||||
assert np.isclose(path.curvature, 0.004)
|
||||
assert path.curvature_rate == 0.0
|
||||
assert path.path_angle == 0.0
|
||||
|
||||
|
||||
def test_model_unwind_does_not_remove_upstream_c2_baseline():
|
||||
steady = _command(_path(0.015), 0.015, v_ego=10.0, current_curvature=0.012)
|
||||
unwinding = _command(_path(0.015, -0.0005), 0.015, v_ego=10.0, current_curvature=0.012)
|
||||
|
||||
assert np.isclose(steady.curvature, 0.015)
|
||||
assert np.isclose(unwinding.curvature, 0.015)
|
||||
assert np.isclose(_equivalent_curvature(unwinding), _equivalent_curvature(steady))
|
||||
|
||||
|
||||
def test_action_curvature_wins_over_opposing_model_geometry():
|
||||
path = _command(_path(0.008), -0.004, v_ego=7.0, current_curvature=0.0)
|
||||
|
||||
assert path.path_angle < 0.0
|
||||
assert _equivalent_curvature(path) < -0.003
|
||||
|
||||
|
||||
def test_changing_path_keeps_c3_zero_without_software_drain():
|
||||
controller = FordPathController()
|
||||
for _ in range(100):
|
||||
changing = controller.update(_path(0.004, 0.001), 0.004, v_ego=15.0, current_curvature=0.004)
|
||||
flat = controller.update(_path(0.004), 0.004, v_ego=15.0, current_curvature=0.004)
|
||||
|
||||
assert changing.curvature_rate == 0.0
|
||||
assert flat.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_sunnypilot_path_message_round_trip():
|
||||
message = custom.CarControlSP.new_message()
|
||||
message.fordLateralPath.pathOffset = 0.3
|
||||
message.fordLateralPath.pathAngle = -0.2
|
||||
message.fordLateralPath.curvature = 0.008
|
||||
message.fordLateralPath.curvatureRate = -0.0004
|
||||
message.fordLateralPath.valid = True
|
||||
|
||||
path = convert_carControlSP(message.as_reader()).fordLateralPath
|
||||
assert np.isclose(path.pathOffset, 0.3)
|
||||
assert np.isclose(path.pathAngle, -0.2)
|
||||
assert np.isclose(path.curvature, 0.008)
|
||||
assert np.isclose(path.curvatureRate, -0.0004)
|
||||
assert path.valid
|
||||
|
||||
|
||||
def test_tight_arc_uses_c2_limit_and_signed_fast_residual():
|
||||
left = _command(_path(0.04), 0.04, v_ego=8.0)
|
||||
right = _command(_path(-0.04), -0.04, v_ego=8.0)
|
||||
|
||||
assert left.curvature == DBC_CURVATURE[1]
|
||||
assert right.curvature == DBC_CURVATURE[0]
|
||||
assert abs(left.curvature_rate) < 1e-4
|
||||
assert abs(right.curvature_rate) < 1e-4
|
||||
assert left.path_angle > 0.06
|
||||
assert right.path_angle < -0.06
|
||||
assert left.path_offset > 0.5
|
||||
assert right.path_offset < -0.5
|
||||
|
||||
|
||||
def test_c2_does_not_increase_when_model_shows_tight_curve_unwind():
|
||||
curvatures = (0.04, 0.018, 0.016, 0.014, 0.012, 0.010, 0.008, 0.006, 0.0)
|
||||
measured = (0.04,) + curvatures[:-1]
|
||||
commands = [_command(_path(curvature, -curvature / 8.0), curvature, v_ego=8.0, current_curvature=actual).curvature
|
||||
for curvature, actual in zip(curvatures, measured, strict=True)]
|
||||
|
||||
assert np.all(np.diff(commands) <= 1e-9)
|
||||
|
||||
|
||||
def test_fresh_model_change_uses_fast_fields_until_one_frame_c2_catches_up():
|
||||
controller = FordPathController(dt=1.0)
|
||||
initial = controller.update(_path(0.04), 0.04, v_ego=8.0)
|
||||
replanned = controller.update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.0)
|
||||
caught_up = controller.update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.0)
|
||||
|
||||
assert initial.path_offset > 0.5
|
||||
assert replanned.curvature > 0.0
|
||||
assert replanned.path_offset < 0.0
|
||||
assert replanned.path_angle < 0.0
|
||||
assert caught_up == FordPathController().update(_path(0.0), 0.0, v_ego=8.0)
|
||||
|
||||
|
||||
def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
|
||||
controller = FordPathController(dt=0.05)
|
||||
controller.update(_path(0.04), 0.04, v_ego=8.0)
|
||||
controller.update(_path(0.04), 0.04, v_ego=8.0)
|
||||
|
||||
outputs = []
|
||||
for frame_id in range(5):
|
||||
model = _path(-0.02)
|
||||
model.frameId = frame_id + 1
|
||||
model.timestampEof = frame_id + 1
|
||||
outputs.append(controller.update(model, -0.02, v_ego=8.0, current_curvature=0.02))
|
||||
|
||||
assert all(path.valid for path in outputs)
|
||||
assert all(DBC_CURVATURE[0] <= path.curvature <= DBC_CURVATURE[1] for path in outputs)
|
||||
assert outputs[0].curvature > 0.0
|
||||
assert all(path.curvature <= 0.0 for path in outputs[1:])
|
||||
assert np.all(np.diff([path.path_angle for path in outputs]) < 0.0)
|
||||
assert np.all(np.diff([path.path_offset for path in outputs]) < 0.0)
|
||||
assert outputs[2].path_angle < 0.0
|
||||
assert outputs[2].path_offset < 0.0
|
||||
assert outputs[-1].path_angle < -0.03
|
||||
assert outputs[-1].path_offset < 0.0
|
||||
|
||||
|
||||
def test_reversal_countersteers_during_bounded_one_frame_c2_persistence():
|
||||
controller = FordPathController()
|
||||
for _ in range(10):
|
||||
assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0
|
||||
|
||||
reversing = [controller.update(_path(-0.02), -0.02, v_ego=8.0, current_curvature=0.002) for _ in range(6)]
|
||||
|
||||
assert all(command.curvature > 0.0 for command in reversing[:5])
|
||||
assert np.all(np.diff([command.path_offset for command in reversing]) < 0.0)
|
||||
assert np.all(np.diff([command.path_angle for command in reversing]) < 0.0)
|
||||
assert all(command.path_offset < 0.0 and command.path_angle < 0.0 for command in reversing[1:])
|
||||
assert reversing[5].curvature <= 0.0
|
||||
|
||||
|
||||
def test_requested_turn_is_not_cancelled_by_previous_path():
|
||||
controller = FordPathController(dt=1.0)
|
||||
previous = _path(-0.02, speed=3.0)
|
||||
previous.frameId = 1
|
||||
previous.timestampEof = 1
|
||||
controller.update(previous, -0.02, v_ego=3.0, current_curvature=-0.01)
|
||||
|
||||
requested = _path(0.02, speed=3.0)
|
||||
requested.frameId = 2
|
||||
requested.timestampEof = 2
|
||||
command = controller.update(requested, 0.02, v_ego=3.0, current_curvature=0.007)
|
||||
|
||||
assert command.path_offset >= 0.0
|
||||
assert command.path_angle >= 0.0
|
||||
assert command.curvature < 0.0
|
||||
assert np.isclose(command.curvature + _fast_curvatures(command)[0], 0.033)
|
||||
assert np.isclose(command.curvature + _fast_curvatures(command)[1], 0.033)
|
||||
assert _equivalent_curvature(command) >= 0.02
|
||||
|
||||
|
||||
def test_short_low_speed_model_uses_available_path_endpoint():
|
||||
command = FordPathController().update(_path(0.02, speed=1.0), 0.02, v_ego=1.0, current_curvature=0.0)
|
||||
|
||||
assert command.valid
|
||||
assert command.path_offset > 0.0
|
||||
assert command.path_angle > 0.0
|
||||
|
||||
|
||||
def test_action_demand_exposes_forward_path_authority():
|
||||
command = FordPathController().update(_path(0.002), 0.0055, v_ego=8.0, current_curvature=0.0005)
|
||||
|
||||
assert _equivalent_curvature(command) >= 0.004
|
||||
|
||||
|
||||
def test_model_curvature_does_not_change_fast_command_for_same_action():
|
||||
gentle_model = _command(_path(0.004), 0.004, v_ego=8.0, current_curvature=0.002)
|
||||
aggressive_model = _command(_path(0.04), 0.004, v_ego=8.0, current_curvature=0.002)
|
||||
|
||||
assert np.isclose(aggressive_model.path_offset, gentle_model.path_offset)
|
||||
assert np.isclose(aggressive_model.path_angle, gentle_model.path_angle)
|
||||
|
||||
|
||||
def test_fast_fields_encode_one_virtual_curvature():
|
||||
command = _command(_path(0.004), 0.008, v_ego=8.0, current_curvature=0.0)
|
||||
|
||||
offset_curvature = 2.0 * command.path_offset / 7.0 ** 2
|
||||
angle_curvature = command.path_angle / 7.0
|
||||
assert np.isclose(offset_curvature, angle_curvature)
|
||||
|
||||
|
||||
def test_action_turn_exposes_fast_authority_without_large_model_arc():
|
||||
command = FordPathController(dt=1.0).update(_path(0.002), 0.04, v_ego=8.0, current_curvature=0.002)
|
||||
|
||||
assert command.curvature == DBC_CURVATURE[1]
|
||||
assert command.path_offset > 0.5
|
||||
assert command.path_angle > 0.2
|
||||
|
||||
|
||||
def test_minor_curve_blends_c2_with_small_pose_authority_when_tracking_is_close():
|
||||
command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048)
|
||||
|
||||
assert 0.0 < command.path_offset < 0.05
|
||||
assert 0.0 < command.path_angle < 0.02
|
||||
assert command.curvature > 0.003
|
||||
|
||||
|
||||
def test_minor_changing_curve_keeps_future_geometry_in_pose_not_c3():
|
||||
command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048)
|
||||
|
||||
assert abs(command.path_offset) < 0.05
|
||||
assert abs(command.path_angle) < 0.02
|
||||
assert command.curvature > 0.003
|
||||
assert command.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_c2_uses_stable_action_curvature_not_independent_model_fit():
|
||||
controller = FordPathController(dt=1.0)
|
||||
first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
|
||||
second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
|
||||
|
||||
assert np.isclose(first.curvature, 0.002)
|
||||
assert np.isclose(second.curvature, 0.002)
|
||||
|
||||
|
||||
def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
|
||||
command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
|
||||
|
||||
assert command.path_offset > 0.0
|
||||
assert command.path_angle > 0.0
|
||||
assert command.curvature >= 0.0
|
||||
assert _equivalent_curvature(command) >= 0.004
|
||||
|
||||
|
||||
def test_action_sign_wins_over_opposing_model_path():
|
||||
command = FordPathController(dt=1.0).update(_path(0.04), -0.0005, v_ego=6.0, current_curvature=0.02)
|
||||
|
||||
assert command.path_offset < 0.0
|
||||
assert command.path_angle < 0.0
|
||||
assert _equivalent_curvature(command) < 0.0
|
||||
|
||||
|
||||
def test_measured_curvature_after_path_exit_commands_countersteer():
|
||||
command = FordPathController().update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.006)
|
||||
|
||||
assert command.curvature == 0.0
|
||||
assert command.path_offset < 0.0
|
||||
assert command.path_angle < 0.0
|
||||
|
||||
|
||||
def test_large_measured_curvature_overshoot_countersteers_without_dropping_c2():
|
||||
controller = FordPathController(dt=1.0)
|
||||
command = controller.update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.012)
|
||||
tracking = FordPathController(dt=1.0).update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.003)
|
||||
|
||||
assert command.path_offset < 0.0
|
||||
assert command.path_angle < 0.0
|
||||
assert command.path_angle < tracking.path_angle
|
||||
assert np.isclose(command.curvature, tracking.curvature)
|
||||
|
||||
|
||||
def test_model_reversal_suppresses_old_c2_and_countersteers():
|
||||
reversing = FordPathController().update(_path(-0.02), -0.0005, v_ego=8.0, current_curvature=0.01)
|
||||
|
||||
assert reversing.curvature <= 0.0
|
||||
assert reversing.path_angle < 0.0
|
||||
|
||||
|
||||
def test_reversal_noise_band_is_continuous():
|
||||
inside = FordPathController(dt=1.0).update(_path(0.02), -0.000099, v_ego=8.0, current_curvature=0.01)
|
||||
outside = FordPathController(dt=1.0).update(_path(0.02), -0.000101, v_ego=8.0, current_curvature=0.01)
|
||||
|
||||
assert abs(outside.path_angle - inside.path_angle) < 0.005
|
||||
|
||||
|
||||
def test_curvature_error_increases_forward_pose_command_while_behind():
|
||||
behind = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.0)
|
||||
tracking = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
|
||||
|
||||
assert behind.path_offset > tracking.path_offset + 0.01
|
||||
assert behind.path_angle > tracking.path_angle + 0.015
|
||||
assert np.isclose(behind.curvature, tracking.curvature)
|
||||
assert tracking.curvature > 0.005
|
||||
assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
|
||||
|
||||
|
||||
def test_major_turn_undertracking_uses_bounded_fast_feedback_authority():
|
||||
command = FordPathController(dt=1.0).update(_path(0.02), 0.02, v_ego=8.0, current_curvature=0.01)
|
||||
|
||||
assert command.curvature == DBC_CURVATURE[1]
|
||||
assert np.isclose(command.path_angle, 0.07)
|
||||
|
||||
|
||||
def test_driver_well_beyond_requested_curvature_gets_countersteer():
|
||||
controller = FordPathController()
|
||||
controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)
|
||||
outputs = [controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01) for _ in range(4)]
|
||||
resumed = outputs[-1]
|
||||
|
||||
assert resumed.curvature > 0.0
|
||||
assert resumed.path_offset < 0.0
|
||||
assert resumed.path_angle < 0.0
|
||||
|
||||
|
||||
def test_action_c2_remains_active_for_centering():
|
||||
centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
|
||||
|
||||
assert centering.curvature > 0.001
|
||||
assert centering.path_offset == 0.0
|
||||
assert centering.path_angle == 0.0
|
||||
|
||||
|
||||
def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():
|
||||
controller = FordPathController()
|
||||
outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)]
|
||||
path = outputs[-1]
|
||||
|
||||
assert path.curvature == DBC_CURVATURE[1]
|
||||
assert path.path_offset > 0.7
|
||||
assert path.path_angle > 0.15
|
||||
assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9
|
||||
assert np.max(np.abs(np.diff([output.path_angle for output in outputs]))) <= 0.01 + 1e-9
|
||||
|
||||
|
||||
def test_curvature_feedback_is_bounded_for_bad_measurement():
|
||||
bounded = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-0.2)
|
||||
corrupted = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-1.0)
|
||||
|
||||
assert np.isclose(corrupted.path_offset, bounded.path_offset)
|
||||
assert np.isclose(corrupted.path_angle, bounded.path_angle)
|
||||
|
||||
|
||||
def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode():
|
||||
controller = FordPathController()
|
||||
for _ in range(10):
|
||||
active = controller.update(_path(0.04), 0.04, v_ego=12.0)
|
||||
missing = controller.update(None, 0.0, v_ego=12.0)
|
||||
|
||||
assert active.path_offset > 0.0
|
||||
assert missing.valid
|
||||
assert np.isclose(active.path_offset - missing.path_offset, 0.04)
|
||||
assert missing.curvature == 0.0
|
||||
assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid
|
||||
|
||||
|
||||
def test_fast_pose_holds_only_demand_beyond_c2_at_measured_target():
|
||||
command = _command(_path(0.03), 0.03, v_ego=20.0, current_curvature=0.03)
|
||||
offset_curvature, angle_curvature = _fast_curvatures(command)
|
||||
|
||||
assert np.isclose(command.curvature, DBC_CURVATURE[1])
|
||||
assert np.isclose(offset_curvature, 0.01)
|
||||
assert np.isclose(angle_curvature, 0.01)
|
||||
|
||||
|
||||
def test_fast_pose_adds_tracking_error_to_demand_beyond_c2():
|
||||
entering = _command(_path(0.03), 0.03, current_curvature=0.0)
|
||||
halfway = _command(_path(0.03), 0.03, current_curvature=0.015)
|
||||
tracking = _command(_path(0.03), 0.03, current_curvature=0.03)
|
||||
|
||||
assert np.allclose(_fast_curvatures(entering), (0.04, 0.04))
|
||||
assert np.allclose(_fast_curvatures(halfway), (0.025, 0.025))
|
||||
assert np.allclose(_fast_curvatures(tracking), (0.01, 0.01))
|
||||
|
||||
|
||||
def test_small_overshoot_reduces_hold_without_reversing_active_arc():
|
||||
tracking = _command(_path(0.03), 0.03, current_curvature=0.03)
|
||||
overshoot = _command(_path(0.03), 0.03, current_curvature=0.031)
|
||||
|
||||
assert 0.0 < overshoot.path_offset < tracking.path_offset
|
||||
assert 0.0 < overshoot.path_angle < tracking.path_angle
|
||||
assert np.allclose(_fast_curvatures(overshoot), (0.009, 0.009))
|
||||
|
||||
|
||||
def test_path_exit_uses_measured_curvature_to_unwind():
|
||||
command = _command(_path(0.0), 0.0, current_curvature=0.03)
|
||||
|
||||
assert np.allclose(_fast_curvatures(command), (-0.03, -0.03))
|
||||
|
||||
|
||||
def test_gentle_c2_share_is_subtracted_once_from_fast_feedforward():
|
||||
command = _command(_path(0.004), 0.004, current_curvature=0.004)
|
||||
offset_curvature, angle_curvature = _fast_curvatures(command)
|
||||
|
||||
assert np.isclose(command.curvature, 0.004)
|
||||
assert np.isclose(offset_curvature, 0.0)
|
||||
assert np.isclose(angle_curvature, 0.0)
|
||||
@@ -32,14 +32,7 @@ from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
|
||||
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
|
||||
AssistCategory,
|
||||
AssistedDrivingMilestones,
|
||||
MilestoneEvent,
|
||||
MilestoneStore,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.system.statsd import statlog
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
@@ -95,8 +88,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.big_model_ready_t = 0.
|
||||
|
||||
# Setup sockets
|
||||
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
|
||||
['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
|
||||
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
|
||||
|
||||
self.gps_location_service = get_gps_location_service(self.params)
|
||||
self.gps_packets = [self.gps_location_service]
|
||||
@@ -135,7 +127,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.params.remove("ExperimentalMode")
|
||||
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
self.car_state_log_mono_time = 0
|
||||
self.AM = AlertManager()
|
||||
self.events = Events()
|
||||
|
||||
@@ -146,11 +137,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.cruise_mismatch_counter = 0
|
||||
self.last_steering_pressed_frame = 0
|
||||
self.distance_traveled = 0
|
||||
self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
|
||||
self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
|
||||
self.assisted_driving_milestone_drive_id = ""
|
||||
self._milestone_event: MilestoneEvent | None = None
|
||||
self._milestone_event_expires_ns = 0
|
||||
self.last_functional_fan_frame = 0
|
||||
self.events_prev = []
|
||||
self.logged_comm_issue = None
|
||||
@@ -541,8 +527,6 @@ class SelfdriveD(CruiseHelper):
|
||||
def data_sample(self):
|
||||
_car_state = messaging.recv_one(self.car_state_sock)
|
||||
CS = _car_state.carState if _car_state else self.CS_prev
|
||||
if _car_state is not None:
|
||||
self.car_state_log_mono_time = _car_state.logMonoTime
|
||||
|
||||
self.sm.update(0)
|
||||
|
||||
@@ -661,31 +645,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.pm.send('onroadEventsSP', ce_send_sp)
|
||||
self.events_sp_prev = self.events_sp.names.copy()
|
||||
|
||||
def publish_assisted_driving_milestones(self, now_ns: int, event: MilestoneEvent | None) -> None:
|
||||
if event is not None:
|
||||
self._milestone_event = event
|
||||
self._milestone_event_expires_ns = now_ns + 1_000_000_000
|
||||
elif now_ns >= self._milestone_event_expires_ns:
|
||||
self._milestone_event = None
|
||||
|
||||
if event is None and self.sm.frame % 10 != 0:
|
||||
return
|
||||
|
||||
snapshot = self.assisted_driving_milestones.snapshot()
|
||||
msg = messaging.new_message("assistedDrivingMilestoneState")
|
||||
msg.valid = True
|
||||
state = msg.assistedDrivingMilestoneState
|
||||
state.enabled = self.assisted_driving_milestones_enabled
|
||||
state.madsDistanceMeters = snapshot.distances_meters[AssistCategory.MADS]
|
||||
state.fullAssistDistanceMeters = snapshot.distances_meters[AssistCategory.FULL_ASSIST]
|
||||
if self._milestone_event is not None:
|
||||
state.event.id = self._milestone_event.event_id
|
||||
state.event.category = self._milestone_event.category.value
|
||||
state.event.distanceMeters = self._milestone_event.distance_meters
|
||||
state.event.previousDistanceMeters = self._milestone_event.previous_distance_meters
|
||||
state.event.unit = self._milestone_event.unit.value
|
||||
self.pm.send("assistedDrivingMilestoneState", msg)
|
||||
|
||||
def step(self):
|
||||
CS = self.data_sample()
|
||||
self.update_events(CS)
|
||||
@@ -695,28 +654,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.mads.update(CS)
|
||||
self.update_alerts(CS)
|
||||
|
||||
now_ns = time.monotonic_ns()
|
||||
if not self.assisted_driving_milestone_drive_id:
|
||||
self.assisted_driving_milestone_drive_id = self.params.get("CurrentRoute") or ""
|
||||
self.assisted_driving_milestones.set_drive_id(self.assisted_driving_milestone_drive_id)
|
||||
car_control = self.sm['carControl']
|
||||
milestone_event = self.assisted_driving_milestones.update(
|
||||
self.car_state_log_mono_time,
|
||||
CS.vEgo,
|
||||
lat_active=car_control.latActive,
|
||||
long_active=car_control.longActive,
|
||||
is_metric=self.is_metric,
|
||||
enabled=self.assisted_driving_milestones_enabled,
|
||||
)
|
||||
if milestone_event is not None:
|
||||
cloudlog.event("assisted_driving_milestone_reached",
|
||||
event_id=milestone_event.event_id,
|
||||
category=milestone_event.category.value,
|
||||
distance_meters=milestone_event.distance_meters)
|
||||
statlog.gauge(f"assisted_driving_milestone.{milestone_event.category.value}.meters",
|
||||
milestone_event.distance_meters)
|
||||
self.publish_assisted_driving_milestones(now_ns, milestone_event)
|
||||
|
||||
self.button_state_tracker.update(CS)
|
||||
self.publish_selfdriveState(CS)
|
||||
|
||||
@@ -729,7 +666,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
|
||||
self.personality = self.params.get("LongitudinalPersonality", return_default=True)
|
||||
self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
|
||||
|
||||
self.mads.read_params()
|
||||
time.sleep(0.1)
|
||||
@@ -743,7 +679,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.step()
|
||||
self.rk.monitor_time()
|
||||
finally:
|
||||
self.assisted_driving_milestones.close()
|
||||
e.set()
|
||||
t.join()
|
||||
|
||||
|
||||
@@ -1,8 +1,5 @@
|
||||
import os
|
||||
|
||||
import pyray as rl
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.hardware import PC
|
||||
from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
|
||||
from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
|
||||
from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
|
||||
@@ -64,8 +61,7 @@ class MiciMainLayout(Scroller):
|
||||
|
||||
# Start onboarding if terms or training not completed, make sure to push after self
|
||||
self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
|
||||
skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
|
||||
if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
|
||||
if not self._onboarding_window.completed:
|
||||
gui_app.push_widget(self._onboarding_window)
|
||||
|
||||
# initialize correct onroad layout
|
||||
@@ -123,8 +119,6 @@ class MiciMainLayout(Scroller):
|
||||
self._onroad_time_delay = rl.get_time()
|
||||
else:
|
||||
self._scroll_to(self._home_layout)
|
||||
if hasattr(self._home_layout, "request_drive_summary"):
|
||||
self._home_layout.request_drive_summary()
|
||||
|
||||
# FIXME: these two pops can interrupt user interacting in the settings
|
||||
if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
|
||||
|
||||
@@ -47,7 +47,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
|
||||
ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
|
||||
always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
|
||||
milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
|
||||
record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
|
||||
record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
|
||||
enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
|
||||
@@ -58,7 +57,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
is_metric_toggle,
|
||||
ldw_toggle,
|
||||
always_on_dm_toggle,
|
||||
milestone_celebrations_toggle,
|
||||
record_front,
|
||||
record_mic,
|
||||
enable_openpilot,
|
||||
@@ -70,7 +68,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
("AlwaysOnDM", always_on_dm_toggle),
|
||||
("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
|
||||
("RecordFront", record_front),
|
||||
("RecordAudio", record_mic),
|
||||
("OpenpilotEnabledToggle", enable_openpilot),
|
||||
|
||||
@@ -20,7 +20,6 @@ AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
|
||||
ALERT_MARGIN = 18
|
||||
ALERT_BACKGROUND_OPACITY = 0.90
|
||||
|
||||
ALERT_FONT_SMALL = 66 - 50
|
||||
ALERT_FONT_BIG = 88 - 40
|
||||
@@ -280,7 +279,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
|
||||
def _draw_background(self, alert: Alert) -> None:
|
||||
# draw top gradient for alert text at top
|
||||
color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
|
||||
color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
|
||||
color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
|
||||
translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
|
||||
|
||||
small_alert_height = round(self._rect.height * 0.583) # 140px at mici height
|
||||
|
||||
@@ -19,15 +19,10 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
from enum import IntEnum
|
||||
|
||||
MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
|
||||
|
||||
if MILESTONE_CELEBRATION_ENABLED:
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
|
||||
NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
|
||||
@@ -161,7 +156,6 @@ class AugmentedRoadView(CameraView):
|
||||
self._alert_renderer = AlertRenderer()
|
||||
self._driver_state_renderer = DriverStateRenderer()
|
||||
self._confidence_ball = ConfidenceBall()
|
||||
self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
|
||||
self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
|
||||
text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
|
||||
@@ -229,12 +223,6 @@ class AugmentedRoadView(CameraView):
|
||||
|
||||
alert_to_render, not_animating_out = self._alert_renderer.will_render()
|
||||
|
||||
if self._milestone_celebration is not None:
|
||||
if alert_to_render is not None:
|
||||
self._milestone_celebration.cancel_for_alert()
|
||||
else:
|
||||
self._milestone_celebration.render(self._content_rect)
|
||||
|
||||
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
|
||||
should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
|
||||
(ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
|
||||
@@ -259,6 +247,7 @@ class AugmentedRoadView(CameraView):
|
||||
self._confidence_ball.render(self.rect)
|
||||
|
||||
self._bookmark_icon.render(self.rect)
|
||||
|
||||
def _switch_stream_if_needed(self, sm):
|
||||
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
|
||||
v_ego = sm['carState'].vEgo
|
||||
@@ -366,12 +355,10 @@ class AugmentedRoadView(CameraView):
|
||||
return self._cached_matrix
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
if gui_app.sunnypilot_ui():
|
||||
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
|
||||
|
||||
def hide_event(self):
|
||||
super().hide_event()
|
||||
if gui_app.sunnypilot_ui():
|
||||
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
|
||||
|
||||
|
||||
@@ -24,8 +24,14 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
|
||||
SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
|
||||
FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)
|
||||
|
||||
AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
|
||||
DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
|
||||
|
||||
VOLUME_BASE = 20
|
||||
if HARDWARE.get_device_type() == "tizi":
|
||||
AMBIENT_DB = 30
|
||||
VOLUME_BASE = 10
|
||||
|
||||
AudibleAlert = log.SelfdriveState.AudibleAlert
|
||||
AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
|
||||
|
||||
@@ -47,7 +53,6 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
|
||||
AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),
|
||||
|
||||
AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),
|
||||
|
||||
AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
|
||||
@@ -55,14 +60,6 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
|
||||
**sound_list_sp,
|
||||
}
|
||||
|
||||
|
||||
def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
|
||||
ambient_db = 30 if device_type in ("mici", "tizi") else 26
|
||||
volume_base = 10 if device_type in ("mici", "tizi") else 20
|
||||
volume_boost = 1.5 if device_type == "mici" else 1.0
|
||||
volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
|
||||
return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
|
||||
|
||||
def check_selfdrive_timeout_alert(sm):
|
||||
ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
|
||||
|
||||
@@ -77,7 +74,6 @@ class Soundd(QuietMode):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
self.device_type = HARDWARE.get_device_type()
|
||||
self.load_sounds()
|
||||
|
||||
self.current_alert = AudibleAlert.none
|
||||
@@ -89,7 +85,6 @@ class Soundd(QuietMode):
|
||||
|
||||
self.selfdrive_timeout_alert = False
|
||||
self.pending_stop = False
|
||||
self.last_milestone_event_id = 0
|
||||
|
||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
||||
|
||||
@@ -169,19 +164,9 @@ class Soundd(QuietMode):
|
||||
self.update_alert(AudibleAlert.none)
|
||||
self.selfdrive_timeout_alert = False
|
||||
|
||||
def update_milestone_alert(self, sm):
|
||||
if not sm.updated['assistedDrivingMilestoneState']:
|
||||
return
|
||||
milestone_state = sm['assistedDrivingMilestoneState']
|
||||
event_id = milestone_state.event.id
|
||||
if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
|
||||
return
|
||||
self.last_milestone_event_id = event_id
|
||||
if self.current_alert == AudibleAlert.none and not self.enabled:
|
||||
self.update_alert(AudibleAlert.complete)
|
||||
|
||||
def calculate_volume(self, weighted_db):
|
||||
return calculate_volume_for_device(weighted_db, self.device_type)
|
||||
volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
|
||||
return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
|
||||
|
||||
@retry(attempts=10, delay=3)
|
||||
def get_stream(self, sd):
|
||||
@@ -195,7 +180,7 @@ class Soundd(QuietMode):
|
||||
import sounddevice as sd
|
||||
micd.patch_sounddevice(sd)
|
||||
|
||||
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
|
||||
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
|
||||
|
||||
with self.get_stream(sd) as stream:
|
||||
rk = Ratekeeper(20)
|
||||
@@ -213,7 +198,6 @@ class Soundd(QuietMode):
|
||||
self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
|
||||
|
||||
self.get_audible_alert(sm)
|
||||
self.update_milestone_alert(sm)
|
||||
|
||||
# Ramp up immediate warning sound over 4s
|
||||
if self.current_alert == AudibleAlert.warningImmediate:
|
||||
|
||||
@@ -5,28 +5,14 @@ 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 math
|
||||
import time
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets.icon_widget import IconWidget
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
|
||||
|
||||
METERS_PER_MILE = 1609.344
|
||||
METERS_PER_KILOMETER = 1000.0
|
||||
SUMMARY_DURATION_SECONDS = 10.0
|
||||
SUMMARY_WAIT_SECONDS = 3.0
|
||||
|
||||
|
||||
def _nonnegative_float(value) -> float:
|
||||
try:
|
||||
return max(0.0, float(value))
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
|
||||
|
||||
class MiciHomeLayoutSP(MiciHomeLayout):
|
||||
@@ -40,52 +26,6 @@ class MiciHomeLayoutSP(MiciHomeLayout):
|
||||
gray_idx = self._status_bar_layout.widgets.index(self._egpu_icon_gray)
|
||||
self._status_bar_layout.widgets.insert(gray_idx + 1, self._egpu_icon_default)
|
||||
self._status_bar_layout.widgets.insert(gray_idx + 2, self._egpu_icon_orange)
|
||||
initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
|
||||
self._last_summary_id = initial_summary.get("id", 0)
|
||||
self._summary_wait_until = 0.0
|
||||
self._summary_visible_until = 0.0
|
||||
self._drive_summary = {}
|
||||
|
||||
def request_drive_summary(self) -> None:
|
||||
self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
|
||||
|
||||
def _render(self, _: rl.Rectangle) -> None:
|
||||
super()._render(_)
|
||||
now = time.monotonic()
|
||||
if now < self._summary_wait_until:
|
||||
summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
|
||||
summary_id = summary.get("id", 0)
|
||||
if summary_id and summary_id != self._last_summary_id:
|
||||
self._last_summary_id = summary_id
|
||||
distances = summary.get("distancesMeters", {})
|
||||
enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
|
||||
if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
|
||||
self._drive_summary = summary
|
||||
self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
|
||||
self._summary_wait_until = 0.0
|
||||
|
||||
if now < self._summary_visible_until:
|
||||
self._draw_drive_summary(_)
|
||||
|
||||
def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
|
||||
distances = self._drive_summary.get("distancesMeters", {})
|
||||
metric = self._drive_summary.get("unit") == "metric"
|
||||
meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
|
||||
unit = "KM" if metric else "MI"
|
||||
mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
|
||||
full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
|
||||
|
||||
rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
|
||||
gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
|
||||
font_weight=FontWeight.SEMI_BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
|
||||
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
|
||||
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
|
||||
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
|
||||
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
|
||||
def _set_egpu_visibility(self):
|
||||
chestnut = ui_state.sm["deviceState"].chestnutPresent
|
||||
|
||||
@@ -1,228 +0,0 @@
|
||||
"""Render assisted-driving milestone celebrations over the on-road view."""
|
||||
|
||||
import math
|
||||
import random
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
|
||||
from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
|
||||
CELEBRATION_DURATION = 4.5
|
||||
PARTICLE_COUNT = 150
|
||||
METERS_PER_MILE = 1609.344
|
||||
METERS_PER_KILOMETER = 1000.0
|
||||
|
||||
CONFETTI_COLORS = (
|
||||
rl.Color(255, 55, 95, 255),
|
||||
rl.Color(255, 183, 3, 255),
|
||||
rl.Color(48, 209, 88, 255),
|
||||
rl.Color(36, 179, 255, 255),
|
||||
rl.Color(112, 72, 232, 255),
|
||||
rl.Color(255, 45, 196, 255),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ConfettiParticle:
|
||||
x: float
|
||||
y: float
|
||||
width: float
|
||||
height: float
|
||||
speed: float
|
||||
drift: float
|
||||
angle: float
|
||||
spin: float
|
||||
phase: float
|
||||
color: rl.Color
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CelebrationMilestone:
|
||||
event_id: int
|
||||
full_assist: bool
|
||||
distance_meters: float
|
||||
previous_distance_meters: float
|
||||
metric: bool
|
||||
|
||||
|
||||
class MilestoneCelebration(Widget):
|
||||
"""Pure renderer for typed assisted-driving milestone events."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._drive_started_time = -1.0
|
||||
self._celebration_started_time: float | None = None
|
||||
self._current_milestone: CelebrationMilestone | None = None
|
||||
self._pending_milestones: deque[CelebrationMilestone] = deque()
|
||||
self._last_event_id = 0
|
||||
self._particles = self._make_particles()
|
||||
|
||||
@staticmethod
|
||||
def _make_particles() -> list[ConfettiParticle]:
|
||||
rng = random.Random(20260828)
|
||||
return [
|
||||
ConfettiParticle(
|
||||
x=rng.random(),
|
||||
y=rng.uniform(-0.25, 0.95),
|
||||
width=rng.uniform(10, 24),
|
||||
height=rng.uniform(24, 58),
|
||||
speed=rng.uniform(0.12, 0.34),
|
||||
drift=rng.uniform(-0.035, 0.035),
|
||||
angle=rng.uniform(0, 360),
|
||||
spin=rng.uniform(-150, 150),
|
||||
phase=rng.uniform(0, math.tau),
|
||||
color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
|
||||
)
|
||||
for _ in range(PARTICLE_COUNT)
|
||||
]
|
||||
|
||||
def _render(self, rect: rl.Rectangle, /) -> None:
|
||||
now = time.monotonic()
|
||||
if ui_state.started_time != self._drive_started_time:
|
||||
self._drive_started_time = ui_state.started_time
|
||||
self._celebration_started_time = None
|
||||
self._current_milestone = None
|
||||
self._pending_milestones.clear()
|
||||
|
||||
self._consume_event(suppress=False)
|
||||
|
||||
if self._current_milestone is None and self._pending_milestones:
|
||||
self._current_milestone = self._pending_milestones.popleft()
|
||||
self._celebration_started_time = now
|
||||
|
||||
if self._celebration_started_time is None or self._current_milestone is None:
|
||||
return
|
||||
|
||||
elapsed = now - self._celebration_started_time
|
||||
if elapsed >= CELEBRATION_DURATION:
|
||||
self._celebration_started_time = None
|
||||
self._current_milestone = None
|
||||
return
|
||||
|
||||
alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
|
||||
self._draw_background_scrim(rect, alpha)
|
||||
self._draw_confetti(rect, elapsed, alpha)
|
||||
self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
|
||||
|
||||
def cancel_for_alert(self) -> None:
|
||||
self._consume_event(suppress=True)
|
||||
self._celebration_started_time = None
|
||||
self._current_milestone = None
|
||||
self._pending_milestones.clear()
|
||||
|
||||
def _consume_event(self, suppress: bool) -> None:
|
||||
if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
|
||||
return
|
||||
state = ui_state.sm["assistedDrivingMilestoneState"]
|
||||
event = state.event
|
||||
if not state.enabled:
|
||||
self._celebration_started_time = None
|
||||
self._current_milestone = None
|
||||
self._pending_milestones.clear()
|
||||
return
|
||||
if event.id == 0 or event.id == self._last_event_id:
|
||||
return
|
||||
self._last_event_id = event.id
|
||||
if suppress:
|
||||
return
|
||||
self._pending_milestones.append(CelebrationMilestone(
|
||||
event_id=event.id,
|
||||
full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
|
||||
distance_meters=event.distanceMeters,
|
||||
previous_distance_meters=event.previousDistanceMeters,
|
||||
metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
|
||||
))
|
||||
|
||||
def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
|
||||
travel_height = rect.height * 1.45
|
||||
compact = rect.height <= 300
|
||||
particle_scale = rect.height / 1080.0
|
||||
particles = self._particles[:100] if compact else self._particles
|
||||
for particle in particles:
|
||||
x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
|
||||
y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
|
||||
flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
|
||||
particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
|
||||
origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
|
||||
color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
|
||||
rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
|
||||
|
||||
@staticmethod
|
||||
def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
|
||||
# Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
|
||||
scale = rect.height / 240.0
|
||||
pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
|
||||
number_size = int(FONT_SIZES.set_speed * scale * pulse)
|
||||
milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
|
||||
category_size = int(22 * scale * pulse)
|
||||
unit_size = category_size
|
||||
|
||||
display_font = gui_app.font(FontWeight.DISPLAY)
|
||||
semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
tween_progress = min(elapsed / 0.85, 1.0)
|
||||
tween_progress = 1.0 - (1.0 - tween_progress) ** 3
|
||||
meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
|
||||
previous_distance = milestone.previous_distance_meters / meters_per_unit
|
||||
milestone_distance = milestone.distance_meters / meters_per_unit
|
||||
displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
|
||||
if tween_progress >= 1.0:
|
||||
number = f"{round(milestone_distance):,}"
|
||||
else:
|
||||
number = f"{displayed_distance:,.1f}"
|
||||
unit = tr("KM") if milestone.metric else tr("MI")
|
||||
category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
|
||||
milestone_label = tr("MILESTONE")
|
||||
|
||||
unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
|
||||
number_bounds = measure_text_cached(display_font, number, number_size)
|
||||
max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
|
||||
if number_bounds.x > max_number_width:
|
||||
number_size = max(1, int(number_size * max_number_width / number_bounds.x))
|
||||
number_bounds = measure_text_cached(display_font, number, number_size)
|
||||
category_bounds = measure_text_cached(semibold_font, category, category_size)
|
||||
milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
|
||||
|
||||
center_x = rect.x + rect.width / 2
|
||||
center_y = rect.y + rect.height / 2
|
||||
text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
|
||||
secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
|
||||
number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
|
||||
number_x = center_x - number_line_width / 2
|
||||
number_y = center_y - 76 * scale
|
||||
unit_y = center_y + 14 * scale
|
||||
category_y = center_y - 91 * scale
|
||||
milestone_y = center_y + 50 * scale
|
||||
|
||||
rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
|
||||
category_size, 0, secondary_color)
|
||||
rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
|
||||
rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
|
||||
unit_size, 0, secondary_color)
|
||||
rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
|
||||
milestone_size, 0, text_color)
|
||||
|
||||
@staticmethod
|
||||
def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
|
||||
# Match the alert background: a mostly opaque black core fading to transparent.
|
||||
fade_height = round(rect.height * 0.25)
|
||||
solid_height = round(rect.height * 0.50)
|
||||
solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
|
||||
transparent = rl.Color(0, 0, 0, 0)
|
||||
x = int(rect.x)
|
||||
y = int(rect.y)
|
||||
width = int(rect.width)
|
||||
|
||||
rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
|
||||
rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
|
||||
rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)
|
||||
@@ -35,8 +35,7 @@ class UIStateSP:
|
||||
self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
|
||||
self.sm_services_ext = [
|
||||
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
|
||||
"gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
|
||||
"assistedDrivingMilestoneState",
|
||||
"gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
|
||||
]
|
||||
|
||||
self.sunnylink_state = SunnylinkState()
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate the assisted-driving milestone celebration chime."""
|
||||
|
||||
import math
|
||||
import wave
|
||||
from array import array
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
SAMPLE_RATE = 48_000
|
||||
DURATION_SECONDS = 0.82
|
||||
NOTES = (
|
||||
(0.00, 523.25),
|
||||
(0.11, 659.25),
|
||||
(0.22, 783.99),
|
||||
)
|
||||
|
||||
|
||||
def note_sample(age: float, frequency: float) -> float:
|
||||
if not 0 <= age <= 0.58:
|
||||
return 0.0
|
||||
attack = min(age / 0.008, 1.0)
|
||||
release = min((0.58 - age) / 0.15, 1.0)
|
||||
envelope = attack * release * math.exp(-3.8 * age)
|
||||
tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
|
||||
return envelope * tone
|
||||
|
||||
|
||||
def main() -> None:
|
||||
output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
|
||||
samples = array('h')
|
||||
for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
|
||||
t = frame / SAMPLE_RATE
|
||||
value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
|
||||
samples.append(round(max(-1.0, min(1.0, value)) * 32767))
|
||||
|
||||
with wave.open(str(output), "wb") as wav:
|
||||
wav.setnchannels(1)
|
||||
wav.setsampwidth(2)
|
||||
wav.setframerate(SAMPLE_RATE)
|
||||
wav.writeframes(samples.tobytes())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,38 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Publish deterministic milestone events for the local comma-four UI preview."""
|
||||
|
||||
import itertools
|
||||
import time
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
|
||||
|
||||
def main() -> None:
|
||||
pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
|
||||
milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
|
||||
event_id = 0
|
||||
milestone, previous_milestone, category = 0, 0, "mads"
|
||||
next_event_time = time.monotonic() + 1.0
|
||||
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
if now >= next_event_time:
|
||||
event_id += 1
|
||||
milestone, previous_milestone, category = next(milestones)
|
||||
next_event_time = now + 6.0
|
||||
|
||||
msg = messaging.new_message("assistedDrivingMilestoneState")
|
||||
state = msg.assistedDrivingMilestoneState
|
||||
state.enabled = True
|
||||
if event_id:
|
||||
state.event.id = event_id
|
||||
state.event.category = category
|
||||
state.event.distanceMeters = milestone * 1609.344
|
||||
state.event.previousDistanceMeters = previous_milestone * 1609.344
|
||||
state.event.unit = "imperial"
|
||||
pm.send("assistedDrivingMilestoneState", msg)
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,27 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
|
||||
replay_pid=""
|
||||
preview_pid=""
|
||||
|
||||
cleanup() {
|
||||
for pid in "$preview_pid" "$replay_pid"; do
|
||||
if [[ -n "$pid" ]]; then
|
||||
kill "$pid" 2>/dev/null || true
|
||||
wait "$pid" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
}
|
||||
trap cleanup EXIT INT TERM
|
||||
|
||||
export PATH="$repo_root/.venv/bin:$PATH"
|
||||
export SP_MILESTONE_PREVIEW=1
|
||||
playback="${SP_MILESTONE_PLAYBACK:-1}"
|
||||
|
||||
"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
|
||||
replay_pid=$!
|
||||
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
|
||||
preview_pid=$!
|
||||
|
||||
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
|
||||
@@ -4,63 +4,12 @@ import time
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.cereal.messaging import SubMaster, PubMaster
|
||||
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
|
||||
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
|
||||
|
||||
AudibleAlert = log.SelfdriveState.AudibleAlert
|
||||
|
||||
|
||||
class TestSoundd(OpenpilotTestCase):
|
||||
@staticmethod
|
||||
def milestone_submaster(event_id=42):
|
||||
class SubMasterStub:
|
||||
def __init__(self):
|
||||
self.updated = {'assistedDrivingMilestoneState': True}
|
||||
msg = messaging.new_message('assistedDrivingMilestoneState')
|
||||
msg.assistedDrivingMilestoneState.enabled = True
|
||||
msg.assistedDrivingMilestoneState.event.id = event_id
|
||||
self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
|
||||
|
||||
def __getitem__(self, service):
|
||||
return self.data[service]
|
||||
|
||||
return SubMasterStub()
|
||||
|
||||
def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
|
||||
for weighted_db in (20.0, 30.0, 40.0, 50.0):
|
||||
with self.subTest(weighted_db=weighted_db):
|
||||
comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
|
||||
comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
|
||||
assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
|
||||
|
||||
def test_milestone_chime_uses_typed_milestone_event_once(self):
|
||||
soundd = Soundd()
|
||||
sm = self.milestone_submaster()
|
||||
soundd.update_milestone_alert(sm)
|
||||
|
||||
assert soundd.current_alert == AudibleAlert.complete
|
||||
soundd.current_alert = AudibleAlert.none
|
||||
soundd.update_milestone_alert(sm)
|
||||
assert soundd.current_alert == AudibleAlert.none
|
||||
|
||||
def test_safety_alert_consumes_milestone_without_replaying_it(self):
|
||||
soundd = Soundd()
|
||||
sm = self.milestone_submaster()
|
||||
soundd.current_alert = AudibleAlert.warningImmediate
|
||||
|
||||
soundd.update_milestone_alert(sm)
|
||||
soundd.current_alert = AudibleAlert.none
|
||||
soundd.update_milestone_alert(sm)
|
||||
|
||||
assert soundd.current_alert == AudibleAlert.none
|
||||
|
||||
def test_quiet_mode_consumes_milestone_without_playing_it(self):
|
||||
soundd = Soundd()
|
||||
soundd.enabled = True
|
||||
|
||||
soundd.update_milestone_alert(self.milestone_submaster())
|
||||
|
||||
assert soundd.current_alert == AudibleAlert.none
|
||||
|
||||
def test_check_selfdrive_timeout_alert(self, mocker):
|
||||
sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
|
||||
pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
|
||||
|
||||
@@ -104,14 +104,6 @@ class ControlsExt(ModelStateBase):
|
||||
CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
|
||||
CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
|
||||
|
||||
ford_path = getattr(self, 'ford_path', None)
|
||||
if ford_path is not None:
|
||||
CC_SP.fordLateralPath.valid = ford_path.valid
|
||||
CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
|
||||
CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
|
||||
CC_SP.fordLateralPath.curvature = ford_path.curvature
|
||||
CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
|
||||
|
||||
return CC_SP
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -1,260 +0,0 @@
|
||||
"""Authoritative assisted-driving distance and milestone tracking."""
|
||||
|
||||
import math
|
||||
from collections.abc import Mapping
|
||||
from dataclasses import dataclass
|
||||
from enum import StrEnum
|
||||
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
METERS_PER_MILE = 1609.344
|
||||
METERS_PER_KILOMETER = 1000.0
|
||||
MAX_SAMPLE_INTERVAL_SECONDS = 0.5
|
||||
PERSIST_INTERVAL_NS = 10_000_000_000
|
||||
STATE_VERSION = 1
|
||||
STATE_PARAM = "AssistedDrivingMilestoneState"
|
||||
LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
|
||||
|
||||
|
||||
class AssistCategory(StrEnum):
|
||||
MADS = "mads"
|
||||
FULL_ASSIST = "fullAssist"
|
||||
|
||||
|
||||
class MilestoneUnit(StrEnum):
|
||||
IMPERIAL = "imperial"
|
||||
METRIC = "metric"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MilestoneEvent:
|
||||
event_id: int
|
||||
category: AssistCategory
|
||||
distance_meters: float
|
||||
previous_distance_meters: float
|
||||
unit: MilestoneUnit
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MilestoneSnapshot:
|
||||
distances_meters: dict[AssistCategory, float]
|
||||
drive_start_distances_meters: dict[AssistCategory, float]
|
||||
next_event_id: int
|
||||
next_summary_id: int
|
||||
unit: MilestoneUnit
|
||||
active_drive_id: str
|
||||
|
||||
|
||||
def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
|
||||
if not lat_active:
|
||||
return None
|
||||
return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
|
||||
|
||||
|
||||
def _meters_per_unit(unit: MilestoneUnit) -> float:
|
||||
return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
|
||||
|
||||
|
||||
def _next_ladder_value(value: float) -> float:
|
||||
value = max(0.0, value)
|
||||
magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
|
||||
for multiplier in (1.0, 2.0, 5.0):
|
||||
candidate = multiplier * magnitude
|
||||
if candidate > value + 1e-9:
|
||||
return candidate
|
||||
return 10.0 * magnitude
|
||||
|
||||
|
||||
def _previous_ladder_value(value: float) -> float:
|
||||
if value <= 1.0:
|
||||
return 0.0
|
||||
magnitude = 10.0 ** math.floor(math.log10(value))
|
||||
normalized = value / magnitude
|
||||
if normalized <= 1.0 + 1e-9:
|
||||
return 5.0 * magnitude / 10.0
|
||||
if normalized <= 2.0 + 1e-9:
|
||||
return magnitude
|
||||
return 2.0 * magnitude
|
||||
|
||||
|
||||
def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
|
||||
meters_per_unit = _meters_per_unit(unit)
|
||||
return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
|
||||
|
||||
|
||||
class MilestoneStore:
|
||||
def __init__(self, params: Params | None = None):
|
||||
self._params = params or Params()
|
||||
|
||||
def load(self) -> MilestoneSnapshot:
|
||||
raw = self._params.get(STATE_PARAM, return_default=True)
|
||||
raw = raw if isinstance(raw, dict) else {}
|
||||
raw_distances = raw.get("distancesMeters", {})
|
||||
raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
|
||||
try:
|
||||
unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
|
||||
except ValueError:
|
||||
unit = MilestoneUnit.IMPERIAL
|
||||
|
||||
def distance(category: AssistCategory) -> float:
|
||||
try:
|
||||
return max(0.0, float(raw_distances.get(category.value, 0.0)))
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
|
||||
distances = {category: distance(category) for category in AssistCategory}
|
||||
raw_drive_start = raw.get("driveStartDistancesMeters", {})
|
||||
raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
|
||||
|
||||
def drive_start_distance(category: AssistCategory) -> float:
|
||||
try:
|
||||
return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
|
||||
except (TypeError, ValueError):
|
||||
return distances[category]
|
||||
|
||||
try:
|
||||
next_event_id = max(1, int(raw.get("nextEventId", 1)))
|
||||
except (TypeError, ValueError):
|
||||
next_event_id = 1
|
||||
try:
|
||||
next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
|
||||
except (TypeError, ValueError):
|
||||
next_summary_id = 1
|
||||
|
||||
return MilestoneSnapshot(
|
||||
distances_meters=distances,
|
||||
drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
|
||||
next_event_id=next_event_id,
|
||||
next_summary_id=next_summary_id,
|
||||
unit=unit,
|
||||
active_drive_id=str(raw.get("activeDriveId", "")),
|
||||
)
|
||||
|
||||
def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
|
||||
if block:
|
||||
self._params.flush()
|
||||
self._params.put(STATE_PARAM, {
|
||||
"version": STATE_VERSION,
|
||||
"distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
|
||||
"driveStartDistancesMeters": {
|
||||
category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
|
||||
},
|
||||
"nextEventId": max(1, snapshot.next_event_id),
|
||||
"nextSummaryId": max(1, snapshot.next_summary_id),
|
||||
"unit": snapshot.unit.value,
|
||||
"activeDriveId": snapshot.active_drive_id,
|
||||
}, block=block)
|
||||
|
||||
def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
|
||||
self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
|
||||
"version": STATE_VERSION,
|
||||
"id": summary_id,
|
||||
"distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
|
||||
"unit": unit.value,
|
||||
}, block=True)
|
||||
|
||||
|
||||
class AssistedDrivingMilestones:
|
||||
"""Tracks, persists, and emits milestones through one small interface."""
|
||||
|
||||
def __init__(self, store: MilestoneStore | None = None):
|
||||
self._store = store or MilestoneStore()
|
||||
snapshot = self._store.load()
|
||||
self._distances_meters = snapshot.distances_meters
|
||||
self._drive_start_distances_meters = snapshot.drive_start_distances_meters
|
||||
self._next_event_id = snapshot.next_event_id
|
||||
self._next_summary_id = snapshot.next_summary_id
|
||||
self._unit = snapshot.unit
|
||||
self._active_drive_id = snapshot.active_drive_id
|
||||
self._next_milestone_meters = {
|
||||
category: next_milestone_meters(distance, self._unit)
|
||||
for category, distance in self._distances_meters.items()
|
||||
}
|
||||
self._last_timestamp_ns: int | None = None
|
||||
self._last_persist_timestamp_ns: int | None = None
|
||||
self._last_speed_mps = 0.0
|
||||
self._last_category: AssistCategory | None = None
|
||||
self._enabled = False
|
||||
self._closed = False
|
||||
|
||||
def snapshot(self) -> MilestoneSnapshot:
|
||||
return MilestoneSnapshot(
|
||||
self._distances_meters.copy(),
|
||||
self._drive_start_distances_meters.copy(),
|
||||
self._next_event_id,
|
||||
self._next_summary_id,
|
||||
self._unit,
|
||||
self._active_drive_id,
|
||||
)
|
||||
|
||||
def set_drive_id(self, drive_id: str) -> None:
|
||||
if not drive_id or drive_id == self._active_drive_id:
|
||||
return
|
||||
self._active_drive_id = drive_id
|
||||
self._drive_start_distances_meters = self._distances_meters.copy()
|
||||
self._persist()
|
||||
|
||||
def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
|
||||
is_metric: bool, enabled: bool) -> MilestoneEvent | None:
|
||||
self._enabled = enabled
|
||||
unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
|
||||
if unit != self._unit:
|
||||
self._unit = unit
|
||||
self._next_milestone_meters = {
|
||||
category: next_milestone_meters(distance, unit)
|
||||
for category, distance in self._distances_meters.items()
|
||||
}
|
||||
|
||||
speed_mps = max(0.0, speed_mps)
|
||||
category = assist_category(lat_active, long_active) if enabled else None
|
||||
event = None
|
||||
|
||||
if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
|
||||
dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
|
||||
if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
|
||||
active_category = self._last_category
|
||||
self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
|
||||
threshold_meters = self._next_milestone_meters[active_category]
|
||||
if self._distances_meters[active_category] >= threshold_meters:
|
||||
meters_per_unit = _meters_per_unit(self._unit)
|
||||
threshold_units = threshold_meters / meters_per_unit
|
||||
event = MilestoneEvent(
|
||||
event_id=self._next_event_id,
|
||||
category=active_category,
|
||||
distance_meters=threshold_meters,
|
||||
previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
|
||||
unit=self._unit,
|
||||
)
|
||||
self._next_event_id += 1
|
||||
self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
|
||||
self._persist(timestamp_ns=timestamp_ns)
|
||||
|
||||
self._last_timestamp_ns = timestamp_ns
|
||||
self._last_speed_mps = speed_mps
|
||||
self._last_category = category
|
||||
|
||||
if self._last_persist_timestamp_ns is None:
|
||||
self._last_persist_timestamp_ns = timestamp_ns
|
||||
elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
|
||||
self._persist(timestamp_ns=timestamp_ns)
|
||||
|
||||
return event
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
drive_distances = {
|
||||
category: self._distances_meters[category] - self._drive_start_distances_meters[category]
|
||||
for category in AssistCategory
|
||||
}
|
||||
summary_id = self._next_summary_id
|
||||
self._next_summary_id += 1
|
||||
self._persist(block=True)
|
||||
if self._enabled:
|
||||
self._store.save_drive_summary(summary_id, drive_distances, self._unit)
|
||||
|
||||
def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
|
||||
self._store.save(self.snapshot(), block=block)
|
||||
self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns
|
||||
@@ -1,123 +0,0 @@
|
||||
import unittest
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
|
||||
METERS_PER_MILE,
|
||||
AssistCategory,
|
||||
AssistedDrivingMilestones,
|
||||
MilestoneStore,
|
||||
MilestoneUnit,
|
||||
)
|
||||
|
||||
|
||||
class ParamsStub:
|
||||
def __init__(self, state=None):
|
||||
self.values = {"AssistedDrivingMilestoneState": state or {}}
|
||||
self.writes = []
|
||||
|
||||
def get(self, key, return_default=False):
|
||||
return self.values.get(key, {} if return_default else None)
|
||||
|
||||
def put(self, key, value, block=False):
|
||||
self.values[key] = value
|
||||
self.writes.append((key, value, block))
|
||||
|
||||
def flush(self):
|
||||
pass
|
||||
|
||||
|
||||
class TestAssistedDrivingMilestones(unittest.TestCase):
|
||||
def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
|
||||
params = ParamsStub({
|
||||
"version": 1,
|
||||
"distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
|
||||
"nextEventId": 7,
|
||||
"unit": "imperial",
|
||||
})
|
||||
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
|
||||
self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
|
||||
event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
|
||||
self.assertIsNotNone(event)
|
||||
assert event is not None
|
||||
self.assertEqual(event.event_id, 7)
|
||||
self.assertEqual(event.category, AssistCategory.MADS)
|
||||
self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
|
||||
self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
|
||||
self.assertFalse(params.writes[-1][2])
|
||||
|
||||
def test_switching_units_schedules_only_a_future_milestone(self):
|
||||
params = ParamsStub({
|
||||
"version": 1,
|
||||
"distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
|
||||
"nextEventId": 2,
|
||||
"unit": "imperial",
|
||||
})
|
||||
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
|
||||
self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
|
||||
event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
|
||||
|
||||
self.assertIsNotNone(event)
|
||||
assert event is not None
|
||||
self.assertEqual(event.unit, MilestoneUnit.METRIC)
|
||||
self.assertAlmostEqual(event.distance_meters, 10_000.0)
|
||||
|
||||
def test_ignores_disabled_reverse_and_timestamp_gaps(self):
|
||||
params = ParamsStub()
|
||||
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
|
||||
milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
|
||||
milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
|
||||
milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
|
||||
self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
|
||||
|
||||
def test_close_persists_totals_and_last_drive_summary(self):
|
||||
params = ParamsStub()
|
||||
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
|
||||
milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
|
||||
|
||||
milestones.close()
|
||||
|
||||
summary = params.values["LastDriveAssistedDrivingSummary"]
|
||||
self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
|
||||
self.assertTrue(params.writes[-1][2])
|
||||
|
||||
write_count = len(params.writes)
|
||||
milestones.close()
|
||||
self.assertEqual(len(params.writes), write_count)
|
||||
|
||||
def test_process_restart_preserves_the_current_drive_start(self):
|
||||
params = ParamsStub()
|
||||
first_process = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
first_process.set_drive_id("route-1")
|
||||
first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
first_process.close()
|
||||
|
||||
second_process = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
second_process.set_drive_id("route-1")
|
||||
second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
second_process.close()
|
||||
|
||||
summary = params.values["LastDriveAssistedDrivingSummary"]
|
||||
self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
|
||||
|
||||
def test_disabled_feature_does_not_publish_drive_summary(self):
|
||||
params = ParamsStub()
|
||||
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
|
||||
milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
|
||||
milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
|
||||
|
||||
milestones.close()
|
||||
|
||||
self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1383,12 +1383,6 @@
|
||||
"title": "Steering Arc",
|
||||
"description": "Display steering arc on the driving screen when lateral control is enabled."
|
||||
},
|
||||
{
|
||||
"key": "AssistedDrivingMilestonesEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Assisted Driving Milestones",
|
||||
"description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
|
||||
},
|
||||
{
|
||||
"key": "ShowTurnSignals",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -20,10 +20,6 @@ sections:
|
||||
widget: toggle
|
||||
title: Steering Arc
|
||||
description: Display steering arc on the driving screen when lateral control is enabled.
|
||||
- key: AssistedDrivingMilestonesEnabled
|
||||
widget: toggle
|
||||
title: Assisted Driving Milestones
|
||||
description: Celebrate cumulative MADS and full-assist distance milestones while driving.
|
||||
- key: ShowTurnSignals
|
||||
widget: toggle
|
||||
title: Display Turn Signals
|
||||
|
||||
@@ -99,32 +99,6 @@ def _migrate_model_bundle_slots(_params):
|
||||
cloudlog.exception(f"Error migrating model bundle slots: {e}")
|
||||
|
||||
|
||||
def _migrate_assisted_driving_milestones(_params):
|
||||
try:
|
||||
state = _params.get("AssistedDrivingMilestoneState", return_default=True)
|
||||
if isinstance(state, dict) and state.get("version") == 1:
|
||||
return
|
||||
|
||||
_params.put("AssistedDrivingMilestoneState", {
|
||||
"version": 1,
|
||||
"distancesMeters": {
|
||||
"mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
|
||||
"fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
|
||||
},
|
||||
"driveStartDistancesMeters": {
|
||||
"mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
|
||||
"fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
|
||||
},
|
||||
"nextEventId": 1,
|
||||
"nextSummaryId": 1,
|
||||
"unit": "metric" if _params.get_bool("IsMetric") else "imperial",
|
||||
"activeDriveId": "",
|
||||
}, block=True)
|
||||
cloudlog.info("params_migration: migrated assisted-driving milestone state")
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
|
||||
|
||||
|
||||
def run_migration(_params):
|
||||
# migrate OnroadScreenOffBrightness
|
||||
if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
|
||||
@@ -164,5 +138,3 @@ def run_migration(_params):
|
||||
|
||||
# seed the usbgpu model slot from the pre-split single slot
|
||||
_migrate_model_bundle_slots(_params)
|
||||
|
||||
_migrate_assisted_driving_milestones(_params)
|
||||
|
||||
@@ -7,44 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
|
||||
|
||||
|
||||
class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
|
||||
def test_preserves_prototype_distances_once(self):
|
||||
class ParamsStub:
|
||||
def __init__(self):
|
||||
self.values = {
|
||||
"MadsDrivenDistanceMeters": 123.0,
|
||||
"FullAssistDrivenDistanceMeters": 456.0,
|
||||
"OnroadScreenOffBrightness": 0,
|
||||
"OnroadScreenOffTimer": 15,
|
||||
"AssistedDrivingMilestoneState": {},
|
||||
"IsMetric": False,
|
||||
}
|
||||
|
||||
def get(self, key, return_default=False):
|
||||
return self.values.get(key)
|
||||
|
||||
def put(self, key, value, block=False):
|
||||
self.values[key] = value
|
||||
|
||||
def get_bool(self, key):
|
||||
return bool(self.values.get(key, False))
|
||||
|
||||
params = ParamsStub()
|
||||
|
||||
run_migration(params)
|
||||
|
||||
state = params.get("AssistedDrivingMilestoneState")
|
||||
assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
|
||||
|
||||
params.put("MadsDrivenDistanceMeters", 12.0, block=True)
|
||||
params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
|
||||
run_migration(params)
|
||||
|
||||
state = params.get("AssistedDrivingMilestoneState")
|
||||
assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
|
||||
from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
|
||||
|
||||
|
||||
class TestModelBundleSlotMigration(OpenpilotTestCase):
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
Reference in New Issue
Block a user