diff --git a/RELEASES.md b/RELEASES.md index a5527dacc..b0c101784 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Carrot2-v9 (2026-01-xx) +======================== +* WMI model +* Activate corner radar(HDA2) +* fix Angle Steering(HKG car) +* Keep blinker while LaneChange +* Speed based TF adjustment +* Sorento HEV 4WD(Niro HEV), Long bug fix. + Carrot2-v9 (2025-12-06) ======================== * DarkSouls model diff --git a/cereal/messaging/bridge.cc b/cereal/messaging/bridge.cc index 69ecd188e..22c6ef78a 100644 --- a/cereal/messaging/bridge.cc +++ b/cereal/messaging/bridge.cc @@ -1,4 +1,7 @@ #include +#include +#include +#include #include "cereal/messaging/msgq_to_zmq.h" #include "cereal/services.h" @@ -6,19 +9,47 @@ ExitHandler do_exit; -static std::vector get_services(const std::string &whitelist_str, bool zmq_to_msgq) { - std::vector service_list; - for (const auto& it : services) { - std::string name = it.second.name; - bool in_whitelist = whitelist_str.find(name) != std::string::npos; - if (zmq_to_msgq && !in_whitelist) { - continue; +static std::unordered_set parse_whitelist(const std::string& s) { + std::unordered_set out; + std::string token; + token.reserve(s.size()); + + auto flush = [&]() { + if (!token.empty()) { + out.insert(token); + token.clear(); } + }; + + for (char c : s) { + // 구분자: 콤마/공백/탭/세미콜론/파이프 등 + if (std::isspace((unsigned char)c) || c == ',' || c == ';' || c == '|') { + flush(); + } + else { + token.push_back(c); + } + } + flush(); + return out; +} + +static std::vector get_services(const std::string& whitelist_str, bool /*zmq_to_msgq*/) { + std::vector service_list; + + const bool use_filter = !whitelist_str.empty(); + std::unordered_set whitelist = use_filter ? parse_whitelist(whitelist_str) + : std::unordered_set{}; + + for (const auto& it : services) { + const std::string& name = it.second.name; + if (use_filter && whitelist.find(name) == whitelist.end()) continue; service_list.push_back(name); } return service_list; } + void msgq_to_zmq(const std::vector &endpoints, const std::string &ip) { MsgqToZmq bridge; bridge.run(endpoints, ip); @@ -57,9 +88,9 @@ void zmq_to_msgq(const std::vector &endpoints, const std::string &i } int main(int argc, char **argv) { - bool is_zmq_to_msgq = argc > 2; - std::string ip = is_zmq_to_msgq ? argv[1] : "127.0.0.1"; - std::string whitelist_str = is_zmq_to_msgq ? std::string(argv[2]) : ""; + bool is_zmq_to_msgq = argc > 3; + std::string ip = argc > 2 ? argv[1] : "127.0.0.1"; + std::string whitelist_str = argc > 2 ? std::string(argv[2]) : ""; std::vector endpoints = get_services(whitelist_str, is_zmq_to_msgq); if (is_zmq_to_msgq) { diff --git a/common/params.cc b/common/params.cc index d1e5a3646..6af00fe95 100644 --- a/common/params.cc +++ b/common/params.cc @@ -115,8 +115,16 @@ bool Params::checkKey(const std::string &key) { return keys.find(key) != keys.end(); } +ParamKeyFlag Params::getKeyFlag(const std::string &key) { + return static_cast(keys[key].flags); +} + ParamKeyType Params::getKeyType(const std::string &key) { - return static_cast(keys[key]); + return keys[key].type; +} + +std::optional Params::getKeyDefaultValue(const std::string &key) { + return keys[key].default_value; } int Params::put(const char* key, const char* value, size_t value_size) { @@ -140,7 +148,7 @@ int Params::put(const char* key, const char* value, size_t value_size) { } // fsync to force persist the changes. - if ((result = fsync(tmp_fd)) < 0) break; + if ((result = HANDLE_EINTR(fsync(tmp_fd))) < 0) break; FileLock file_lock(params_path + "/.lock"); @@ -195,17 +203,17 @@ std::map Params::readAll() { return util::read_files_in_dir(getParamPath()); } -void Params::clearAll(ParamKeyType key_type) { +void Params::clearAll(ParamKeyFlag key_flag) { FileLock file_lock(params_path + "/.lock"); - // 1) delete params of key_type + // 1) delete params of key_flag // 2) delete files that are not defined in the keys. if (DIR *d = opendir(getParamPath().c_str())) { struct dirent *de = NULL; while ((de = readdir(d))) { if (de->d_type != DT_DIR) { auto it = keys.find(de->d_name); - if (it == keys.end() || (it->second & key_type)) { + if (it == keys.end() || (it->second.flags & key_flag)) { unlink(getParamPath(de->d_name).c_str()); } } diff --git a/common/params.h b/common/params.h index 9c8af82c5..35b201a67 100644 --- a/common/params.h +++ b/common/params.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -9,16 +10,33 @@ #include "common/queue.h" -enum ParamKeyType { +enum ParamKeyFlag { PERSISTENT = 0x02, CLEAR_ON_MANAGER_START = 0x04, CLEAR_ON_ONROAD_TRANSITION = 0x08, CLEAR_ON_OFFROAD_TRANSITION = 0x10, DONT_LOG = 0x20, DEVELOPMENT_ONLY = 0x40, + CLEAR_ON_IGNITION_ON = 0x80, ALL = 0xFFFFFFFF }; +enum ParamKeyType { + STRING = 0, // must be utf-8 decodable + BOOL = 1, + INT = 2, + FLOAT = 3, + TIME = 4, // ISO 8601 + JSON = 5, + BYTES = 6 +}; + +struct ParamKeyAttributes { + uint32_t flags; + ParamKeyType type; + std::optional default_value = std::nullopt; +}; + class Params { public: explicit Params(const std::string &path = {}); @@ -29,14 +47,16 @@ public: std::vector allKeys() const; bool checkKey(const std::string &key); + ParamKeyFlag getKeyFlag(const std::string &key); ParamKeyType getKeyType(const std::string &key); + std::optional getKeyDefaultValue(const std::string &key); inline std::string getParamPath(const std::string &key = {}) { return params_path + params_prefix + (key.empty() ? "" : "/" + key); } // Delete a value int remove(const std::string &key); - void clearAll(ParamKeyType type); + void clearAll(ParamKeyFlag flag); // helpers for reading values std::string get(const std::string &key, bool block = false); diff --git a/common/params.py b/common/params.py index 66808083d..494617200 100644 --- a/common/params.py +++ b/common/params.py @@ -1,5 +1,6 @@ -from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName +from openpilot.common.params_pyx import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName assert Params +assert ParamKeyFlag assert ParamKeyType assert UnknownKeyName diff --git a/common/params_keys.h b/common/params_keys.h index 3654a6d7a..2a274bc3d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -3,303 +3,336 @@ #include #include -inline static std::unordered_map keys = { - {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, - {"AdbEnabled", PERSISTENT}, - {"AlwaysOnDM", PERSISTENT}, - {"ApiCache_Device", PERSISTENT}, - {"ApiCache_FirehoseStats", PERSISTENT}, - {"AssistNowToken", PERSISTENT}, - {"AthenadPid", PERSISTENT}, - {"AthenadUploadQueue", PERSISTENT}, - {"AthenadRecentlyViewedRoutes", PERSISTENT}, - {"BootCount", PERSISTENT}, - {"CalibrationParams", PERSISTENT}, - {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, - {"CameraDebugExpTime", CLEAR_ON_MANAGER_START}, - {"CarBatteryCapacity", PERSISTENT}, - {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"CarParamsCache", CLEAR_ON_MANAGER_START}, - {"CarParamsPersistent", PERSISTENT}, - {"CarParamsPrevRoute", PERSISTENT}, - {"CompletedTrainingVersion", PERSISTENT}, - {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"CurrentBootlog", PERSISTENT}, - {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"DisablePowerDown", PERSISTENT}, - {"DisableUpdates", PERSISTENT}, - {"DisengageOnAccelerator", PERSISTENT}, - {"DongleId", PERSISTENT}, - {"DoReboot", CLEAR_ON_MANAGER_START}, - {"DoShutdown", CLEAR_ON_MANAGER_START}, - {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"DriverTooDistracted", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"AlphaLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY}, - {"ExperimentalMode", PERSISTENT}, - {"ExperimentalModeConfirmed", PERSISTENT}, - {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"ForcePowerDown", PERSISTENT}, - {"GitBranch", PERSISTENT}, - {"GitCommit", PERSISTENT}, - {"GitCommitDate", PERSISTENT}, - {"GitDiff", PERSISTENT}, - {"GithubSshKeys", PERSISTENT}, - {"GithubUsername", PERSISTENT}, - {"GitRemote", PERSISTENT}, - {"GsmApn", PERSISTENT}, - {"GsmMetered", PERSISTENT}, - {"GsmRoaming", PERSISTENT}, - {"HardwareSerial", PERSISTENT}, - {"HasAcceptedTerms", PERSISTENT}, - {"InstallDate", PERSISTENT}, - {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, - {"IsEngaged", PERSISTENT}, - {"IsLdwEnabled", PERSISTENT}, - {"IsMetric", PERSISTENT}, - {"IsOffroad", CLEAR_ON_MANAGER_START}, - {"IsOnroad", PERSISTENT}, - {"IsRhdDetected", PERSISTENT}, - {"IsReleaseBranch", CLEAR_ON_MANAGER_START}, - {"IsTakingSnapshot", CLEAR_ON_MANAGER_START}, - {"IsTestedBranch", CLEAR_ON_MANAGER_START}, - {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"LanguageSetting", PERSISTENT}, - {"LastAthenaPingTime", CLEAR_ON_MANAGER_START}, - {"LastGPSPosition", PERSISTENT}, - {"LastManagerExitReason", CLEAR_ON_MANAGER_START}, - {"LastOffroadStatusPacket", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, - {"LastUpdateException", CLEAR_ON_MANAGER_START}, - {"LastUpdateTime", PERSISTENT}, - {"LiveDelay", PERSISTENT}, - {"LiveParameters", PERSISTENT}, - {"LiveParametersV2", PERSISTENT}, - {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, - {"LocationFilterInitialState", PERSISTENT}, - {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"LongitudinalPersonality", PERSISTENT}, - {"NetworkMetered", PERSISTENT}, - {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"Offroad_BadNvme", CLEAR_ON_MANAGER_START}, - {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, - {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, - {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, - {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, - {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"Offroad_Recalibration", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"Offroad_StorageMissing", CLEAR_ON_MANAGER_START}, - {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, - {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, - {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, - {"Offroad_DriverMonitoringUncertain", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"OpenpilotEnabledToggle", PERSISTENT}, - {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"PandaSignatures", CLEAR_ON_MANAGER_START}, - {"PrimeType", PERSISTENT}, - {"RecordAudio", PERSISTENT}, - {"RecordFront", PERSISTENT}, - {"RecordFrontLock", PERSISTENT}, // for the internal fleet - {"SecOCKey", PERSISTENT | DONT_LOG}, - {"RouteCount", PERSISTENT}, - {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"SshEnabled", PERSISTENT}, - {"TermsVersion", PERSISTENT}, - {"TrainingVersion", PERSISTENT}, - {"UbloxAvailable", PERSISTENT}, - {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, - {"UpdaterAvailableBranches", PERSISTENT}, - {"UpdaterCurrentDescription", CLEAR_ON_MANAGER_START}, - {"UpdaterCurrentReleaseNotes", CLEAR_ON_MANAGER_START}, - {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, - {"UpdaterNewDescription", CLEAR_ON_MANAGER_START}, - {"UpdaterNewReleaseNotes", CLEAR_ON_MANAGER_START}, - {"UpdaterState", CLEAR_ON_MANAGER_START}, - {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, - {"UpdaterLastFetchTime", PERSISTENT}, - {"Version", PERSISTENT}, +#include "cereal/gen/cpp/log.capnp.h" + +inline static std::unordered_map keys = { + {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}}, + {"AdbEnabled", {PERSISTENT, BOOL}}, + {"AlwaysOnDM", {PERSISTENT, BOOL}}, + {"ApiCache_Device", {PERSISTENT, STRING}}, + {"ApiCache_FirehoseStats", {PERSISTENT, JSON}}, + {"AssistNowToken", {PERSISTENT, STRING}}, + {"AthenadPid", {PERSISTENT, INT}}, + {"AthenadUploadQueue", {PERSISTENT, JSON}}, + {"AthenadRecentlyViewedRoutes", {PERSISTENT, STRING}}, + {"BootCount", {PERSISTENT, INT}}, + {"CalibrationParams", {PERSISTENT, BYTES}}, + {"CameraDebugExpGain", {CLEAR_ON_MANAGER_START, STRING}}, + {"CameraDebugExpTime", {CLEAR_ON_MANAGER_START, STRING}}, + {"CarBatteryCapacity", {PERSISTENT, INT}}, + {"CarParams", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BYTES}}, + {"CarParamsCache", {CLEAR_ON_MANAGER_START, BYTES}}, + {"CarParamsPersistent", {PERSISTENT, BYTES}}, + {"CarParamsPrevRoute", {PERSISTENT, BYTES}}, + {"CompletedTrainingVersion", {PERSISTENT, STRING, "0"}}, + {"ControlsReady", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"CurrentBootlog", {PERSISTENT, STRING}}, + {"CurrentRoute", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, STRING}}, + {"DisableLogging", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"DisablePowerDown", {PERSISTENT, BOOL}}, + {"DisableUpdates", {PERSISTENT, BOOL}}, + {"DisengageOnAccelerator", {PERSISTENT, BOOL, "0"}}, + {"DongleId", {PERSISTENT, STRING}}, + {"DoReboot", {CLEAR_ON_MANAGER_START, BOOL}}, + {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, + {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, + {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, + {"AlphaLongitudinalEnabled", {PERSISTENT | DEVELOPMENT_ONLY, BOOL}}, + {"ExperimentalMode", {PERSISTENT, BOOL}}, + {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, + {"FirmwareQueryDone", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"ForcePowerDown", {PERSISTENT, BOOL}}, + {"GitBranch", {PERSISTENT, STRING}}, + {"GitCommit", {PERSISTENT, STRING}}, + {"GitCommitDate", {PERSISTENT, STRING}}, + {"GitDiff", {PERSISTENT, STRING}}, + {"GithubSshKeys", {PERSISTENT, STRING}}, + {"GithubUsername", {PERSISTENT, STRING}}, + {"GitRemote", {PERSISTENT, STRING}}, + {"GsmApn", {PERSISTENT, STRING}}, + {"GsmMetered", {PERSISTENT, BOOL, "1"}}, + {"GsmRoaming", {PERSISTENT, BOOL}}, + {"HardwareSerial", {PERSISTENT, STRING}}, + {"HasAcceptedTerms", {PERSISTENT, STRING, "0"}}, + {"InstallDate", {PERSISTENT, TIME}}, + {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}}, + {"IsEngaged", {PERSISTENT, BOOL}}, + {"IsLdwEnabled", {PERSISTENT, BOOL}}, + {"IsMetric", {PERSISTENT, BOOL}}, + {"IsOffroad", {CLEAR_ON_MANAGER_START, BOOL}}, + {"IsOnroad", {PERSISTENT, BOOL}}, + {"IsRhdDetected", {PERSISTENT, BOOL}}, + {"IsReleaseBranch", {CLEAR_ON_MANAGER_START, BOOL}}, + {"IsTakingSnapshot", {CLEAR_ON_MANAGER_START, BOOL}}, + {"IsTestedBranch", {CLEAR_ON_MANAGER_START, BOOL}}, + {"JoystickDebugMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"LanguageSetting", {PERSISTENT, STRING, "en"}}, + {"LastAthenaPingTime", {CLEAR_ON_MANAGER_START, INT}}, + {"LastGPSPosition", {PERSISTENT, STRING}}, + {"LastManagerExitReason", {CLEAR_ON_MANAGER_START, STRING}}, + {"LastOffroadStatusPacket", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}}, + {"LastPowerDropDetected", {CLEAR_ON_MANAGER_START, STRING}}, + {"LastUpdateException", {CLEAR_ON_MANAGER_START, STRING}}, + {"LastUpdateTime", {PERSISTENT, TIME}}, + {"LiveDelay", {PERSISTENT, BYTES}}, + {"LiveParameters", {PERSISTENT, JSON}}, + {"LiveParametersV2", {PERSISTENT, BYTES}}, + {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}}, + {"LocationFilterInitialState", {PERSISTENT, BYTES}}, + {"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast(cereal::LongitudinalPersonality::STANDARD))}}, + {"NetworkMetered", {PERSISTENT, BOOL}}, + {"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"Offroad_BadNvme", {CLEAR_ON_MANAGER_START, BOOL}}, + {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_IsTakingSnapshot", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_NeosUpdate", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_NoFirmware", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_Recalibration", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_StorageMissing", {CLEAR_ON_MANAGER_START, BOOL}}, + {"Offroad_TemperatureTooHigh", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_UnofficialHardware", {CLEAR_ON_MANAGER_START, BOOL}}, + {"Offroad_UpdateFailed", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_DriverMonitoringUncertain", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"OpenpilotEnabledToggle", {PERSISTENT, BOOL, "1"}}, + {"PandaHeartbeatLost", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"PandaSomResetTriggered", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"PandaSignatures", {CLEAR_ON_MANAGER_START, BYTES}}, + {"PrimeType", {PERSISTENT, INT}}, + {"RecordAudio", {PERSISTENT, BOOL}}, + {"RecordFront", {PERSISTENT, BOOL}}, + {"RecordFrontLock", {PERSISTENT, BOOL}}, // for the internal fleet + {"SecOCKey", {PERSISTENT | DONT_LOG, STRING}}, + {"RouteCount", {PERSISTENT, INT, "0"}}, + {"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"SshEnabled", {PERSISTENT, BOOL}}, + {"TermsVersion", {PERSISTENT, STRING}}, + {"TrainingVersion", {PERSISTENT, STRING}}, + {"UbloxAvailable", {PERSISTENT, BOOL}}, + {"UpdateAvailable", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, + {"UpdateFailedCount", {CLEAR_ON_MANAGER_START, INT}}, + {"UpdaterAvailableBranches", {PERSISTENT, STRING}}, + {"UpdaterCurrentDescription", {CLEAR_ON_MANAGER_START, STRING}}, + {"UpdaterCurrentReleaseNotes", {CLEAR_ON_MANAGER_START, BYTES}}, + {"UpdaterFetchAvailable", {CLEAR_ON_MANAGER_START, BOOL}}, + {"UpdaterNewDescription", {CLEAR_ON_MANAGER_START, STRING}}, + {"UpdaterNewReleaseNotes", {CLEAR_ON_MANAGER_START, BYTES}}, + {"UpdaterState", {CLEAR_ON_MANAGER_START, STRING}}, + {"UpdaterTargetBranch", {CLEAR_ON_MANAGER_START, STRING}}, + {"UpdaterLastFetchTime", {PERSISTENT, TIME}}, + {"Version", {PERSISTENT, STRING}}, // carrot - {"LongitudinalPersonalityMax", PERSISTENT}, - {"NetworkAddress", CLEAR_ON_MANAGER_START}, + {"LongitudinalPersonalityMax", {PERSISTENT, INT, "3"}}, + {"NetworkAddress", {CLEAR_ON_MANAGER_START, STRING}}, - {"ApiCache_NavDestinations", PERSISTENT}, - {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"NavPastDestinations", PERSISTENT}, - {"NavSettingLeftSide", PERSISTENT}, - {"NavSettingTime24h", PERSISTENT}, - {"MapboxStyle", PERSISTENT }, - {"MapboxPublicKey", PERSISTENT}, - {"MapboxSecretKey", PERSISTENT}, - {"GMapKey", PERSISTENT}, - {"SearchInput", PERSISTENT}, + {"ApiCache_NavDestinations", {PERSISTENT, STRING}}, + {"NavDestination", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, STRING}}, + {"NavDestinationWaypoints", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, STRING}}, + {"NavPastDestinations", {PERSISTENT, STRING}}, + {"NavSettingLeftSide", {PERSISTENT, BOOL}}, + {"NavSettingTime24h", {PERSISTENT, BOOL}}, + {"MapboxStyle", {PERSISTENT, INT}}, + {"MapboxPublicKey", {PERSISTENT, STRING}}, + {"MapboxSecretKey", {PERSISTENT, STRING}}, + {"GMapKey", {PERSISTENT, STRING}}, + {"SearchInput", {PERSISTENT, INT}}, - {"CarSelected3", PERSISTENT}, - {"SupportedCars", PERSISTENT}, - {"SupportedCars_gm", PERSISTENT}, - {"ShowDebugUI", PERSISTENT}, - {"ShowTpms", PERSISTENT}, - {"ShowDateTime", PERSISTENT}, - {"ShowPathEnd", PERSISTENT}, - {"ShowCustomBrightness", PERSISTENT}, - {"ShowLaneInfo", PERSISTENT}, - {"ShowRadarInfo", PERSISTENT}, - {"ShowDeviceState", PERSISTENT}, - {"ShowRouteInfo", PERSISTENT }, - {"ShowPathMode", PERSISTENT}, - {"ShowPathColor", PERSISTENT}, - {"ShowPathColorCruiseOff", PERSISTENT}, - {"ShowPathModeLane", PERSISTENT}, - {"ShowPathColorLane", PERSISTENT}, - {"ShowPlotMode", PERSISTENT}, - {"RecordRoadCam", PERSISTENT }, - {"HDPuse", PERSISTENT }, + {"CarSelected3", {PERSISTENT, STRING}}, + {"SupportedCars", {PERSISTENT, STRING}}, + {"SupportedCars_gm", {PERSISTENT, STRING}}, + {"ShowDebugUI", {PERSISTENT, INT, "0"}}, + {"ShowTpms", {PERSISTENT, INT, "1"}}, + {"ShowDateTime", {PERSISTENT, INT, "1"}}, + {"ShowPathEnd", {PERSISTENT, INT, "1"}}, + {"ShowCustomBrightness", {PERSISTENT, INT, "100"}}, + {"ShowLaneInfo", {PERSISTENT, INT, "1"}}, + {"ShowRadarInfo", {PERSISTENT, INT, "1"}}, + {"ShowDeviceState", {PERSISTENT, INT, "1"}}, + {"ShowRouteInfo", {PERSISTENT, INT, "1"}}, + {"ShowPathMode", {PERSISTENT, INT, "9"}}, + {"ShowPathColor", {PERSISTENT, INT, "13"}}, + {"ShowPathColorCruiseOff", {PERSISTENT, INT, "19"}}, + {"ShowPathModeLane", {PERSISTENT, INT, "14"}}, + {"ShowPathColorLane", {PERSISTENT, INT, "13"}}, + {"ShowPlotMode", {PERSISTENT, INT, "0"}}, + {"RecordRoadCam", {PERSISTENT, INT, "0"}}, + {"HDPuse", {PERSISTENT, INT, "0"}}, - {"AutoCruiseControl", PERSISTENT}, - {"CruiseEcoControl", PERSISTENT}, - {"CarrotCruiseDecel", PERSISTENT}, - {"CarrotCruiseAtcDecel", PERSISTENT}, - {"CommaLongAcc", PERSISTENT}, - {"AutoGasTokSpeed", PERSISTENT}, - {"AutoGasSyncSpeed", PERSISTENT}, - {"AutoEngage", PERSISTENT}, - {"DisableMinSteerSpeed", PERSISTENT}, - {"AutoCurveSpeedLowerLimit", PERSISTENT}, - {"AutoCurveSpeedFactor", PERSISTENT}, - {"AutoCurveSpeedAggressiveness", PERSISTENT}, - {"AutoTurnControl", PERSISTENT}, - {"AutoTurnControlSpeedTurn", PERSISTENT}, - {"AutoTurnControlTurnEnd", PERSISTENT}, - {"AutoTurnMapChange", PERSISTENT }, - {"AutoNaviSpeedCtrlEnd", PERSISTENT}, - {"AutoNaviSpeedCtrlMode", PERSISTENT}, - {"AutoRoadSpeedLimitOffset", PERSISTENT}, - {"AutoNaviSpeedBumpTime", PERSISTENT}, - {"AutoNaviSpeedBumpSpeed", PERSISTENT}, - {"AutoNaviSpeedDecelRate", PERSISTENT}, - {"AutoNaviSpeedSafetyFactor", PERSISTENT}, - {"AutoNaviCountDownMode", PERSISTENT}, - {"TurnSpeedControlMode", PERSISTENT}, - {"CarrotSmartSpeedControl", PERSISTENT}, - {"MapTurnSpeedFactor", PERSISTENT}, - {"ModelTurnSpeedFactor", PERSISTENT}, - {"StoppingAccel", PERSISTENT}, - {"AutoSpeedUptoRoadSpeedLimit", PERSISTENT}, - {"AutoRoadSpeedAdjust", PERSISTENT}, - {"StopDistanceCarrot", PERSISTENT}, - {"ComfortBrake", PERSISTENT}, - {"JLeadFactor3", PERSISTENT}, - {"CruiseButtonMode", PERSISTENT}, - {"CancelButtonMode", PERSISTENT}, - {"LfaButtonMode", PERSISTENT}, - {"CruiseButtonTest1", PERSISTENT}, - {"CruiseButtonTest2", PERSISTENT}, - {"CruiseButtonTest3", PERSISTENT}, - {"CruiseSpeedUnit", PERSISTENT}, - {"CruiseSpeedUnitBasic", PERSISTENT}, - {"CruiseSpeed1", PERSISTENT}, - {"CruiseSpeed2", PERSISTENT}, - {"CruiseSpeed3", PERSISTENT}, - {"CruiseSpeed4", PERSISTENT}, - {"CruiseSpeed5", PERSISTENT}, - {"PaddleMode", PERSISTENT}, - {"MyDrivingMode", PERSISTENT}, - {"MyDrivingModeAuto", PERSISTENT}, - {"TrafficLightDetectMode", PERSISTENT}, - {"SteerActuatorDelay", PERSISTENT}, - {"LatSmoothSec", PERSISTENT}, - {"CruiseOnDist", PERSISTENT}, - {"CruiseMaxVals0", PERSISTENT}, - {"CruiseMaxVals1", PERSISTENT}, - {"CruiseMaxVals2", PERSISTENT}, - {"CruiseMaxVals3", PERSISTENT}, - {"CruiseMaxVals4", PERSISTENT}, - {"CruiseMaxVals5", PERSISTENT}, - {"CruiseMaxVals6", PERSISTENT}, - {"LongTuningKpV", PERSISTENT}, - {"LongTuningKiV", PERSISTENT}, - {"LongTuningKf", PERSISTENT}, - {"LongActuatorDelay", PERSISTENT }, - {"VEgoStopping", PERSISTENT }, - {"RadarReactionFactor", PERSISTENT}, - {"EnableRadarTracks", PERSISTENT}, - {"RadarLatFactor", PERSISTENT}, - {"EnableCornerRadar", PERSISTENT}, - {"EnableRadarTracksResult", PERSISTENT | CLEAR_ON_MANAGER_START}, - {"CanParserResult", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION }, - {"HotspotOnBoot", PERSISTENT}, - {"SoftwareMenu", PERSISTENT}, - {"HyundaiCameraSCC", PERSISTENT}, - {"FingerPrints", PERSISTENT | CLEAR_ON_MANAGER_START}, - {"IsLdwsCar", PERSISTENT}, - {"CanfdHDA2", PERSISTENT}, - {"CanfdDebug", PERSISTENT}, - {"SoundVolumeAdjust", PERSISTENT}, - {"SoundVolumeAdjustEngage", PERSISTENT}, - {"TFollowGap1", PERSISTENT}, - {"TFollowGap2", PERSISTENT}, - {"TFollowGap3", PERSISTENT}, - {"TFollowGap4", PERSISTENT}, - {"DynamicTFollow", PERSISTENT}, - {"DynamicTFollowLC", PERSISTENT}, - {"AChangeCostStarting", PERSISTENT}, - {"TrafficStopDistanceAdjust", PERSISTENT}, - {"HapticFeedbackWhenSpeedCamera", PERSISTENT}, - {"UseLaneLineSpeed", PERSISTENT}, - {"UseLaneLineCurveSpeed", PERSISTENT}, - {"AdjustLaneOffset", PERSISTENT}, - {"LaneChangeNeedTorque", PERSISTENT}, - {"LaneChangeDelay", PERSISTENT }, - {"LaneChangeBsd", PERSISTENT}, - {"MaxAngleFrames", PERSISTENT}, - {"SoftHoldMode", PERSISTENT}, - {"LatMpcPathCost", PERSISTENT}, - {"LatMpcMotionCost", PERSISTENT}, - {"LatMpcAccelCost", PERSISTENT}, - {"LatMpcJerkCost", PERSISTENT}, - {"LatMpcSteeringRateCost", PERSISTENT }, - {"LatMpcInputOffset", PERSISTENT}, - {"PathOffset", PERSISTENT}, - {"LateralTorqueCustom", PERSISTENT}, - {"LateralTorqueAccelFactor", PERSISTENT}, - {"LateralTorqueFriction", PERSISTENT}, - {"LateralTorqueKpV", PERSISTENT}, - {"LateralTorqueKiV", PERSISTENT}, - {"LateralTorqueKf", PERSISTENT}, - {"LateralTorqueKd", PERSISTENT}, - {"CustomSteerMax", PERSISTENT}, - {"CustomSteerDeltaUp", PERSISTENT}, - {"CustomSteerDeltaDown", PERSISTENT}, - {"CustomSteerDeltaUpLC", PERSISTENT}, - {"CustomSteerDeltaDownLC", PERSISTENT}, - {"SpeedFromPCM", PERSISTENT}, - {"MaxTimeOffroadMin", PERSISTENT}, - {"DisableDM", PERSISTENT}, - {"EnableConnect", PERSISTENT}, - {"MuteDoor", PERSISTENT}, - {"MuteSeatbelt", PERSISTENT}, - {"CarrotException", CLEAR_ON_MANAGER_START}, - {"CarrotSpeed", PERSISTENT}, - {"CarrotSpeedViz", PERSISTENT}, - {"CarrotSpeedTable", PERSISTENT}, - {"CarName", PERSISTENT}, - {"EVTable", PERSISTENT}, - {"LongPitch", PERSISTENT}, - {"ActivateCruiseAfterBrake", CLEAR_ON_MANAGER_START}, // for GM autoResume - {"CustomSR", PERSISTENT}, - {"SteerRatioRate", PERSISTENT}, - {"SoftRestartTriggered", CLEAR_ON_MANAGER_START}, + {"AutoCruiseControl", {PERSISTENT, INT, "0"}}, + {"CruiseEcoControl", {PERSISTENT, INT, "2"}}, + {"CarrotCruiseDecel", {PERSISTENT, INT, "-1"}}, + {"CarrotCruiseAtcDecel", {PERSISTENT, INT, "-1"}}, + {"CommaLongAcc", {PERSISTENT, INT, "0"}}, - {"DevicePosition", CLEAR_ON_MANAGER_START}, - {"NNFF", PERSISTENT}, - {"NNFFLite", PERSISTENT}, - {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION}, + {"AutoGasTokSpeed", {PERSISTENT, INT, "0"}}, + {"AutoGasSyncSpeed", {PERSISTENT, INT, "1"}}, + {"AutoEngage", {PERSISTENT, INT, "0"}}, + {"DisableMinSteerSpeed", {PERSISTENT, INT, "0"}}, + {"AutoCurveSpeedLowerLimit", {PERSISTENT, INT, "30"}}, + {"AutoCurveSpeedFactor", {PERSISTENT, INT, "120"}}, + {"AutoCurveSpeedAggressiveness", {PERSISTENT, INT, "100"}}, - {"HardwareC3xLite", PERSISTENT}, - {"ShareData", PERSISTENT} + {"AutoTurnControl", {PERSISTENT, INT, "0"}}, + {"AutoTurnControlSpeedTurn", {PERSISTENT, INT, "20"}}, + {"AutoTurnControlTurnEnd", {PERSISTENT, INT, "6"}}, + {"AutoTurnMapChange", {PERSISTENT, INT, "0"}}, + + {"AutoNaviSpeedCtrlEnd", {PERSISTENT, INT, "7"}}, + {"AutoNaviSpeedCtrlMode", {PERSISTENT, INT, "2"}}, + {"AutoRoadSpeedLimitOffset", {PERSISTENT, INT, "-1"}}, + {"AutoNaviSpeedBumpTime", {PERSISTENT, INT, "1"}}, + {"AutoNaviSpeedBumpSpeed", {PERSISTENT, INT, "35"}}, + {"AutoNaviSpeedDecelRate", {PERSISTENT, INT, "120"}}, + {"AutoNaviSpeedSafetyFactor", {PERSISTENT, INT, "105"}}, + {"AutoNaviCountDownMode", {PERSISTENT, INT, "2"}}, + {"TurnSpeedControlMode", {PERSISTENT, INT, "1"}}, + + {"CarrotSmartSpeedControl", {PERSISTENT, INT, "0"}}, + {"MapTurnSpeedFactor", {PERSISTENT, INT, "90"}}, + {"ModelTurnSpeedFactor", {PERSISTENT, INT, "0"}}, + {"StoppingAccel", {PERSISTENT, INT, "0"}}, + {"AutoSpeedUptoRoadSpeedLimit", {PERSISTENT, INT, "0"}}, + {"AutoRoadSpeedAdjust", {PERSISTENT, INT, "50"}}, + + {"StopDistanceCarrot", {PERSISTENT, INT, "550"}}, + {"JLeadFactor3", {PERSISTENT, INT, "0"}}, + {"CruiseButtonMode", {PERSISTENT, INT, "0"}}, + {"CancelButtonMode", {PERSISTENT, INT, "0"}}, + {"LfaButtonMode", {PERSISTENT, INT, "0"}}, + {"CruiseButtonTest1", {PERSISTENT, INT, "8"}}, + {"CruiseButtonTest2", {PERSISTENT, INT, "30"}}, + {"CruiseButtonTest3", {PERSISTENT, INT, "1"}}, + + {"CruiseSpeedUnit", {PERSISTENT, INT, "10"}}, + {"CruiseSpeedUnitBasic", {PERSISTENT, INT, "1"}}, + {"CruiseSpeed1", {PERSISTENT, INT, "30"}}, + {"CruiseSpeed2", {PERSISTENT, INT, "50"}}, + {"CruiseSpeed3", {PERSISTENT, INT, "80"}}, + {"CruiseSpeed4", {PERSISTENT, INT, "110"}}, + {"CruiseSpeed5", {PERSISTENT, INT, "130"}}, + + {"PaddleMode", {PERSISTENT, INT, "0"}}, + {"MyDrivingMode", {PERSISTENT, INT, "3"}}, + {"MyDrivingModeAuto", {PERSISTENT, INT, "0"}}, + {"TrafficLightDetectMode", {PERSISTENT, INT, "2"}}, + + {"SteerActuatorDelay", {PERSISTENT, INT, "0"}}, + {"LatSmoothSec", {PERSISTENT, INT, "13"}}, + {"CruiseOnDist", {PERSISTENT, INT, "400"}}, + + {"CruiseMaxVals0", {PERSISTENT, INT, "160"}}, + {"CruiseMaxVals1", {PERSISTENT, INT, "200"}}, + {"CruiseMaxVals2", {PERSISTENT, INT, "160"}}, + {"CruiseMaxVals3", {PERSISTENT, INT, "130"}}, + {"CruiseMaxVals4", {PERSISTENT, INT, "110"}}, + {"CruiseMaxVals5", {PERSISTENT, INT, "95"}}, + {"CruiseMaxVals6", {PERSISTENT, INT, "80"}}, + + {"LongTuningKpV", {PERSISTENT, INT, "100"}}, + {"LongTuningKiV", {PERSISTENT, INT, "0"}}, + {"LongTuningKf", {PERSISTENT, INT, "100"}}, + {"LongActuatorDelay", {PERSISTENT, INT, "20"}}, + {"VEgoStopping", {PERSISTENT, INT, "50"}}, + + {"RadarReactionFactor", {PERSISTENT, INT, "100"}}, + {"EnableRadarTracks", {PERSISTENT, INT, "0"}}, + {"RadarLatFactor", {PERSISTENT, INT, "0"}}, + {"EnableCornerRadar", {PERSISTENT, INT, "0"}}, + + {"EnableRadarTracksResult", {PERSISTENT | CLEAR_ON_MANAGER_START, INT}}, + {"CanParserResult", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, STRING}}, + + {"HotspotOnBoot", {PERSISTENT, INT, "0"}}, + {"SoftwareMenu", {PERSISTENT, INT, "1"}}, + + {"HyundaiCameraSCC", {PERSISTENT, INT, "0"}}, + {"FingerPrints", {PERSISTENT | CLEAR_ON_MANAGER_START, STRING}}, + {"IsLdwsCar", {PERSISTENT, INT, "0"}}, + {"CanfdHDA2", {PERSISTENT, INT, "0"}}, + {"CanfdDebug", {PERSISTENT, INT, "0"}}, + + {"SoundVolumeAdjust", {PERSISTENT, INT, "100"}}, + {"SoundVolumeAdjustEngage", {PERSISTENT, INT, "10"}}, + + {"TFollowGap1", {PERSISTENT, INT, "110"}}, + {"TFollowGap2", {PERSISTENT, INT, "120"}}, + {"TFollowGap3", {PERSISTENT, INT, "140"}}, + {"TFollowGap4", {PERSISTENT, INT, "160"}}, + + {"DynamicTFollow", {PERSISTENT, INT, "0"}}, + {"DynamicTFollowLC", {PERSISTENT, INT, "100"}}, + {"EnableSpeedTF", {PERSISTENT, INT, "0"}}, + {"AChangeCostStarting", {PERSISTENT, INT, "10"}}, + {"TrafficStopDistanceAdjust", {PERSISTENT, INT, "400"}}, + + {"HapticFeedbackWhenSpeedCamera", {PERSISTENT, INT, "0"}}, + {"UseLaneLineSpeed", {PERSISTENT, INT, "0"}}, + {"UseLaneLineCurveSpeed", {PERSISTENT, INT, "0"}}, + + {"AdjustLaneOffset", {PERSISTENT, INT, "0"}}, + {"LaneChangeNeedTorque", {PERSISTENT, INT, "0"}}, + {"LaneChangeDelay", {PERSISTENT, INT, "0"}}, + {"LaneChangeBsd", {PERSISTENT, INT, "0"}}, + {"MaxAngleFrames", {PERSISTENT, INT, "89"}}, + + {"SoftHoldMode", {PERSISTENT, INT, "0"}}, + + {"LatMpcPathCost", {PERSISTENT, INT, "200"}}, + {"LatMpcMotionCost", {PERSISTENT, INT, "7"}}, + {"LatMpcAccelCost", {PERSISTENT, INT, "120"}}, + {"LatMpcJerkCost", {PERSISTENT, INT, "4"}}, + {"LatMpcSteeringRateCost", {PERSISTENT, INT, "7"}}, + {"LatMpcInputOffset", {PERSISTENT, INT, "4"}}, + + {"PathOffset", {PERSISTENT, INT, "0"}}, + + {"LateralTorqueCustom", {PERSISTENT, INT, "0"}}, + {"LateralTorqueAccelFactor", {PERSISTENT, INT, "2500"}}, + {"LateralTorqueFriction", {PERSISTENT, INT, "100"}}, + {"LateralTorqueKpV", {PERSISTENT, INT, "100"}}, + {"LateralTorqueKiV", {PERSISTENT, INT, "10"}}, + {"LateralTorqueKf", {PERSISTENT, INT, "100"}}, + {"LateralTorqueKd", {PERSISTENT, INT, "0"}}, + + {"CustomSteerMax", {PERSISTENT, INT, "0"}}, + {"CustomSteerDeltaUp", {PERSISTENT, INT, "0"}}, + {"CustomSteerDeltaDown", {PERSISTENT, INT, "0"}}, + {"CustomSteerDeltaUpLC", {PERSISTENT, INT, "0"}}, + {"CustomSteerDeltaDownLC", {PERSISTENT, INT, "0"}}, + + {"SpeedFromPCM", {PERSISTENT, INT, "2"}}, + {"MaxTimeOffroadMin", {PERSISTENT, INT, "60"}}, + + {"DisableDM", {PERSISTENT, INT, "0"}}, + {"EnableConnect", {PERSISTENT, INT, "0"}}, + {"MuteDoor", {PERSISTENT, INT, "0"}}, + {"MuteSeatbelt", {PERSISTENT, INT, "0"}}, + + {"CarrotException", {CLEAR_ON_MANAGER_START, STRING}}, + + {"CarrotSpeed", {PERSISTENT, INT} }, + {"CarrotSpeedViz", {PERSISTENT, JSON} }, + {"CarrotSpeedTable", {PERSISTENT, BYTES} }, + {"CarName", {PERSISTENT, STRING}}, + {"EVTable", {PERSISTENT, BOOL, "0"}}, + {"LongPitch", {PERSISTENT, BOOL, "0"}}, + + {"ActivateCruiseAfterBrake", {CLEAR_ON_MANAGER_START, INT, "0"}}, + + {"CustomSR", {PERSISTENT, INT, "0"}}, + {"SteerRatioRate", {PERSISTENT, INT, "100"}}, + + {"SoftRestartTriggered", {CLEAR_ON_MANAGER_START, INT}}, + + {"DevicePosition", {CLEAR_ON_MANAGER_START, STRING}}, + {"NNFF", {PERSISTENT, INT, "0"}}, + {"NNFFLite", {PERSISTENT, INT, "0"}}, + {"NNFFModelName", {CLEAR_ON_OFFROAD_TRANSITION, STRING}}, + + {"HardwareC3xLite", {PERSISTENT, INT, "0"}}, + {"ShareData", {PERSISTENT, INT, "0"}}, }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 824a774f8..10ca3f9b5 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -1,18 +1,34 @@ # distutils: language = c++ # cython: language_level = 3 +import builtins +import datetime +import json from libcpp cimport bool from libcpp.string cimport string from libcpp.vector cimport vector +from libcpp.optional cimport optional + +from openpilot.common.swaglog import cloudlog cdef extern from "common/params.h": - cpdef enum ParamKeyType: + cpdef enum ParamKeyFlag: PERSISTENT CLEAR_ON_MANAGER_START CLEAR_ON_ONROAD_TRANSITION CLEAR_ON_OFFROAD_TRANSITION DEVELOPMENT_ONLY + CLEAR_ON_IGNITION_ON ALL + cpdef enum ParamKeyType: + STRING + BOOL + INT + FLOAT + TIME + JSON + BYTES + cdef cppclass c_Params "Params": c_Params(string) except + nogil string get(string, bool) nogil @@ -29,10 +45,31 @@ cdef extern from "common/params.h": int putInt(string, int) nogil int putFloat(string, float) nogil bool checkKey(string) nogil + ParamKeyType getKeyType(string) nogil + optional[string] getKeyDefaultValue(string) nogil string getParamPath(string) nogil - void clearAll(ParamKeyType) + void clearAll(ParamKeyFlag) vector[string] allKeys() +PYTHON_2_CPP = { + (str, STRING): lambda v: v, + (builtins.bool, BOOL): lambda v: "1" if v else "0", + (int, INT): str, + (float, FLOAT): str, + (datetime.datetime, TIME): lambda v: v.isoformat(), + (dict, JSON): json.dumps, + (list, JSON): json.dumps, + (bytes, BYTES): lambda v: v, +} +CPP_2_PYTHON = { + STRING: lambda v: v.decode("utf-8"), + BOOL: lambda v: v == b"1", + INT: int, + FLOAT: float, + TIME: lambda v: datetime.datetime.fromisoformat(v.decode("utf-8")), + JSON: json.loads, + BYTES: lambda v: v, +} def ensure_bytes(v): return v.encode() if isinstance(v, str) else v @@ -56,8 +93,8 @@ cdef class Params: def __dealloc__(self): del self.p - def clear_all(self, tx_type=ParamKeyType.ALL): - self.p.clearAll(tx_type) + def clear_all(self, tx_flag=ParamKeyFlag.ALL): + self.p.clearAll(tx_flag) def check_key(self, key): key = ensure_bytes(key) @@ -65,21 +102,38 @@ cdef class Params: raise UnknownKeyName(key) return key - def get(self, key, bool block=False, encoding=None): + def python2cpp(self, proposed_type, expected_type, value, key): + cast = PYTHON_2_CPP.get((proposed_type, expected_type)) + if cast: + return cast(value) + raise TypeError(f"Type mismatch while writing param {key}: {proposed_type=} {expected_type=} {value=}") + + def _cpp2python(self, t, value, default, key): + if value is None: + return None + try: + return CPP_2_PYTHON[t](value) + except (KeyError, TypeError, ValueError): + cloudlog.warning(f"Failed to cast param {key} with {value=} from type {t=}") + return self._cpp2python(t, default, None, key) + + def get(self, key, bool block=False, bool return_default=False): cdef string k = self.check_key(key) + cdef ParamKeyType t = self.p.getKeyType(k) + cdef optional[string] default = self.p.getKeyDefaultValue(k) cdef string val with nogil: val = self.p.get(k, block) + default_val = (default.value() if default.has_value() else None) if return_default else None if val == b"": if block: # If we got no value while running in blocked mode # it means we got an interrupt while waiting raise KeyboardInterrupt else: - return None - - return val if encoding is None else val.decode(encoding) + return self._cpp2python(t, default_val, None, key) + return self._cpp2python(t, val, default_val, key) def get_bool(self, key, bool block=False): cdef string k = self.check_key(key) @@ -88,6 +142,11 @@ cdef class Params: r = self.p.getBool(k, block) return r + def _put_cast(self, key, dat): + cdef string k = self.check_key(key) + cdef ParamKeyType t = self.p.getKeyType(k) + return ensure_bytes(self.python2cpp(type(dat), t, dat, key)) + def get_int(self, key, bool block=False): cdef string k = self.check_key(key) cdef int r @@ -110,7 +169,7 @@ cdef class Params: in general try to avoid writing params as much as possible. """ cdef string k = self.check_key(key) - cdef string dat_bytes = ensure_bytes(dat) + cdef string dat_bytes = self._put_cast(key, dat) with nogil: self.p.put(k, dat_bytes) @@ -131,7 +190,7 @@ cdef class Params: def put_nonblocking(self, key, dat): cdef string k = self.check_key(key) - cdef string dat_bytes = ensure_bytes(dat) + cdef string dat_bytes = self._put_cast(key, dat) with nogil: self.p.putNonBlocking(k, dat_bytes) @@ -159,5 +218,19 @@ cdef class Params: cdef string key_bytes = ensure_bytes(key) return self.p.getParamPath(key_bytes).decode("utf-8") + def get_type(self, key): + return self.p.getKeyType(self.check_key(key)) + def all_keys(self): return self.p.allKeys() + + def get_default_value(self, key): + cdef string k = self.check_key(key) + cdef ParamKeyType t = self.p.getKeyType(k) + cdef optional[string] default = self.p.getKeyDefaultValue(k) + return self._cpp2python(t, default.value(), None, key) if default.has_value() else None + + def cpp2python(self, key, value): + cdef string k = self.check_key(key) + cdef ParamKeyType t = self.p.getKeyType(k) + return self._cpp2python(t, value, None, key) diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index 1ef82a5e3..7057c8c83 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -451,15 +451,6 @@ struct CarControl { modelDesire @16: Int16; atcDistance @17: Float32; - leadLeftDist @18: Float32; - leadRightDist @19: Float32; - leadLeftLat @20: Float32; - leadRightLat @21: Float32; - leadLeftDist2 @22: Float32; - leadRightDist2 @23: Float32; - leadLeftLat2 @24: Float32; - leadRightLat2 @25: Float32; - # not used with the dash, TODO: separate structs for dash UI and device UI audibleAlert @5: AudibleAlert; diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index c688d4416..b3283d5c5 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -183,12 +183,12 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip if name == doc.name: return platform return None - found_car = find_car(selected_car.decode("utf-8")) + found_car = find_car(selected_car) if found_car is not None: candidate = found_car print(f"SelectedCar = {candidate}") - Params().put("CarName", candidate) + Params().put("CarName", str(candidate)) Params().put("FingerPrints", str(fingerprints)) CarInterface = interfaces[candidate] diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 6396aa4f2..777feb65f 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -7,6 +7,7 @@ from opendbc.car.hyundai.carstate import CarState from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR, CAN_GEARS, HyundaiExtFlags from opendbc.car.interfaces import CarControllerBase +from opendbc.car.vehicle_model import VehicleModel VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -53,39 +54,58 @@ def process_hud_alert(enabled, fingerprint, hud_control): return sys_warning, sys_state, left_lane_warning, right_lane_warning -def calc_rate_limit_by_lat_accel(delta_deg: float, - v_ego: float, - wheelbase: float, - max_lat_accel: float, - max_rate_low: float, - max_rate_high: float) -> float: - """ - 諛섑솚媛�: �뿀�슜 �뒪�떚�뼱留곹쑀 媛곸냽�룄 (deg/s) - delta_deg : �쁽�옱 �뒪�떚�뼱留곹쑀 媛곷룄 (deg) - v_ego : �냽�룄 (m/s) - wheelbase : 異뺢굅 (m) - """ +def rate_limit(x, x_last, lo, hi): + return float(np.clip(x, x_last + lo, x_last + hi)) - # v媛� �꼫臾� �옉�쑝硫� 怨듭떇�씠 �꽣吏��땲源�, ����냽 �쟾�슜 �긽�닔 �궗�슜 - if v_ego < 0.5: - return max_rate_low # �삁: 300 deg/s +def apply_steer_angle_limits_physics(desired_sw_deg: float, + last_sw_deg: float, + v_ego: float, + steering_sw_deg: float, + lat_active: bool, + wheelbase_m: float, + steer_ratio: float, + steer_sw_max_deg: float) -> float: + max_lat_accel = 5.0 # m/s^2 + max_lat_jerk = 4.0 # m/s^3 + max_sw_rate_deg_per_tick = 2.0 # �쁾 EPS 蹂댄샇�슜 �긽�븳 - delta_rad = np.radians(delta_deg) + v = max(float(v_ego), 1.0) - # cos^2 �빆�씠 0�뿉 媛�源뚯썙吏�硫� rate媛� �룺諛쒗븯�땲 �븯�븳 �꽔�뼱以� - cos_delta = np.cos(delta_rad) - cos2 = max(cos_delta * cos_delta, 0.05) # 0.05 �젙�룄硫� 異⑸텇 + target_sw = float(np.clip(desired_sw_deg, -steer_sw_max_deg, steer_sw_max_deg)) - # 臾쇰━�떇: |灌��| <= L * a_lat_max / (v^2 * sec^2(灌)) - # �뿬湲곗꽌 sec^2(灌) = 1 / cos^2(灌) - delta_rate_rad_s = (wheelbase * max_lat_accel) / (v_ego * v_ego * cos2) + target_rw = target_sw / steer_ratio + last_rw = float(last_sw_deg) / steer_ratio - delta_rate_deg_s = np.degrees(delta_rate_rad_s) + # --- accel limit --- + rw_max_rad = np.arctan((max_lat_accel * wheelbase_m) / (v * v)) + rw_max = float(np.degrees(rw_max_rad)) - # ����냽�뿉�꽌�뒗 �꼫臾� �겕吏� �븡寃�, 怨좎냽�뿉�꽌�뒗 �꼫臾� �옉吏� �븡寃� clip - # max_rate_high <= delta_rate_deg_s <= max_rate_low - return float(np.clip(delta_rate_deg_s, max_rate_high, max_rate_low)) + # --- jerk -> rate limit --- + sec2 = 1.2 + max_drw_dt = (max_lat_jerk * wheelbase_m) / (v * v * sec2) # rad/s + max_drw_per_tick = max_drw_dt * DT_CTRL # rad/tick + max_drw_per_tick_deg = float(np.degrees(max_drw_per_tick)) + max_drw_per_tick_deg = min( + max_drw_per_tick_deg, + max_sw_rate_deg_per_tick / steer_ratio + ) + err = abs(target_sw - last_sw_deg) + if err > 20.0: + max_drw_per_tick_deg *= 0.5 + + # --- rate limit --- + cmd_rw = rate_limit(target_rw, last_rw, -max_drw_per_tick_deg, max_drw_per_tick_deg) + + # --- accel clip --- + cmd_rw = float(np.clip(cmd_rw, -rw_max, rw_max)) + + if not lat_active: + cmd_rw = float(steering_sw_deg) / steer_ratio + + cmd_sw = cmd_rw * steer_ratio + return float(np.clip(cmd_sw, -steer_sw_max_deg, steer_sw_max_deg)) + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -128,6 +148,7 @@ class CarController(CarControllerBase): self.activeCarrot = 0 self.camera_scc_params = Params().get_int("HyundaiCameraSCC") self.is_ldws_car = Params().get_bool("IsLdwsCar") + self.enable_corner_radar = 0 self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP self.steerDeltaDownOrg = self.steerDeltaDown = self.steerDeltaDownLC = self.params.STEER_DELTA_DOWN @@ -174,6 +195,7 @@ class CarController(CarControllerBase): self.canfd_debug = params.get_int("CanfdDebug") self.camera_scc_params = params.get_int("HyundaiCameraSCC") + self.enable_corner_radar = params.get_int("EnableCornerRadar") actuators = CC.actuators hud_control = CC.hudControl @@ -199,45 +221,24 @@ class CarController(CarControllerBase): #apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, # CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS) - MAX_LAT_ACCEL = 8.0 - MAX_RATE_LOW = 200 # ����냽, deg/s - MAX_RATE_HIGH = 40 # 怨좎냽, deg/s - UNWIND_SCALE = 1.5 - UNWIND_MAX = 200 - - delta = actuators.steeringAngleDeg - self.apply_angle_last - same_dir = (np.sign(delta) == np.sign(self.apply_angle_last)) or (abs(self.apply_angle_last) < 2.0) - - rate_deg_s = calc_rate_limit_by_lat_accel(self.apply_angle_last, CS.out.vEgoRaw, self.CP.wheelbase, MAX_LAT_ACCEL, MAX_RATE_LOW, MAX_RATE_HIGH) - if not same_dir: - rate_deg_s = min(rate_deg_s * UNWIND_SCALE, UNWIND_MAX) - - rate_deg_per_tick = rate_deg_s * DT_CTRL - apply_angle = np.clip(actuators.steeringAngleDeg, - self.apply_angle_last - rate_deg_per_tick, - self.apply_angle_last + rate_deg_per_tick) - - angle_limits = self.params.ANGLE_LIMITS - apply_angle = np.clip(apply_angle, -angle_limits.STEER_ANGLE_MAX, angle_limits.STEER_ANGLE_MAX) - - #if abs(apply_angle - self.apply_angle_last) > 0.1: - # alpha = min(0.1 + 0.9 * CS.out.vEgoRaw / (30.0 * CV.KPH_TO_MS), 1.0) - # apply_angle = self.apply_angle_last * (1 - alpha) + apply_angle * alpha - - v_ego_kph = CS.out.vEgoRaw * CV.MS_TO_KPH - if abs(apply_angle - self.apply_angle_last) < 0.1: - alpha = min(0.05 + 0.45 * v_ego_kph / 30.0, 0.5) - else: - alpha = 1.0 # min(0.1 + 0.9 * v_ego_kph / 30.0, 1.0) - - apply_angle = self.apply_angle_last * (1 - alpha) + apply_angle * alpha + apply_angle = apply_steer_angle_limits_physics( + actuators.steeringAngleDeg, + self.apply_angle_last, + CS.out.vEgoRaw, + CS.out.steeringAngleDeg, + CC.latActive, + self.CP.wheelbase, + self.CP.steerRatio, + self.params.ANGLE_LIMITS.STEER_ANGLE_MAX + ) + if angle_control: apply_steer_req = CC.latActive if CS.out.steeringPressed: #self.apply_angle_last = CS.out.steeringAngleDeg - self.lkas_max_torque = self.lkas_max_torque = max(self.lkas_max_torque - 20, 25) + self.lkas_max_torque = max(self.lkas_max_torque - 20, 25) else: target_torque = self.angle_max_torque @@ -344,7 +345,7 @@ class CarController(CarControllerBase): self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget) if True: #not camera_scc: - can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.canfd_debug, self.MainMode_ACC_trigger, self.LFA_trigger)) + can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.enable_corner_radar)) if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.CP, self.packer, self.CAN, self.frame)) else: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 527ba4fb8..7785201e5 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -121,7 +121,7 @@ class CarState(CarStateBase): #self.lf_lateral = 0 #self.rf_lateral = 0 - fingerprints_str = Params().get("FingerPrints", encoding='utf-8') + fingerprints_str = Params().get("FingerPrints") fingerprints = ast.literal_eval(fingerprints_str) #print("fingerprints =", fingerprints) ecu_disabled = False @@ -542,8 +542,19 @@ class CarState(CarStateBase): if not corner: ret.leftLongDist = self.adrv_info_1ea["LF_DETECT_DISTANCE"] ret.rightLongDist = self.adrv_info_1ea["RF_DETECT_DISTANCE"] + self.lr_distance = self.adrv_info_1ea["LR_DETECT_DISTANCE"] + self.rr_distance = self.adrv_info_1ea["RR_DETECT_DISTANCE"] ret.leftLatDist = self.adrv_info_1ea["LF_DETECT_LATERAL"] ret.rightLatDist = self.adrv_info_1ea["RF_DETECT_LATERAL"] + corner = True + if corner: + left_block = True if 0 < ret.leftLongDist < 7.0 or 0 < self.lr_distance < 7.0 else False + right_block = True if 0 < ret.rightLongDist < 7.0 or 0 < self.rr_distance < 7.0 else False + if left_block: + ret.leftBlindspot = True + if right_block: + ret.rightBlindspot = True + self.adrv_info_160 = cp_cam.vl["ADRV_0x160"] if self.ADRV_0x160 else None self.hda_info_4a3 = cp.vl["HDA_INFO_4A3"] if self.HDA_INFO_4A3 else None diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index bd28358b5..eacb2b7a0 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -5,6 +5,12 @@ from opendbc.car.crc import CRC16_XMODEM from opendbc.car.hyundai.values import HyundaiFlags, HyundaiExtFlags from openpilot.common.params import Params from opendbc.car.common.conversions import Conversions as CV +from cereal import log + +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection +TurnDirection = log.Desire + def hyundai_crc8(data: bytes) -> int: poly = 0x2F @@ -304,6 +310,7 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g a_val = np.clip(accel, accel_last - jn, accel_last + jn) values = copy.copy(CS.cruise_info) + values.pop("COUNTER", None) values["ACCMode"] = acc_mode values["MainMode_ACC"] = 1 values["StopReq"] = 1 if stopping or CS.softHoldActive > 0 else 0 # 1: Stop control is required, 2: Not used, 3: Error Indicator @@ -320,8 +327,8 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g #values["ACC_ObjDist"] = 1 #values["ObjValid"] = 0 #values["OBJ_STATUS"] = 2 - values["NSCCOper"] = 1 if enabled else 0 # 0: off, 1: Ready, 2: Act, 3: Error Indicator - values["NSCCOnOff"] = 2 # 0: Default, 1: Off, 2: On, 3: Invalid + #values["NSCCOper"] = 1 if enabled else 0 # 0: off, 1: Ready, 2: Act, 3: Error Indicator + #values["NSCCOnOff"] = 2 # 0: Default, 1: Off, 2: On, 3: Invalid #values["SET_ME_3"] = 0x3 # objRelsped��� 異⑸룎 #values["ACC_ObjLatPos"] = - hud_control.leadDPath values["DriveMode"] = 0 # 0: Default, 1: Comfort Mode, 2:Normal mode, 3:Dynamic mode, reserved @@ -349,6 +356,8 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g values["AccelLimitBandUpper"] = 0.0 # �씠媛믪씠 1.26�씪�븣 媛��냽�쓣 �븞�븯�뒗 利앹긽�씠 蹂댁엫.. values["AccelLimitBandLower"] = 0.0 + values["ZEROS_7"] = 1 + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, jerk_u, jerk_l, CS): @@ -432,6 +441,9 @@ def create_tcs_messages(packer, CAN, CS): values = copy.copy(CS.tcs_info_373) values["DriverBraking"] = 0 values["DriverBrakingLowSens"] = 0 + #values["NEW_SIGNAL_1"] = 0 # accel怨� 愿��젴.. �쁿�몢遺� 爰쇱���뒗寃껉낵 愿��젴? �솗�씤�븘�슂 + #values["ACC_REQ"] = 1 # �쁿�몢遺� 爰쇱���뒗寃껉낵 愿��젴? �솗�씤�븘�슂.. �빆�긽 耳쒖��寃뚰븿.. + values["NEW_SIGNAL_1"] = 0 if values["ACC_REQ"] == 1 else 1 # �쁿�몢遺�.. ret.append(packer.make_can_msg("TCS", CAN.CAM, values)) return ret @@ -452,10 +464,75 @@ def forward_button_message(packer, CAN, frame, CS, cruise_button, MainMode_ACC_t ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values)) return ret -def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, canfd_debug, MainMode_ACC_trigger, LFA_trigger): +""" +def _make_ccnc_values___(values, CS, lat_active, frame, hud_control, lane_line = True, corner_radar = True): + if lane_line: + curvature = round(CS.out.steeringAngleDeg / 3) + values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0 + values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0 + + md = CS.MD + if md is not None: + desire = md.meta.desire.raw + if desire == 1: # # 醫뚰쉶�쟾 + values['LANE_CHANGING'] = 1 # �쇊履� �솕�궡�몴 + values["LANELINE_CURVATURE"] = 15 # 而ㅻ툕 理쒕�� + values["LANELINE_CURVATURE_DIRECTION"] = 0 # �쇊履쎌쑝濡� + + elif desire == 2: # �슦�쉶�쟾 + values['LANE_CHANGING'] = 2 # �삤瑜몄そ �솕�궡�몴 + values["LANELINE_CURVATURE"] = 15 # 李⑥꽑而ㅻ툕 理쒕��濡� + values["LANELINE_CURVATURE_DIRECTION"] = 1 # �삤瑜몄そ�쑝濡� + + elif desire == 3: # 醫뚯감�꽑蹂�寃� + values['LANE_CHANGING'] = 3 # �쇊履� �솕�궡�몴 + 諛붾떏 + + elif desire == 4: # �슦李⑥꽑蹂�寃� + values['LANE_CHANGING'] = 4 # �삤瑜몄そ �솕�궡�몴 + 諛붾떏 + + if corner_radar: + if values['LF_DETECT'] >= 4 and values['LF_DETECT_DISTANCE'] != 0: values['LF_DETECT'] = 1 + if values['RF_DETECT'] >= 4 and values['RF_DETECT_DISTANCE'] != 0: values['RF_DETECT'] = 1 + if values['LR_DETECT'] >= 4 and values['LR_DETECT_DISTANCE'] != 0: values['LR_DETECT'] = 1 + if values['RR_DETECT'] >= 4 and values['RR_DETECT_DISTANCE'] != 0: values['RR_DETECT'] = 1 + + disp_dist = 30.0 + min_dist = 14.0 + max_interval = 100 + t = 1.0 # �씠 媛믩쭔 諛붽씀硫� �쟾泥� 源쒕묀�엫 �냽�룄 議곗젅�맖 (0.6 鍮좊쫫, 1.0 湲곕낯, 1.5 �뒓由�) + def apply_one(detect_key, dist_key): + dist = values.get(dist_key, 0.0) + if dist <= min_dist: + return + d = min(dist, disp_dist) + interval = int((1 + (max_interval - 1) * (d / disp_dist)) * t) + interval = max(1, min(interval, max_interval)) + blink = (frame // interval) & 1 + values[detect_key] = 2 - blink + values[dist_key] = min_dist + + apply_one('LR_DETECT', 'LR_DETECT_DISTANCE') + apply_one('RR_DETECT', 'RR_DETECT_DISTANCE') + +def create_ccnc_messages___(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, enable_corner_radar): ret = [] + md = CS.MD + desire = 0 + lane_changing = 0 + if md is not None: + desire = md.meta.desire.raw + desire_state = md.meta.desireState + if len(desire_state) > 4: + if desire_state[1] > 0.3 : lane_changing = 1 + if desire_state[2] > 0.3 : lane_changing = 2 + if desire_state[3] > 0.3 : lane_changing = 3 + if desire_state[4] > 0.3 : lane_changing = 4 if CP.flags & HyundaiFlags.CAMERA_SCC.value: + HDA_CntrlModSta = 0 + if CS.lfahda_cluster_info is not None: + HDA_CntrlModSta = CS.lfahda_cluster_info["HDA_CntrlModSta"] + if frame % 2 == 0: if CS.adrv_info_160 is not None: values = copy.copy(CS.adrv_info_160) @@ -466,20 +543,29 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) if CS.cruise_buttons_msg is not None: - values = copy.copy(CS.cruise_buttons_msg) - if MainMode_ACC_trigger > 0: - values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1 - elif LFA_trigger > 0: + values = copy.copy(CS.cruise_buttons_msg) + if CS.lfahda_cluster_info["HDA_LFA_SymSta"] == 0 and 0 < frame % 200 < 12: values["LFA_BTN"] = 1 + #else: + # values["LFA_BTN"] = 0 + + if CC.enabled and CS.MainMode_ACC: + if CS.ACCMode in [0, 4] and 10 < frame % 200 < 22: + values["CRUISE_BUTTONS"] = 2 + elif CC.enabled and not CS.MainMode_ACC and 10 < frame % 200 <= 16 and CS.out.vEgo > 3.: + values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1 + else: + values["ADAPTIVE_CRUISE_MAIN_BTN"] = 0 + ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values)) if frame % 5 == 0: + lat_active = CC.latActive if CS.adrv_info_161 is not None: main_enabled = CS.out.cruiseState.available cruise_enabled = CC.enabled lat_enabled = CS.out.latEnabled - lat_active = CC.latActive nav_active = hud_control.activeCarrot > 1 # hdpuse carrot @@ -508,7 +594,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["TARGET_DISTANCE"] = int(hud_control.leadDistance) values["BACKGROUND"] = 6 if CS.paddle_button_prev > 0 else 1 if cruise_enabled else 3 if main_enabled else 7 - values["CENTERLINE"] = 1 if lat_enabled else 0 + values["CENTERLINE"] = 1 if HDA_CntrlModSta > 0 else 0 #lat_enabled else 0 values["CAR_CIRCLE"] = 2 if hdp_active else 1 if cruise_enabled else 0 values["NAV_ICON"] = 2 if nav_active else 0 @@ -517,7 +603,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0 values["FCA_ALT_ICON"] = 0 - if values["ALERTS_2"] in [1, 2, 5, 10, 21, 22]: # 10,21,22: �슫�쟾�옄紐⑤땲�꽣 �븣�엺/寃쎄퀬 + if values["ALERTS_2"] in [1, 2, 5, 6, 10, 21, 22]: # 10,21,22: �슫�쟾�옄紐⑤땲�꽣 �븣�엺/寃쎄퀬, 6: enable lanechange alert values["ALERTS_2"] = 0 values["DAW_ICON"] = 0 @@ -539,13 +625,15 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0 values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0 - + # lane_color = 6 if lat_active else 2 - lane_color = 2 # 6: green, 2: white, 4: yellow + #lane_color = 2 # 6: green, 2: white, 4: yellow + lane_color = 2 if CS.out.leftLaneLine < 20 else 4 if hud_control.leftLaneDepart: values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1 else: values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0 + lane_color = 2 if CS.out.rightLaneLine < 20 else 4 if hud_control.rightLaneDepart: values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1 else: @@ -559,6 +647,9 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["LCA_LEFT_ICON"] = 1 if CS.out.leftBlindspot else 2 values["LCA_RIGHT_ICON"] = 1 if CS.out.rightBlindspot else 2 + values["LANE_LEFT"] = 1 if desire in (1, 3) else 0 + values["LANE_RIGHT"] = 1 if desire in (2, 4) else 0 + ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values)) if CS.adrv_info_200 is not None: @@ -570,24 +661,28 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values = copy.copy(CS.adrv_info_1ea) #values["HDA_MODE1"] = 8 #values["HDA_MODE2"] = 1 - if values['LF_DETECT'] == 0 and hud_control.leadLeftDist > 0: - values['LF_DETECT'] = 3 if hud_control.leadLeftDist > 30 else 4 - values['LF_DETECT_DISTANCE'] = hud_control.leadLeftDist - values['LF_DETECT_LATERAL'] = hud_control.leadLeftLat - if values['RF_DETECT'] == 0 and hud_control.leadRightDist > 0: - values['RF_DETECT'] = 3 if hud_control.leadRightDist > 30 else 4 - values['RF_DETECT_DISTANCE'] = hud_control.leadRightDist - values['RF_DETECT_LATERAL'] = hud_control.leadRightLat - """ - if values['LR_DETECT'] == 0 and hud_control.leadLeftDist2 > 0: - values['LR_DETECT'] = 4 - values['LR_DETECT_DISTANCE'] = 2 - values['LR_DETECT_LATERAL'] = hud_control.leadLeftLat2 - if values['RR_DETECT'] == 0 and hud_control.leadRightDist2 > 0: - values['RR_DETECT'] = 4 - values['RR_DETECT_DISTANCE'] = 2 - values['RR_DETECT_LATERAL'] = hud_control.leadRightLat2 - """ + if lane_changing == 3: + values['LEFT_BLINK_HOLD'] = 1 + elif lane_changing == 4: + values['RIGHT_BLINK_HOLD'] = 1 + + _make_ccnc_values(values, CS, lat_active, frame, hud_control) + # values['AUTOLANECHANGE_MSG'] = 1 # 二쇰�� �긽�솴�쓣 �솗�씤�븯�꽭�슂 + # values['AUTOLANECHANGE_MSG'] = 2 # �옉�룞 議곌굔�씠 �븘�떃�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 3 # 二쇳뻾 李⑤줈瑜� 遺꾩꽍以묒엯�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 4 # 湲됱빱釉� 援ш컙�엯�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 5 # 二쇳뻾 以묒씤 李⑤줈�쓽 �룺�씠 醫곸뒿�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 6 # �옉�룞 援ш컙�씠 �븘�떃�땲�떎. + # values['AUTOLANECHANGE_MSG'] = 7 # 鍮꾩긽�벑�씠 耳쒖졇�엳�뒿�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 8 # 二쇳뻾�냽�룄媛� �궙�뒿�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 9 # �빖�뱾�쓣 �옟�쑝�떗�떆�삤 + # values['AUTOLANECHANGE_MSG'] = 10 # �옉�룞 媛��뒫�븳 李⑤줈媛� �븘�떃�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 11 # �빖�뱾 議곗옉�씠 媛먯���릺�뿀�뒿�땲�떎. + # �뼐�뒗 �슦痢� RPM 寃뚯씠吏��뿉 �겕寃� �굹�샂 + # values['AUTOLANECHANGE_MSG'] = 12 # ok 踰꾪듉�쓣 �늻瑜대㈃ 李⑤줈蹂�寃� 蹂댁“湲곕뒫�씠 耳쒖쭛�땲�떎 + # values['AUTOLANECHANGE_MSG'] = 13 # �뾾�쓬. + # values['AUTOLANECHANGE_MSG'] = 14 # �뾾�쓬. + # values['AUTOLANECHANGE_MSG'] = 15 # �뾾�쓬. ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values)) if CS.adrv_info_162 is not None: @@ -599,65 +694,54 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle ff_type = 3 if hud_control.leadRadar == 1 else 13 values["FF_DETECT"] = ff_type if hud_control.leadRelSpeed > -0.1 else ff_type + 1 #values["FF_DETECT_LAT"] = - hud_control.leadDPath + _make_ccnc_values(values, CS, lat_active, frame, hud_control, lane_line = False, corner_radar= True) - if True: - if values['LF_DETECT'] == 0 and hud_control.leadLeftDist > 0: - values['LF_DETECT'] = 3 if hud_control.leadLeftDist > 30 else 4 - values['LF_DETECT_DISTANCE'] = hud_control.leadLeftDist - values['LF_DETECT_LATERAL'] = hud_control.leadLeftLat - if values['RF_DETECT'] == 0 and hud_control.leadRightDist > 0: - values['RF_DETECT'] = 3 if hud_control.leadRightDist > 30 else 4 - values['RF_DETECT_DISTANCE'] = hud_control.leadRightDist - values['RF_DETECT_LATERAL'] = hud_control.leadRightLat - if values['LR_DETECT'] == 0 and hud_control.leadLeftDist2 > 0: - values['LR_DETECT'] = 4 - values['LR_DETECT_DISTANCE'] = 2 - values['LR_DETECT_LATERAL'] = hud_control.leadLeftLat2 - if values['RR_DETECT'] == 0 and hud_control.leadRightDist2 > 0: - values['RR_DETECT'] = 4 - values['RR_DETECT_DISTANCE'] = 2 - values['RR_DETECT_LATERAL'] = hud_control.leadRightLat2 - else: - sensors = [ - ('lf', 'LF_DETECT'), - ('rf', 'RF_DETECT'), - ('lr', 'LR_DETECT'), - ('rr', 'RR_DETECT') - ] - - for sensor_key, detect_key in sensors: - distance = getattr(CS, f"{sensor_key}_distance") - if distance > 0: - values[detect_key] = 3 if distance > 30 else 4 - - """ - values["FAULT_FCA"] = 0 - values["FAULT_LSS"] = 0 - values["FAULT_LFA"] = 0 - values["FAULT_LCA"] = 0 - values["FAULT_DAS"] = 0 - values["FAULT_HDA"] = 0 - """ + #values["FAULT_FCA"] = 0 + #values["FAULT_LSS"] = 0 + #values["FAULT_LFA"] = 0 + #values["FAULT_LCA"] = 0 + #values["FAULT_DAS"] = 0 + #values["FAULT_HDA"] = 0 if (left_lane_warning and not CS.out.leftBlinker) or (right_lane_warning and not CS.out.rightBlinker): values["VIBRATE"] = 1 ret.append(packer.make_can_msg("CCNC_0x162", CAN.ECAN, values)) - if canfd_debug > 0: - if frame % 20 == 0: # �븘吏� �떆�뿕以�.. + if enable_corner_radar > 0: + if HDA_CntrlModSta == 0: + if frame % 500 in [10,20,30]: + values = { + 'BYTE_1': 0, + 'BYTE_2': 0, + 'BYTE_3': 0x80, + 'BYTE_4': 0x8A, + 'BYTE_5': 0x32, + 'BYTE_6': 0x30, + 'BYTE_7': 0x01, + 'BYTE_8': 0x00, + } + ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values)) + elif frame % 500 in [40,50,60]: + values = { + 'BYTE_1': 0xff, + 'BYTE_2': 0xff, + 'BYTE_3': 0xff, + 'BYTE_4': 0xff, + 'BYTE_5': 0xff, + 'BYTE_6': 0xff, + 'BYTE_7': 0xff, + 'BYTE_8': 0xff, + } + ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values)) + if False: #canfd_debug > 1 and frame % 20 == 0: # �븘吏� �떆�뿕以�.. if CS.hda_info_4a3 is not None: values = copy.copy(CS.hda_info_4a3) - #if canfd_debug == 1: - values["SIGNAL_0"] = 5 - values["NEW_SIGNAL_1"] = 4 - values["SPEED_LIMIT"] = 80 - values["NEW_SIGNAL_3"] = 154 - values["NEW_SIGNAL_4"] = 9 - values["NEW_SIGNAL_5"] = 0 - values["NEW_SIGNAL_6"] = 256 + values["LinkClass"] = 1 + values["SPEED_LIMIT"] = 100 ret.append(packer.make_can_msg("HDA_INFO_4A3", CAN.CAM, values)) return ret +""" def create_adrv_messages(CP, packer, CAN, frame): # messages needed to car happy after disabling @@ -727,3 +811,302 @@ def hkg_can_fd_checksum(address: int, sig, d: bytearray) -> int: elif len(d) == 32: crc ^= 0x9F5B return crc + + + + +def _clip_int(x, lo, hi): + return lo if x < lo else hi if x > hi else int(x) + +def _get_desire_and_lane_changing(md): + desire = 0 + lane_changing = 0 + if md is not None: + desire = md.meta.desire.raw + ds = md.meta.desireState + if len(ds) > 4: + if ds[1] > 0.3: lane_changing = 1 + if ds[2] > 0.3: lane_changing = 2 + if ds[3] > 0.3: lane_changing = 3 + if ds[4] > 0.3: lane_changing = 4 + return desire, lane_changing + +def _apply_lane_desire(values, desire): + #values['LANE_CHANGING'] = 0 + + if desire == 1: # 醫뚰쉶�쟾 + values['LANE_CHANGING'] = 1 + values["LANELINE_CURVATURE"] = 15 + values["LANELINE_CURVATURE_DIRECTION"] = 0 + + elif desire == 2: # �슦�쉶�쟾 + values['LANE_CHANGING'] = 2 + values["LANELINE_CURVATURE"] = 15 + values["LANELINE_CURVATURE_DIRECTION"] = 1 + + elif desire == 3: # 醫뚯감�꽑蹂�寃� + values['LANE_CHANGING'] = 3 + + elif desire == 4: # �슦李⑥꽑蹂�寃� + values['LANE_CHANGING'] = 4 + +def _apply_radar_blink(values, radar_pairs, frame, *, + disp_dist=30.0, min_dist=14.0, + max_interval=100, t=1.0): + """ + 嫄곕━ > min_dist �씪 �븣留� 源쒕묀�엫. + 嫄곕━ 硫��닔濡� interval 而ㅼ쭚(�뒓由ш쾶). + """ + for det_key, dist_key in radar_pairs: + dist = values[dist_key] + if dist <= min_dist: + continue + + d = min(dist, disp_dist) + interval = int((1 + (max_interval - 1) * (d / disp_dist)) * t) + interval = _clip_int(interval, 1, max_interval) + + blink = (frame // interval) & 1 + values[det_key] = 2 - blink + values[dist_key] = min_dist + +def _make_ccnc_values(values, CS, lat_active, frame, hud_control, + lane_line=True, corner_radar=True, + desire=0, + blink_pairs=None, + blink_t=1.0): + if lane_line: + curvature = round(CS.out.steeringAngleDeg / 3) + mag = min(abs(curvature), 15) + curv = mag + (-1 if curvature < 0 else 0) + direction = 1 if curvature < 0 else 0 + values["LANELINE_CURVATURE"] = curv if lat_active else 0 + values["LANELINE_CURVATURE_DIRECTION"] = direction if lat_active else 0 + if desire: + _apply_lane_desire(values, desire) + + if corner_radar: + radar_all = [ + ('LF_DETECT', 'LF_DETECT_DISTANCE'), + ('RF_DETECT', 'RF_DETECT_DISTANCE'), + ('LR_DETECT', 'LR_DETECT_DISTANCE'), + ('RR_DETECT', 'RR_DETECT_DISTANCE'), + ] + for det_key, dist_key in radar_all: + if values[det_key] >= 4 and values[dist_key] != 0: + values[det_key] = 1 + + if blink_pairs: + _apply_radar_blink(values, blink_pairs, frame, t=blink_t) + +def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, + disp_angle, left_lane_warning, right_lane_warning, + enable_corner_radar): + ret = [] + + md = CS.MD + desire, lane_changing = _get_desire_and_lane_changing(md) + + if CP.flags & HyundaiFlags.CAMERA_SCC.value: + HDA_CntrlModSta = 0 + if CS.lfahda_cluster_info is not None: + HDA_CntrlModSta = CS.lfahda_cluster_info["HDA_CntrlModSta"] + + if frame % 2 == 0: + #if CS.adrv_info_160 is not None: + # values = copy.copy(CS.adrv_info_160) + # ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) + + if CS.cruise_buttons_msg is not None: + values = copy.copy(CS.cruise_buttons_msg) + + if CS.lfahda_cluster_info["HDA_LFA_SymSta"] == 0 and 0 < frame % 200 < 12: + values["LFA_BTN"] = 1 + + if CC.enabled and CS.MainMode_ACC: + if CS.ACCMode in [0, 4] and 10 < frame % 200 < 22: + values["CRUISE_BUTTONS"] = 2 + elif CC.enabled and (not CS.MainMode_ACC) and 10 < frame % 200 <= 16 and CS.out.vEgo > 3.: + values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1 + else: + values["ADAPTIVE_CRUISE_MAIN_BTN"] = 0 + + ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values)) + + # --- 0x161/0x200/0x1ea/0x162 (frame%5) --- + if frame % 5 == 0: + lat_active = CC.latActive + + if CS.adrv_info_161 is not None: + main_enabled = CS.out.cruiseState.available + cruise_enabled = CC.enabled + lat_enabled = CS.out.latEnabled + nav_active = hud_control.activeCarrot > 1 + + # hdpuse carrot + hdp_use = int(Params().get("HDPuse")) + hdp_active = False + if hdp_use == 1: + hdp_active = cruise_enabled and nav_active + elif hdp_use == 2: + hdp_active = cruise_enabled + # hdpuse carrot + + values = copy.copy(CS.adrv_info_161) + + values["SETSPEED"] = (6 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0 + values["SETSPEED_HUD"] = (5 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0 + + set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + values["vSetDis"] = int(set_speed_in_units + 0.5) + + values["DISTANCE"] = 4 if hdp_active else hud_control.leadDistanceBars + values["DISTANCE_LEAD"] = 2 if cruise_enabled and hud_control.leadVisible else 1 if main_enabled and hud_control.leadVisible else 0 + values["DISTANCE_CAR"] = 3 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0 + values["DISTANCE_SPACING"] = 5 if hdp_active else 1 if cruise_enabled else 0 + + values["TARGET"] = 1 if main_enabled else 0 + values["TARGET_DISTANCE"] = int(hud_control.leadDistance) + + values["BACKGROUND"] = 6 if CS.paddle_button_prev > 0 else 1 if cruise_enabled else 3 if main_enabled else 7 + values["CENTERLINE"] = 1 if HDA_CntrlModSta > 0 else 0 + values["CAR_CIRCLE"] = 2 if hdp_active else 1 if cruise_enabled else 0 + + values["NAV_ICON"] = 2 if nav_active else 0 + values["HDA_ICON"] = 5 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0 + values["LFA_ICON"] = 5 if hdp_active else 2 if lat_active else 1 if lat_enabled else 0 + values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0 + values["FCA_ALT_ICON"] = 0 + + if values["ALERTS_2"] in [1, 2, 5, 6, 10, 21, 22]: + values["ALERTS_2"] = 0 + values["DAW_ICON"] = 0 + + if values["ALERTS_1"] == 0: # alerts媛� �엳�쑝硫� �궗�슫�뱶�룄 媛숈씠 �굹�샂 + values["SOUNDS_1"] = 0 + values["SOUNDS_2"] = 0 + values["SOUNDS_4"] = 0 + + if values["ALERTS_3"] in [3, 4, 13, 17, 19, 26, 7, 8, 9, 10]: + values["ALERTS_3"] = 0 + values["SOUNDS_3"] = 0 + + if values["ALERTS_5"] in [1, 2, 4, 5]: + values["ALERTS_5"] = 0 + + if values["ALERTS_5"] in [11] and CS.softHoldActive == 0: + values["ALERTS_5"] = 0 + + # curvature �몴�떆(0x161履� 湲곗〈 濡쒖쭅 �쑀吏�) + curvature = round(CS.out.steeringAngleDeg / 3) + values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0 + values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0 + + lane_color = 2 if CS.out.leftLaneLine < 20 else 4 + if hud_control.leftLaneDepart: + values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1 + else: + values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0 + + lane_color = 2 if CS.out.rightLaneLine < 20 else 4 + if hud_control.rightLaneDepart: + values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1 + else: + values["LANELINE_RIGHT"] = lane_color if hud_control.rightLaneVisible else 0 + + values["LCA_LEFT_ARROW"] = 2 if CS.out.leftBlinker else 0 + values["LCA_RIGHT_ARROW"] = 2 if CS.out.rightBlinker else 0 + + values["LCA_LEFT_ICON"] = 1 if CS.out.leftBlindspot else 2 + values["LCA_RIGHT_ICON"] = 1 if CS.out.rightBlindspot else 2 + + values["LANE_LEFT"] = 1 if desire in (1, 3) else 0 + values["LANE_RIGHT"] = 1 if desire in (2, 4) else 0 + + ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values)) + + if CS.adrv_info_200 is not None: + values = copy.copy(CS.adrv_info_200) + values["TauGapSet"] = hud_control.leadDistanceBars + ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values)) + + if CS.adrv_info_1ea is not None: + values = copy.copy(CS.adrv_info_1ea) + + # blinker hold + values['LEFT_BLINK_HOLD'] = 1 if lane_changing == 3 else 0 + values['RIGHT_BLINK_HOLD'] = 1 if lane_changing == 4 else 0 + + _make_ccnc_values( + values, CS, lat_active, frame, hud_control, + lane_line=True, + corner_radar=True, + desire=desire, + # 湲곗〈���濡� LR/RR留� 源쒕묀�엫 + blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'), + ('RR_DETECT', 'RR_DETECT_DISTANCE')], + blink_t=1.0 + ) + + ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values)) + + if CS.adrv_info_162 is not None: + values = copy.copy(CS.adrv_info_162) + + if hud_control.leadDistance > 0: + values["FF_DISTANCE"] = hud_control.leadDistance + ff_type = 3 if hud_control.leadRadar == 1 else 13 + values["FF_DETECT"] = ff_type if hud_control.leadRelSpeed > -0.1 else ff_type + 1 + + _make_ccnc_values( + values, CS, lat_active, frame, hud_control, + lane_line=False, + corner_radar=True, + desire=0, + # �븘�슂�븯硫� 162�룄 源쒕묀�엫 �쟻�슜(�썝�옒 肄붾뱶泥섎읆 LR/RR留�) + blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'), + ('RR_DETECT', 'RR_DETECT_DISTANCE')], + blink_t=1.0 + ) + + if (left_lane_warning and not CS.out.leftBlinker) or (right_lane_warning and not CS.out.rightBlinker): + values["VIBRATE"] = 1 + + ret.append(packer.make_can_msg("CCNC_0x162", CAN.ECAN, values)) + + # --- NEW_MSG_4B9 (corner radar keep-alive?) --- + if enable_corner_radar > 0: + if HDA_CntrlModSta == 0: + if frame % 500 in [10, 20, 30]: + values = { + 'BYTE_1': 0, + 'BYTE_2': 0, + 'BYTE_3': 0x80, + 'BYTE_4': 0x8A, + 'BYTE_5': 0x32, + 'BYTE_6': 0x30, + 'BYTE_7': 0x01, + 'BYTE_8': 0x00, + } + ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values)) + elif frame % 500 in [40, 50, 60]: + values = { + 'BYTE_1': 0xff, + 'BYTE_2': 0xff, + 'BYTE_3': 0xff, + 'BYTE_4': 0xff, + 'BYTE_5': 0xff, + 'BYTE_6': 0xff, + 'BYTE_7': 0xff, + 'BYTE_8': 0xff, + } + ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values)) + + if False: # canfd_debug > 1 and frame % 20 == 0: + if CS.hda_info_4a3 is not None: + values = copy.copy(CS.hda_info_4a3) + values["LinkClass"] = 1 + values["SPEED_LIMIT"] = 100 + ret.append(packer.make_can_msg("HDA_INFO_4A3", CAN.CAM, values)) + + return ret diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 7f4ca3199..6f1c2a605 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -173,7 +173,7 @@ class CarInterface(CarInterfaceBase): # carrot, if camera_scc enabled, enable openpilotLongitudinalControl if ret.flags & HyundaiFlags.CAMERA_SCC.value or params.get_int("EnableRadarTracks") > 0: ret.radarUnavailable = False - ret.openpilotLongitudinalControl = True if camera_scc != 3 else False + ret.openpilotLongitudinalControl = True if camera_scc < 3 else False print(f"$$$OenpilotLongitudinalControl = True, CAMERA_SCC({ret.flags & HyundaiFlags.CAMERA_SCC.value}) or RadarTracks{params.get_int('EnableRadarTracks')}") else: print(f"$$$OenpilotLongitudinalControl = {alpha_long}") @@ -236,7 +236,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def init(CP, can_recv, can_send): - Params().put('LongitudinalPersonalityMax', "4") + Params().put_int('LongitudinalPersonalityMax', 4) if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC): addr, bus = 0x7d0, 0 diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index f640f5f0e..492eb67fa 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -20,8 +20,8 @@ class CarControllerParams: # stock comma's #([0, 9, 16, 25], [1.4, 0.6, 0.4, 0.1]), #([0, 9, 16, 25], [1.4, 0.7, 0.5, 0.1]), - ([0, 9, 16, 25], [1.4, 1.2, 0.8, 0.2]), - ([0, 9, 16, 25], [2.0, 1.6, 1.0, 0.35]), + ([0, 9, 16, 25], [1.8, 1.6, 1.3, 0.8]), + ([0, 9, 16, 25], [2.4, 2.0, 1.6, 1.0]), #([0, 9, 16, 25], [1.6, 1.0, 0.6, 0.15]), #([0, 9, 16, 25], [2.0, 1.2, 0.8, 0.28]), # sunny's @@ -393,7 +393,8 @@ class CAR(Platforms): HyundaiCarDocs("Hyundai IONIQ 5 PE (NE1)", car_parts=CarParts.common([CarHarness.hyundai_q])), HyundaiCarDocs("Hyundai Ioniq 5 PE (with HDA II) 2024", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), ], - CarSpecs(mass=2012, wheelbase=3.0, steerRatio=14.26, tireStiffnessFactor=0.65), + #CarSpecs(mass=2012, wheelbase=3.0, steerRatio=14.26, tireStiffnessFactor=0.65), + CarSpecs(mass=2012, wheelbase=3.0, steerRatio=14.26, tireStiffnessFactor=1.0), flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL, ) HYUNDAI_IONIQ_5_N = HyundaiCanFDPlatformConfig( diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 5846cc8ba..bf49e843d 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -380,7 +380,7 @@ class CarInterfaceBase(ABC): dbc_names = {bus: cp.dbc_name for bus, cp in self.can_parsers.items()} self.CC: CarControllerBase = self.CarController(dbc_names, CP) - Params().put('LongitudinalPersonalityMax', "3") + Params().put_int('LongitudinalPersonalityMax', 3) eps_firmware = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) comma_nnff_supported = self.check_comma_nn_ff_support(CP.carFingerprint) @@ -402,9 +402,10 @@ class CarInterfaceBase(ABC): return self.lat_torque_nn_model is not None - def apply(self, c: structs.CarControl, now_nanos: int | None = None) -> tuple[structs.CarControl.Actuators, list[CanData]]: + def apply(self, c: structs.CarControl, now_nanos: int | None = None, MD = None) -> tuple[structs.CarControl.Actuators, list[CanData]]: if now_nanos is None: now_nanos = int(time.monotonic() * 1e9) + self.CS.MD = MD return self.CC.update(c, self.CS, now_nanos) @staticmethod @@ -599,6 +600,8 @@ class CarStateBase(ABC): self.is_metric = True self.lkas_enabled = False + self.MD = None + @abstractmethod def update(self, can_parsers) -> structs.CarState: pass diff --git a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc index 2436f92f2..1b3b74b25 100644 --- a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc +++ b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc @@ -230,20 +230,38 @@ BO_ 373 TCS: 24 XXX SG_ NEW_SIGNAL_4 : 24|7@1+ (1,0) [0|127] "" XXX SG_ NEW_SIGNAL_9 : 31|1@0+ (1,0) [0|1] "" XXX SG_ aBasis : 32|11@1+ (0.01,-10.23) [0|7] "m/s^2" XXX + SG_ NEW_SIGNAL_1 : 47|5@0+ (1,0) [0|31] "" XXX SG_ ACCEL_REF_ACC : 48|11@1- (1,0) [0|1023] "" XXX - SG_ EQUIP_MAYBE : 64|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 63|5@0+ (1,0) [0|31] "" XXX + SG_ SCC_OptTyp : 64|2@1+ (1,0) [0|0] "" AWD,EMS,FR_CMR,HCU,H_U_MM,TCU,VCU,vBDM SG_ ACCEnable : 67|2@0+ (1,0) [0|3] "" XXX SG_ ACC_REQ : 68|1@0+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_5 : 72|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_3 : 69|1@0+ (1,0) [0|1] "" XXX + SG_ SCC_ReqLimSta : 70|2@1+ (1,0) [0|0] "" ADAS_DRV,AWD,Dummy,EMS,FR_CMR,H_U_MM,TCU,vBDM + SG_ ESC_StdStillVal : 72|2@1+ (1,0) [0|3] "" ADAS_DRV,AWD,Dummy,EMS,FR_CMR,H_U_MM,TCU,vBDM SG_ BrakeLight : 74|2@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_3 : 76|1@0+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_1 : 80|1@0+ (1,0) [0|1] "" XXX + SG_ ESC_DclEnblReq : 76|2@1+ (1,0) [0|3] "" AWD,Dummy,EMS,FR_CMR,H_U_MM,TCU,vBDM + SG_ NEW_SIGNAL_5 : 79|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_21 : 80|1@0+ (1,0) [0|1] "" XXX SG_ DriverBraking : 81|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_20 : 82|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_11 : 83|1@0+ (1,0) [0|1] "" XXX SG_ DriverBrakingLowSens : 84|1@1+ (1,0) [0|1] "" XXX - SG_ AEB_EQUIP_MAYBE : 96|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_10 : 85|1@0+ (1,0) [0|1] "" XXX + SG_ ESC_PrkBrkActvSta : 86|2@1+ (1,0) [0|3] "" ADAS_DRV,AWD,Dummy,EMS,FR_CMR,H_U_MM,RR_C_RDR,TCU,vBDM + SG_ NEW_SIGNAL_12 : 95|8@0+ (1,0) [0|255] "" XXX + SG_ FCA_EquipSta : 96|2@1+ (1,0) [0|3] "" ADAS_DRV,FR_CMR,H_U_MM,vBDM + SG_ FCA_AvlblSta : 98|2@1+ (1,0) [0|3] "" ADAS_DRV,AFCU_DRV,CLU,Dummy,FR_CMR,H_U_MM,vBDM + SG_ NEW_SIGNAL_13 : 103|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_14 : 111|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_15 : 119|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_16 : 127|8@0+ (1,0) [0|255] "" XXX SG_ NEW_SIGNAL_6 : 128|4@1+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_17 : 133|2@0+ (1,0) [0|3] "" XXX SG_ NEW_SIGNAL_7 : 135|2@0+ (1,0) [0|3] "" XXX SG_ PROBABLY_EQUIP : 136|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_18 : 143|6@0+ (1,0) [0|63] "" XXX + SG_ NEW_SIGNAL_19 : 151|8@0+ (1,0) [0|255] "" XXX SG_ NEW_SIGNAL_8 : 183|16@0+ (1,0) [0|65535] "" XXX BO_ 352 ADRV_0x160: 16 ADRV @@ -510,8 +528,18 @@ BO_ 480 LFAHDA_CLUSTER: 16 FR_CMR BO_ 490 ADRV_0x1ea: 32 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ HDA_MODE1 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_4 : 25|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_3 : 26|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 27|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_1 : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LEFT_BLINK_HOLD : 29|1@0+ (1,0) [0|1] "" XXX + SG_ RIGHT_BLINK_HOLD : 30|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_5 : 31|1@0+ (1,0) [0|1] "" XXX SG_ HDA_MODE2 : 32|3@1+ (1,0) [0|3] "" XXX + SG_ LANE_LEFT : 36|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_6 : 39|3@0+ (1,0) [0|7] "" XXX + SG_ LANE_RIGHT : 41|2@0+ (1,0) [0|3] "" XXX + SG_ LANE_CHANGING : 45|3@0+ (1,0) [0|1] "" XXX SG_ LF_DETECT_DISTANCE : 46|11@1+ (0.1,0) [0|2047] "" XXX SG_ LF_DETECT_LATERAL : 70|7@0+ (0.1,0) [0|127] "" XXX SG_ LF_DETECT : 74|3@0+ (1,0) [0|7] "" XXX @@ -519,17 +547,18 @@ BO_ 490 ADRV_0x1ea: 32 ADRV SG_ RF_DETECT_LATERAL : 94|7@0+ (0.1,0) [0|127] "" XXX SG_ RF_DETECT : 98|3@0+ (1,0) [0|7] "" XXX SG_ SET_ME_FF : 120|8@1+ (1,0) [0|255] "" XXX - SG_ CORNER_LR_DIST : 138|9@1+ (0.1,0) [0|511] "m" XXX - SG_ CORNER_LR_DETECT : 152|6@1+ (1,0) [0|63] "" XXX - SG_ CORNER_LR_SIG : 160|1@0+ (1,0) [0|1] "" XXX - SG_ CORNER_RR_DIST : 162|9@1+ (0.1,0) [0|511] "m" XXX - SG_ CORNER_RR_DETECT : 172|6@1+ (1,0) [0|63] "" XXX - SG_ CORNER_RR_SIG : 184|1@0+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_5 : 205|1@0+ (1,0) [0|1] "" XXX - SG_ AUTOLANECHANGE_CONFIRM_MSG_MAYBE : 207|2@0+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_2 : 208|5@1+ (1,0) [0|31] "" XXX - SG_ NEW_SIGNAL_3 : 239|8@0+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_1 : 247|8@0+ (1,0) [0|255] "" XXX + SG_ LR_DETECT_DISTANCE : 139|8@1+ (0.1,0) [0|511] "m" XXX + SG_ LR_DETECT_LATERAL : 152|6@1+ (0.1,0) [0|63] "" XXX + SG_ LR_DETECT : 162|3@0+ (1,0) [0|1] "" XXX + SG_ RR_DETECT_DISTANCE : 163|8@1+ (0.1,0) [0|511] "m" XXX + SG_ RR_DETECT_LATERAL : 172|6@1+ (0.1,0) [0|63] "" XXX + SG_ RR_DETECT : 186|3@0+ (1,0) [0|1] "" XXX + SG_ AUTOLANECHANGE_MSG : 207|4@0+ (1,0) [0|3] "" XXX + SG_ LANELINE_CURVATURE : 208|4@1+ (1,0) [0|31] "" XXX + SG_ LANELINE_CURVATURE_DIRECTION : 212|1@0+ (1,0) [0|1] "" XXX + SG_ LANELINE_LEFT_POSITION : 239|8@0+ (1,0) [0|255] "" XXX + SG_ LANELINE_RIGHT_POSITION : 247|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_7 : 248|1@0+ (1,0) [0|1] "" XXX BO_ 507 CAM_0x1fb: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -541,7 +570,9 @@ BO_ 512 ADRV_0x200: 8 ADRV SG_ SET_ME_E1 : 24|8@1+ (1,0) [0|255] "" XXX SG_ TauGapSet : 32|3@1+ (1,0) [0|7] "" XXX SG_ NEW_SIGNAL_2 : 35|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_1 : 39|3@0+ (1,0) [0|7] "" XXX SG_ TauGapSet_ : 42|3@0+ (1,0) [0|31] "" XXX + SG_ NEW_SIGNAL_3 : 47|5@0+ (1,0) [0|31] "" XXX BO_ 513 RADAR_0x201: 32 FRONT_RADAR SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -854,6 +885,26 @@ BO_ 1204 NEW_MSG_4B4: 8 XXX SG_ NEW_SIGNAL_5 : 55|8@0+ (1,0) [0|255] "" XXX SG_ NEW_SIGNAL_6 : 63|8@0+ (1,0) [0|255] "" XXX +BO_ 1209 NEW_MSG_4B9: 8 XXX + SG_ BYTE_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_2 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_3 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_5 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_6 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_8 : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 1214 NEW_MSG_4BE: 8 XXX + SG_ BYTE_1 : 7|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_2 : 15|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_3 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_4 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_5 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_6 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_7 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ BYTE_8 : 63|8@0+ (1,0) [0|255] "" XXX + BO_ 1259 LOCAL_TIME2: 8 XXX SG_ HOURS : 15|5@0+ (1,0) [0|31] "" XXX SG_ MINUTES : 21|6@0+ (1,0) [0|63] "" XXX @@ -979,6 +1030,13 @@ VAL_ 354 FAULT_ESS 0 "HIDDEN" 1 "CHECK_EMERGENCY_STOPPING_FUNCTION" 2 "EMERGENCY VAL_ 362 BLINKER_CONTROL 1 "hazards" 2 "hazards button backlight" 3 "left blinkers" 4 "right blinkers"; VAL_ 373 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue"; +VAL_ 373 SCC_OptTyp 0 "SCC is not equipped" 1 "SCC is equipped" 2 "Not used" 3 "Error Indicator" ; +VAL_ 373 SCC_ReqLimSta 0 "No request" 1 "Limited No Limitation" 2 "Acceleration Limited" 3 "Deceleration Limited" ; +VAL_ 373 ESC_StdStillVal 0 "No stand still detected" 1 "stand still detected" 2 "Not used" 3 "Error Indicator" ; +VAL_ 373 ESC_DclEnblReq 0 "Disable deceleration control" 1 "Enable deceleration control" 2 "Not used" 3 "Error Indicator" ; +VAL_ 373 ESC_PrkBrkActvSta 0 "Parking brake is not activated" 1 "Parking brake is activated" 2 "Not used" 3 "Error Indicator" ; +VAL_ 373 FCA_EquipSta 0 "FCA is not equipped" 1 "FCA is equipped" 2 "Not used" 3 "Error Indicator" ; +VAL_ 373 FCA_AvlblSta 0 "Available" 1 "Temporarily not available" 2 "Permanently not available" 3 "FCA Communication Error" ; VAL_ 416 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled" ; VAL_ 426 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ; VAL_ 463 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ; @@ -994,6 +1052,16 @@ VAL_ 480 HDA_LFA_SymSta 0 "Off" 1 "Gray" 2 "Green" 3 "Green blink" ; VAL_ 480 HDA_LFA_WrnSnd 0 "Off " 1 "Additional Warning Sound" 2 "Reserved" 3 "Error indicator" ; VAL_ 480 HDA_InfoPUDis1 0 "No Pop-up" 1 "System Automatic off (LFA)" 2 "System Automatic off (HDA)" 3 "Reserved" 4 "Reserved" 5 "Reserved" 6 "Not Used" 7 "Error Indicator" ; VAL_ 480 HDA_TDMRMDclReq 0 "Not automatically deactivated state of LFA (default)" 1 "Automatically deactivated state of LFA" 2 "Reserved" 3 "Reserved" ; +VAL_ 490 HDA_MODE2 1 "Lane change icon gray" 2 "lane green + lane change icon green" 3 "lane green + lane change icon green (both blinking)" 4 "lane white + lane change icon white (both blinking rapidly)" 5 "Lane Change Assist system check warning"; +VAL_ 490 LANE_LEFT 0 "IDLE" 1 "ON_SOURCE" 2 "ON_TARGET"; +VAL_ 490 LANE_RIGHT 0 "IDLE" 1 "ON_SOURCE" 2 "ON_TARGET"; +VAL_ 490 LANE_CHANGING 0 "IDLE" 1 "LEFT_CHECK" 3 "LEFT_CHANGING" 2 "RIGHT_CHECK" 4 "RIGHT_CHANGING"; +VAL_ 490 LF_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide"; +VAL_ 490 RF_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide"; +VAL_ 490 LR_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide"; +VAL_ 490 RR_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide"; +VAL_ 490 AUTOLANECHANGE_MSG 1 "Check surrounding conditions" 2 "Operating conditions not met" 3 "Analyzing driving lane" 4 "Sharp curve ahead" 5 "Current lane is too narrow" 6 "Not an operational section" 7 "Hazard lights are on" 8 "Vehicle speed is too low" 9 "Please hold the steering wheel" 10 "Not an operable lane" 11 "Steering input detected" 12 "Press the OK button to activate lane change assist"; +VAL_ 490 LANELINE_CURVATURE_DIRECTION 0 "LEFT" 1 "RIGHT"; VAL_ 676 LEFT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence"; VAL_ 676 RIGHT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence"; VAL_ 736 MSLA_STATUS 0 "disabled" 1 "active" 2 "paused"; diff --git a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h index 9229eb710..bc56c2b59 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h @@ -84,6 +84,9 @@ const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = { {506, 2, 32}, // CLUSTER_SPEED_LIMIT {234, 2, 24}, // MDPS {687, 2, 8}, // STEER_TOUCH_2AF + + {0x4BE, 2, 8}, // NEW_MSG_4BE (may be corner radar enabler x) + {0x4B9, 2, 8}, // NEW_MSG_4B9 (may be corner radar enabler) }; const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = { @@ -442,6 +445,7 @@ static int hyundai_canfd_fwd_hook(int bus_num, int addr) { break; } } + if(addr == 0x4b9) bus_fwd = -1; // maybe corner rara disabler. } if (bus_num == 1) { int i; diff --git a/scripts/add/events_ko.py b/scripts/add/events_ko.py index af1affa4c..fae638317 100644 --- a/scripts/add/events_ko.py +++ b/scripts/add/events_ko.py @@ -253,7 +253,7 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - model_name = Params().get("NNFFModelName", encoding='utf-8') + model_name = Params().get("NNFFModelName") if model_name == "": return Alert( "NNFF Torque Controller not available", diff --git a/scripts/add/events_zh.py b/scripts/add/events_zh.py index 498cca052..1e141dab3 100644 --- a/scripts/add/events_zh.py +++ b/scripts/add/events_zh.py @@ -253,7 +253,7 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - model_name = Params().get("NNFFModelName", encoding='utf-8') + model_name = Params().get("NNFFModelName") if model_name == "": return Alert( "NNFF�돪�윪�렒�댍�솳訝띶룾�뵪", diff --git a/selfdrive/apilot.json b/selfdrive/apilot.json deleted file mode 100644 index e3973ef8e..000000000 --- a/selfdrive/apilot.json +++ /dev/null @@ -1,1332 +0,0 @@ -{ - "apilot": 20220111, - "params": [ - { - "group": "조향튜닝", - "name": "LateralTorqueCustom", - "title": "_LateralTorqueCustom(0)", - "descr": "0:LiveTorque, 1:CustomTorque", - "egroup": "LAT", - "etitle": "_LateralTorqueCustom(0)", - "edescr": "", - "min": 0, - "max": 1, - "default": 1, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "LateralTorqueAccelFactor", - "title": "_LateralTorqueAccelFactor*0.001(2500)", - "descr": "", - "egroup": "LAT", - "etitle": "_LateralTorqueAccelFactor*0.001(2500)", - "edescr": "", - "min": 1000, - "max": 6000, - "default": 2500, - "unit": 100 - }, - { - "group": "조향튜닝", - "name": "LateralTorqueFriction", - "title": "_LateralTorqueFriction*0.001(100)", - "descr": "", - "egroup": "LAT", - "etitle": "_LateralTorqueFriction*0.001(100)", - "edescr": "", - "min": 0, - "max": 1000, - "default": 100, - "unit": 10 - }, - { - "group": "조향튜닝", - "name": "LateralTorqueKd", - "title": "_LateralTorqueKd*0.01(800)", - "descr": "", - "egroup": "LAT", - "etitle": "_LateralTorqueKd*0.01(800)", - "edescr": "", - "min": 0, - "max": 10000, - "default": 100, - "unit": 10 - }, - { - "group": "조향튜닝", - "name": "CarrotLatControl", - "title": "*당근조향제어(0)", - "descr": "0:Default, 1:당근제어", - "egroup": "LAT", - "etitle": "*Carrot method lateral control(0)", - "edescr": "", - "min": 0, - "max": 90, - "default": 1, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "LatMpcPathCost", - "title": "*LATMPC.PathCost(100)", - "descr": "경로추종가중치, 높으면 경로우선시함.", - "egroup": "LAT", - "etitle": "*LATMPC.PathCost(100)", - "edescr": "", - "min": 0, - "max": 500, - "default": 1, - "unit": 10 - }, - { - "group": "조향튜닝", - "name": "LatMpcAccelCost", - "title": "*LATMPC.AccelCost(0)", - "descr": "횡가속도제한, 낮으면 민첩성증가 안정성감소", - "egroup": "LAT", - "etitle": "*LATMPC.AccelCost(0)", - "edescr": "", - "min": 0, - "max": 500, - "default": 1, - "unit": 10 - }, - { - "group": "조향튜닝", - "name": "LatMpcJerkCost", - "title": "*LATMPC.JerkCost(4)", - "descr": "낮추면 빠른조향허용, 부드러음감소", - "egroup": "LAT", - "etitle": "*LATMPC.JerkCost(4)", - "edescr": "", - "min": 0, - "max": 200, - "default": 1, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "LatMpcSteeringRateCost", - "title": "*LATMPC.SteerRateCost(700)", - "descr": "조향변화율제한, 낮추면 조향이 적극적", - "egroup": "LAT", - "etitle": "*LATMPC.SteerRateCost(700)", - "edescr": "", - "min": 0, - "max": 1000, - "default": 1, - "unit": 100 - }, - { - "group": "조향튜닝", - "name": "CustomSteerMax", - "title": "_CustomSteerMax(0)", - "descr": "", - "egroup": "LAT", - "etitle": "_CustomSteerMax(0)", - "edescr": "", - "min": 0, - "max": 30000, - "default": 0, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "CustomSteerDeltaUp", - "title": "_CustomSteerDeltaUp(0)", - "descr": "", - "egroup": "LAT", - "etitle": "_CustomSteerDeltaUp(0)", - "edescr": "", - "min": 0, - "max": 50, - "default": 0, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "CustomSteerDeltaDown", - "title": "_CustomSteerDeltaDown(0)", - "descr": "", - "egroup": "LAT", - "etitle": "_CustomSteerDeltaDown(0)", - "edescr": "", - "min": 0, - "max": 50, - "default": 0, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "SteerActuatorDelay", - "title": "SteerActuatorDelay(40)", - "descr": "조향지연값", - "egroup": "LAT", - "etitle": "SteerActuatorDelay(40)", - "edescr": "", - "min": 1, - "max": 300, - "default": 40, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "ModelActuatorDelay", - "title": "*ModelActuatorDelay(80)x0.01", - "descr": "모델지연값, 커브진입시점로 조절", - "egroup": "LAT", - "etitle": "*ModelActuatorDelay(80)", - "edescr": "", - "min": 1, - "max": 300, - "default": 80, - "unit": 1 - }, - { - "group": "크루즈", - "name": "CruiseOnDist", - "title": "크루즈ON거리(0cm)", - "descr": "엑셀/브레이크OFF 시, 선행차가 가까와지면 크루즈ON", - "egroup": "CRUISE", - "etitle": "CruiseOnDist(0cm)", - "edescr": "Gas/Brake Off and LeadCar close, CruiseON(+)", - "min": 0, - "max": 2500, - "default": 0, - "unit": 50 - }, - { - "group": "크루즈", - "name": "CruiseEcoControl", - "title": "크루즈연비제어(2km/h)", - "descr": "목표속도를 일시적으로 올림", - "egroup": "CRUISE", - "etitle": "CruiseEcoControl(2km/h)", - "edescr": "", - "min": 0, - "max": 10, - "default": 2, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "CustomSR", - "title": "CustomSteerRatiox0.1(0)", - "descr": "0: LiveSteerRatio사용, 단위:0.1", - "egroup": "LAT", - "etitle": "CustomSteerRatiox0.1(0)", - "edescr": "0: Use LiveSteerRatio", - "min": 0, - "max": 300, - "default": 0, - "unit": 1 - }, - { - "group": "조향튜닝", - "name": "SteerRatioRate", - "title": "CustomSteerRatioRatex0.01(100)", - "descr": "LiveSteerRatio 반영률", - "egroup": "LAT", - "etitle": "CustomSteerRatioRatex0.01(100)", - "edescr": "LiveSteerRatio Apply Ratio", - "min": 30, - "max": 200, - "default": 0, - "unit": 1 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals1", - "title": "가속설정1:0km/h(160)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet1:0km/h(160)", - "edescr": "Normal mode only (x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 160, - "unit": 5 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals2", - "title": "가속설정2:40km/h(120)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet2:40km/h(120)", - "edescr": "Normal mode only (x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 120, - "unit": 5 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals3", - "title": "가속설정3:60km/h(100)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet3:60km/h(100)", - "edescr": "Normal mode only (x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 100, - "unit": 5 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals4", - "title": "가속설정4:80km/h(80)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet4:80km/h(80)", - "edescr": "Normal mode only (x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 80, - "unit": 5 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals5", - "title": "가속설정5:110km/h(70)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet5:110km/h(70)", - "edescr": "Normal mode only (x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 70, - "unit": 5 - }, - { - "group": "가속설정", - "name": "CruiseMaxVals6", - "title": "가속설정6:140km/h(60)", - "descr": "Normal mode only (x0.01m/s^2)", - "egroup": "ACCEL", - "etitle": "AccelSet6:140km/h(60)", - "edescr": "(x0.01m/s^2)", - "min": 1, - "max": 250, - "default": 60, - "unit": 5 - }, - { - "group": "주행튜닝", - "name": "StopDistanceCarrot", - "title": "StopDistance(600)cm", - "descr": "", - "egroup": "LONG", - "etitle": "StopDistance(600)cm", - "edescr": "", - "min": 400, - "max": 1000, - "default": 600, - "unit": 10 - }, - { - "group": "주행튜닝", - "name": "ComfortBrake", - "title": "Comfort Brake(240)", - "descr": "x0.01, 값이 크면 급하게 브레이킹", - "egroup": "LONG", - "etitle": "Comfort Brake(240)", - "edescr": "", - "min": 200, - "max": 300, - "default": 240, - "unit": 5 - }, - { - "group": "주행튜닝", - "name": "LongTuningKpV", - "title": "Long KpV(100)x0.01", - "descr": "", - "egroup": "LONG", - "etitle": "Long KpV(100)x0.01", - "edescr": "", - "min": 0, - "max": 200, - "default": 0, - "unit": 5 - }, - { - "group": "주행튜닝", - "name": "LongTuningKf", - "title": "Long Kf(100)x0.01", - "descr": "", - "egroup": "LONG", - "etitle": "Long Kf(100)x0.01", - "edescr": "", - "min": 0, - "max": 200, - "default": 0, - "unit": 5 - }, - { - "group": "주행튜닝", - "name": "LongTuningKiV", - "title": "Long KiV(0)x0.01", - "descr": "", - "egroup": "LONG", - "etitle": "Long KiV(0)x0.01", - "edescr": "", - "min": 0, - "max": 2000, - "default": 0, - "unit": 1 - }, - { - "group": "주행튜닝", - "name": "LongActuatorDelay", - "title": "LongActuatorDelay(20)x0.01", - "descr": "", - "egroup": "LONG", - "etitle": "LongActuatorDelay(20)x0.01", - "edescr": "", - "min": 0, - "max": 200, - "default": 20, - "unit": 5 - }, - { - "group": "주행튜닝", - "name": "RadarReactionFactor", - "title": "Radar reaction factor (10)%", - "descr": "값이 낮아지면 선행차량에 대한 반응이 빨라짐.", - "egroup": "LONG", - "etitle": "Radar reaction factor (10)%", - "edescr": "", - "min": 0, - "max": 200, - "default": 0, - "unit": 1 - }, - { - "group": "감속제어", - "name": "AutoCurveSpeedFactor", - "title": "커브속도 조절비율(100%)", - "descr": "높으면 커브속도가 느려집니다.", - "egroup": "DECEL", - "etitle": "CurveSpeedFactor(100%)", - "edescr": "Higher value, slow curve speed", - "min": 50, - "max": 300, - "default": 100, - "unit": 5 - }, - { - "group": "감속제어", - "name": "AutoCurveSpeedAggressiveness", - "title": "커브속도 적극성(100%)", - "descr": "올리면 진입속도가 적극적으로 변합니다.", - "egroup": "DECEL", - "etitle": "CurveSpeed Aggressiveness(100%)", - "edescr": "Higher value, more speed down", - "min": 50, - "max": 300, - "default": 10, - "unit": 1 - }, - { - "group": "조향일반", - "name": "AutoTurnControl", - "title": "ATC: AutoTurnControl(0)", - "descr": "0:사용안함,1:차선변경, 2:차선변경+속도, 3:속도", - "egroup": "LATSET", - "etitle": "ATC: AutoTurnControl", - "edescr": "0: Not used, 1:laneChange, 2:laneChange+speed, 3:speed", - "min": 0, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "조향일반", - "name": "AutoTurnControlSpeedTurn", - "title": "ATC: 턴속도(20)", - "descr": "0:사용안함, 턴시 적용속도", - "egroup": "LATSET", - "etitle": "ATC: Turn Speed(20)", - "edescr": "0: Not used, Turn Speed", - "min": 0, - "max": 100, - "default": 20, - "unit": 5 - }, - { - "group": "조향일반", - "name": "AutoTurnControlTurnEnd", - "title": "ATC: 시간거리(3)", - "descr": "NOO완료 시간거리(속도*시간)", - "egroup": "LATSET", - "etitle": "ATC: Speed Ctrl left Time(3)", - "edescr": "", - "min": 0, - "max": 30, - "default": 6, - "unit": 1 - }, - { - "group": "조향일반", - "name": "AutoTurnMapChange", - "title": "ATC Helper 맵전환(0)", - "descr": "ATC 작동시 자동맵전환", - "egroup": "LATSET", - "etitle": "ATC Helper Map display(0)", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "버튼설정", - "name": "CruiseButtonMode", - "title": "크루즈버튼작동모드(0)", - "descr": "0:일반,1:사용자1,2:사용자2", - "egroup": "BUTN", - "etitle": "Cruise Button Mode(0)", - "edescr": "0:General,1:User1,2:User2", - "min": 0, - "max": 2, - "default": 0, - "unit": 1 - }, - { - "group": "버튼설정", - "name": "CruiseButtonTest1", - "title": "크루즈버튼스팸쏘기(8)", - "descr": "속도버튼신호 발송제한시간,비롱컨에서 사용", - "egroup": "BUTN", - "etitle": "Cruise Button spam1(8)", - "edescr": "", - "min": 1, - "max": 20, - "default": 0, - "unit": 1 - }, - { - "group": "버튼설정", - "name": "CruiseButtonTest2", - "title": "크루즈버튼스팸쉬기(30)", - "descr": "속도버튼신호 발송 휴지시간, 비롱컨에서 사용", - "egroup": "BUTN", - "etitle": "Cruise Button spam2(30)", - "edescr": "", - "min": 1, - "max": 200, - "default": 0, - "unit": 1 - }, - { - "group": "버튼설정", - "name": "CruiseButtonTest3", - "title": "크루즈버튼스팸연속쏘기(1)", - "descr": "속도버튼신호 연속회수, 비롱컨에서 사용", - "egroup": "BUTN", - "etitle": "Cruise Button spam3(1)", - "edescr": "", - "min": 1, - "max": 20, - "default": 0, - "unit": 1 - }, - { - "group": "버튼설정", - "name": "CruiseSpeedUnit", - "title": "크루즈버튼속도단위(10)", - "descr": "크루즈버튼 속도 단위", - "egroup": "BUTN", - "etitle": "Cruise Speed Unit(10)", - "edescr": "SET/DECEL up/down speed rate", - "min": 1, - "max": 20, - "default": 10, - "unit": 1 - }, - { - "group": "시작", - "name": "AutoCruiseControl", - "title": "오토크루즈제어(HKG only)", - "descr": "1:SOFTHOLD, Auto Cruise, 2:SoftHold오류시", - "egroup": "START", - "etitle": "Auto Cruise control(HKG only)", - "edescr": "Softhold, Auto Cruise ON/OFF control", - "min": 0, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "AutoGasTokSpeed", - "title": "오토크루즈: 엑셀톡 속도", - "descr": "해당속도 이상에서만 작동함.", - "egroup": "START", - "etitle": "Auto Cruise: Gas Tok speed", - "edescr": "", - "min": 0, - "max": 200, - "default": 0, - "unit": 5 - }, - { - "group": "시작", - "name": "AutoEngage", - "title": "오토인게이지", - "descr": "1:SteerEnable, 2:Steer,Cruise Enable", - "egroup": "START", - "etitle": "Auto Engage on onroad", - "edescr": "1:SteerEnable, 2:Steer, Cruise Enable", - "min": 0, - "max": 2, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "MapboxStyle", - "title": "맵박스 스타일", - "descr": "", - "egroup": "START", - "etitle": "MapboxStyle", - "edescr": "", - "min": 0, - "max": 2, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "DisableMinSteerSpeed", - "title": "저속조향제한해제", - "descr": "저속조향이 안되나, SMDPS장착차량", - "egroup": "START", - "etitle": "DisableMinSteerSpeed", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "SpeedFromPCM", - "title": "PCM차량속도제어(순정SCC)", - "descr": "1:버튼스패밍안함(차량속도), 0: 커브/카메라 감속 + OPLong, 2: 커브/카메라 감속, 3:Honda/Toyota", - "egroup": "START", - "etitle": "Speed from PCM(StockSCC)", - "edescr": "1: Cruise speed control from Car, 0,2: Speed OP, 3:Honda", - "min": 0, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "MaxTimeOffroadMin", - "title": "자동전원OFF(분)", - "descr": "", - "egroup": "START", - "etitle": "PowerOffTime(min)", - "edescr": "", - "min": 0, - "max": 3000, - "default": 60, - "unit": 10 - }, - { - "group": "시작", - "name": "DisableDM", - "title": "DisableDM", - "descr": "", - "egroup": "START", - "etitle": "DisableDM", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "크루즈", - "name": "AutoSpeedUptoRoadSpeedLimit", - "title": "자동속도증가 (100)%", - "descr": "전방차량에 따라 자동속도증가. 로드스피드*비율 까지 자동속도올림.", - "egroup": "CRUISE", - "etitle": "AutoSpeedUpx:(100)%", - "edescr": "", - "min": 0, - "max": 200, - "default": 0, - "unit": 10 - }, - { - "group": "감속제어", - "name": "AutoRoadSpeedAdjust", - "title": "자동도로제한속도감속 (50)%", - "descr": "100: 기존속도유지, 50: 중간값, 0: 새로운도로속도", - "egroup": "CRUISE", - "etitle": "AutoRoadLimitSpeedAdjust (50)%", - "edescr": "", - "min": 0, - "max": 100, - "default": 0, - "unit": 10 - }, - { - "group": "크루즈", - "name": "AutoGasSyncSpeed", - "title": "자동크루즈속도 업데이트", - "descr": "크루즈속도 보다빨라지면 속도 업데이트됨", - "egroup": "CRUISE", - "etitle": "Auto update Cruise speed", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - - { - "group": "화면", - "name": "ShowDebugUI", - "title": "디버그정보 표시", - "descr": "", - "egroup": "DISP", - "etitle": "Show Debug Info", - "edescr": "", - "min": 0, - "max": 2, - "default": 1, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowDateTime", - "title": "시간정보표시", - "descr": "", - "egroup": "DISP", - "etitle": "Show Time Info", - "edescr": "0:None, 1:Time/Date, 2:Time, 3:Date", - "min": 0, - "max": 3, - "default": 1, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowPathEnd", - "title": "패쓰끝표시", - "descr": "", - "egroup": "DISP", - "etitle": "Show Path End", - "edescr": "", - "min": 0, - "max": 1, - "default": 1, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowDeviceState", - "title": "디바이스정보", - "descr": "", - "egroup": "DISP", - "etitle": "Show Device Info", - "edescr": "", - "min": 0, - "max": 1, - "default": 1, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowCustomBrightness", - "title": "화면밝기비율", - "descr": "", - "egroup": "DISP", - "etitle": "ScreenBrightness ratio", - "edescr": "", - "min": 0, - "max": 100, - "default": 100, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowLaneInfo", - "title": "차선정보", - "descr": "-1:없음, 0:패스만, 1:레인, 2:로드엣지", - "egroup": "DISP", - "etitle": "Show Lane Info", - "edescr": "-1:None, 0:Path, 1:Path+Lane, 2:Path+Lane+RoadEdge", - "min": -1, - "max": 2, - "default": 1, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowRadarInfo", - "title": "레이더감지정보(0)", - "descr": "0:안함,1:표시함,2:상대위치,3:정지차량", - "egroup": "DISP", - "etitle": "Show Radar Info", - "edescr": "", - "min": 0, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowRouteInfo", - "title": "경로AR표시(0)", - "descr": "0:안함,1:표시함", - "egroup": "DISP", - "etitle": "Show Route Info", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathMode", - "title": "패스종류,레인리스(9)", - "descr": "0:일반,1,2:사각,3,4:^^,5,6:사각,7,8:^^,9,10,11:smooth^^,12:smooth2^^,13:3줄,14:2줄,15:1줄", - "egroup": "DISP", - "etitle": "Show Path Mode:LaneLess(9)", - "edescr": "", - "min": 0, - "max": 15, - "default": 9, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathColor", - "title": "패스색상,레인리스(12)", - "descr": "(+10:띠)0:빨,1:주,2:노,3:초,4:파,5:남,6:보,7:고,8:흰,9:검", - "egroup": "DISP", - "etitle": "Show Path Color:LaneLess(12)", - "edescr": "", - "min": 0, - "max": 19, - "default": 12, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathModeCruiseOff", - "title": "패스종류,크루즈OFF(9)", - "descr": "0:일반,1,2:사각,3,4:^^,5,6:사각,7,8:^^,9,10,11:smooth^^,12:smooth2^^,13:3줄,14:2줄,15:1줄", - "egroup": "DISP", - "etitle": "Show Path Mode:CruiseOFF(9)", - "edescr": "", - "min": 0, - "max": 15, - "default": 9, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathColorCruiseOff", - "title": "패스색상,크루즈OFF(1)", - "descr": "(+10:띠)0:빨,1:주,2:노,3:초,4:파,5:남,6:보,7:고,8:흰,9:검", - "egroup": "DISP", - "etitle": "Show Path Color:CruiseOFF(1)", - "edescr": "", - "min": 0, - "max": 19, - "default": 1, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathModeLane", - "title": "패스종류,레인모드(11)", - "descr": "0:일반,1,2:사각,3,4:^^,5,6:사각,7,8:^^,9,10,11:smooth^^,12:smooth2^^,13:3줄,14:2줄,15:1줄", - "egroup": "DISP", - "etitle": "Show Path Mode:LaneMode(11)", - "edescr": "", - "min": 0, - "max": 15, - "default": 11, - "unit": 1 - }, - { - "group": "화면패스", - "name": "ShowPathColorLane", - "title": "패스색상,레인모드(3)", - "descr": "(+10:띠)0:빨,1:주,2:노,3:초,4:파,5:남,6:보,7:고,8:흰,9:검", - "egroup": "DISP", - "etitle": "Show Path Color:LaneMode(3)", - "edescr": "", - "min": 0, - "max": 19, - "default": 3, - "unit": 1 - }, - { - "group": "화면", - "name": "ShowPlotMode", - "title": "디버그플롯(0)", - "descr": "1:가속도,2:속도+가속도,3:모델,4:Lead,5:Lead2", - "egroup": "DISP", - "etitle": "Show Debug plot(0)", - "edescr": "1:Accel,2:Accel+vel,3:Model,4:Lead,5:Lead2", - "min": 0, - "max": 10, - "default": 0, - "unit": 1 - }, - { - "group": "조향일반", - "name": "UseLaneLineSpeed", - "title": "레인모드: 속도(0)", - "descr": "레인(차선)모드 사용하면 기존의 조향제어(lat_mpc)를 사용합니다.", - "egroup": "LAT", - "etitle": "Laneline: Speed(0)", - "edescr": "Lainline mode, lat_mpc control used", - "min": 0, - "max": 200, - "default": 0, - "unit": 1 - }, - { - "group": "조향일반", - "name": "UseLaneLineCurveSpeed", - "title": "레인모드: 커브속도(0)", - "descr": "레인(차선)모드를 지정턴속도 이하에서는 사용하지 않습니다.", - "egroup": "LAT", - "etitle": "Laneline: Curve Speed(0)", - "edescr": "Lainline mode, above curve speed", - "min": 0, - "max": 200, - "default": 0, - "unit": 10 - }, - { - "group": "조향일반", - "name": "AdjustLaneOffset", - "title": "레인모드: 차선조절옵셋 (0)cm", - "descr": "도로경계쪽으로 주행합니다.", - "egroup": "LAT", - "etitle": "LaneLine: AdjustLaneOffset (0)cm", - "edescr": "", - "min": 0, - "max": 500, - "default": 0, - "unit": 10 - }, - { - "group": "조향일반", - "name": "AdjustCurveOffset", - "title": "레인모드: 커브조절옵셋 (0)cm", - "descr": "커브시 안쪽으로 돌도록합니다.", - "egroup": "LAT", - "etitle": "LaneLine: AdjustCurveOffset (0)cm", - "edescr": "", - "min": 0, - "max": 500, - "default": 0, - "unit": 10 - }, - { - "group": "조향일반", - "name": "AdjustLaneTime", - "title": "레인모드: 시간지연보상 (13)x0.01", - "descr": "레인제어기의 지연시간을 보상하여, 빠른 조향을 유도함", - "egroup": "LAT", - "etitle": "LaneLine: AdjustLaneTime (13)x0.01", - "edescr": "LaneLine controller delay compensation", - "min": 0, - "max": 50, - "default": 13, - "unit": 1 - }, - { - "group": "조향일반", - "name": "LaneChangeNeedTorque", - "title": "차선변경 조향토크 사용", - "descr": "차선변경시 조향토크를 줘야 작동함", - "egroup": "LAT", - "etitle": "LaneChange use steering torque", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "HapticFeedbackWhenSpeedCamera", - "title": "핸들햅틱기능사용(0)", - "descr": "0:사용안함,1:진동,2:계기판,3:HUD표시", - "egroup": "START", - "etitle": "HapticFeedbackSpeedCamera(0)", - "edescr": "", - "min": 0, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "MaxAngleFrames", - "title": "MaxAngleFrames(89)", - "descr": "89:기본값", - "egroup": "START", - "etitle": "MaxAngleFrames(1)", - "edescr": "", - "min": 80, - "max": 100, - "default": 89, - "unit": 1 - }, - { - "group": "시작", - "name": "HyundaiCameraSCC", - "title": "HYUNDAI: CAMERA SCC", - "descr": "SCC배선 개조시 1로 설정", - "egroup": "START", - "etitle": "HYUNDAI: CAMERA SCC(0)", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "CanfdHDA2", - "title": "CANFD HDA2지원(0)", - "descr": "HDA2 강제인식, 일부CANFD차량 오류발생시 켜야함.", - "egroup": "START", - "etitle": "CANFD HDA2(0)", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "CanfdDebug", - "title": "CANFD Debug(0)", - "descr": "", - "egroup": "START", - "etitle": "CANFD Debug(0)", - "edescr": "", - "min": -1, - "max": 3, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "EnableRadarTracks", - "title": "레이더트랙사용(0)", - "descr": "일부차량만 지원됨, 2: SCC mix (문제있음)", - "egroup": "START", - "etitle": "Enable radar tracks(0)", - "edescr": "", - "min": 0, - "max": 2, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "HotspotOnBoot", - "title": "부팅시 핫스팟켜기", - "descr": "COMMA에 USIM을 장착한경우 핫스팟을 자동으로 켜주는 기능", - "egroup": "START", - "etitle": "Enable HotSpot(0)", - "edescr": "", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "시작", - "name": "SoundVolumeAdjust", - "title": "사운드볼륨조절(100%)", - "descr": "", - "egroup": "START", - "etitle": "Sound Volume adjust(100%)", - "edescr": "", - "min": 5, - "max": 200, - "default": 0, - "unit": 5 - }, - { - "group": "시작", - "name": "SoundVolumeAdjustEngage", - "title": "사운드볼륨조절,Engage(10%)", - "descr": "", - "egroup": "START", - "etitle": "Sound Volume adjust,Engage(10%)", - "edescr": "", - "min": 5, - "max": 200, - "default": 0, - "unit": 5 - }, - { - "group": "차량간격", - "name": "TFollowGap1", - "title": "TF(1): 차량간격시간1x0.01s(110)", - "descr": "단계별 차간 시간거리 설정", - "egroup": "FDIST", - "etitle": "TF(1): TimeFollow1x0.01s(110)", - "edescr": "", - "min": 40, - "max": 300, - "default": 110, - "unit": 5 - }, - { - "group": "차량간격", - "name": "TFollowGap2", - "title": "TF(2): 차량간격시간2x0.01s(120)", - "descr": "단계별 차간 시간거리 설정", - "egroup": "FDIST", - "etitle": "TF(2): TimeFollow2x0.01s(120)", - "edescr": "", - "min": 40, - "max": 300, - "default": 120, - "unit": 5 - }, - { - "group": "차량간격", - "name": "TFollowGap3", - "title": "TF(3): 차량간격시간3x0.01s(140)", - "descr": "단계별 차간 시간거리 설정", - "egroup": "FDIST", - "etitle": "TF(3): TimeFollow3x0.01s(140)", - "edescr": "", - "min": 40, - "max": 300, - "default": 140, - "unit": 5 - }, - { - "group": "차량간격", - "name": "TFollowGap4", - "title": "TF(4): 차량간격시간4x0.01s(160)", - "descr": "단계별 차간 시간거리 설정", - "egroup": "FDIST", - "etitle": "TF(4): TimeFollow4x0.01s(160)", - "edescr": "", - "min": 40, - "max": 300, - "default": 160, - "unit": 5 - }, - { - "group": "차량간격", - "name": "DynamicTFollow", - "title": "동적 차간거리 설정(0)", - "descr": "선행차와 거리에따른 동적 설정", - "egroup": "FDIST", - "etitle": "Dynamic TFollow", - "edescr": "", - "min": 0, - "max": 100, - "default": 0, - "unit": 5 - }, - { - "group": "차량간격", - "name": "DynamicTFollowLC", - "title": "동적 차간거리 설정 차선변경(100)%", - "descr": "차선변경시작시 차량간격을 좁힘.", - "egroup": "FDIST", - "etitle": "Dynamic TFollow LaneChange(100)%", - "edescr": "", - "min": 0, - "max": 100, - "default": 0, - "unit": 5 - }, - { - "group": "감속제어", - "name": "AutoCurveSpeedLowerLimit", - "title": "커브속도 최저속도(30)", - "descr": "최저속도설정", - "egroup": "FDIST", - "etitle": "AutoCurveSpeed LowerLimit(30)", - "edescr": "Lower limit speed", - "min": 10, - "max": 200, - "default": 30, - "unit": 10 - }, - { - "group": "주행튜닝", - "name": "StoppingAccel", - "title": "정지시작가속도x0.01(0)", - "descr": "정지시작가속도 조정, 0으로 하면 기존방식", - "egroup": "LONG", - "etitle": "StoppingStartAccelx0.01(0)", - "edescr": "", - "min": -100, - "max": 0, - "default": 0, - "unit": 10 - }, - { - "group": "가속설정", - "name": "MyDrivingMode", - "title": "드라이브모드:(3)", - "descr": "1:연비,2:안전,3:일반,4:고속(신호무시)", - "egroup": "ACCEL", - "etitle": "DrivingMode:Init(3)", - "edescr": "1:Eco,2:Safe,3:General,4:HighSpeed", - "min": 1, - "max": 4, - "default": 3, - "unit": 1 - }, - { - "group": "가속설정", - "name": "MyDrivingModeAuto", - "title": "드라이브모드: 자동", - "descr": "일반모드에서만 작동함", - "egroup": "ACCEL", - "etitle": "DrivingMode: Auto", - "edescr": "General Driving Mode only", - "min": 0, - "max": 1, - "default": 0, - "unit": 1 - }, - { - "group": "감속제어", - "name": "TrafficLightDetectMode", - "title": "신호감지 기능", - "descr": "0:신호감지안함, 1:신호정지만 감지, 2:신호정지/출발 모두", - "egroup": "SPEED", - "etitle": "TrafficLightDetectMode", - "edescr": "0:None,1:Stopping only, 2:Stop & Go", - "min": 0, - "max": 2, - "default": 2, - "unit": 1 - }, - { - "group": "감속제어", - "name": "AutoNaviSpeedCtrlEnd", - "title": "과속카메라감속 완료 시간(6초)", - "descr": "감속종료시점을 설정합니다.", - "egroup": "SPEED", - "etitle": "NaviSpeedEndingPoint(6s)", - "edescr": "Deceleration completion point", - "min": 3, - "max": 20, - "default": 6, - "unit": 1 - }, - { - "group": "감속제어", - "name": "AutoNaviSpeedBumpTime", - "title": "사고방지턱 감속완료시간시점(1s)", - "descr": "", - "egroup": "SPEED", - "etitle": "SpeedBumpEndingPoint(1s)", - "edescr": "", - "min": 1, - "max": 50, - "default": 1, - "unit": 1 - }, - { - "group": "감속제어", - "name": "AutoNaviSpeedBumpSpeed", - "title": "사고방지턱 속도(35Km/h)", - "descr": "", - "egroup": "SPEED", - "etitle": "SpeedBumpSpeed(35km/h)", - "edescr": "", - "min": 10, - "max": 100, - "default": 35, - "unit": 5 - }, - { - "group": "감속제어", - "name": "AutoNaviSpeedDecelRate", - "title": "과속카메라 감속율 x0.01m/s^2(200)", - "descr": "낮으면 멀리서부터 감속함", - "egroup": "SPEED", - "etitle": "NaviSpeedDecelRate 0.01m/s^2x(200)", - "edescr": "", - "min": 100, - "max": 300, - "default": 200, - "unit": 10 - }, - { - "group": "감속제어", - "name": "AutoNaviCountDownMode", - "title": "네비알림 카운트다운 방법", - "descr": "0: 알림없음, 1: 턴지점+속도, 2: 턴지점+속도+방지턱", - "egroup": "SPEED", - "etitle": "NaviCountDownMode", - "edescr": "", - "min": 0, - "max": 2, - "default": 2, - "unit": 1 - }, - { - "group": "감속제어", - "name": "TurnSpeedControlMode", - "title": "턴속도제어방법", - "descr": "0: 속도제어안함,1:비젼,2:비젼+경로,3:경로", - "egroup": "SPEED", - "etitle": "TurnSpeedControlMode", - "edescr": "", - "min": 0, - "max": 3, - "default": 1, - "unit": 1 - }, - { - "group": "감속제어", - "name": "MapTurnSpeedFactor", - "title": "경로턴속도반영비율(100%)", - "descr": "작으면 속도가 줄어듬", - "egroup": "SPEED", - "etitle": "MapTurnSpeedFactor", - "edescr": "", - "min": 50, - "max": 300, - "default": 100, - "unit": 10 - }, - { - "group": "감속제어", - "name": "AutoNaviSpeedSafetyFactor", - "title": "과속카메라 제한속도적용율(105)%", - "descr": "", - "egroup": "SPEED", - "etitle": "NaviSpeedLimitRatio(105)%", - "edescr": "", - "min": 80, - "max": 120, - "default": 105, - "unit": 1 - } - ] -} diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 73ed9b06a..321cb6e19 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -136,7 +136,7 @@ class Car: except Exception: pass - secoc_key = self.params.get("SecOCKey", encoding='utf8') + secoc_key = self.params.get("SecOCKey") if secoc_key is not None: saved_secoc_key = bytes.fromhex(secoc_key.strip()) if len(saved_secoc_key) == 16: @@ -270,7 +270,8 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + MD = self.sm['modelV2'] if self.sm.valid['modelV2'] else None + self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, MD) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index c029677c5..26d0fa210 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -442,7 +442,11 @@ class VCruiseCarrot: return button_kph, button_type, self.long_pressed def _carrot_command(self, v_cruise_kph, button_type, long_pressed): - carrot_speed = self.params_memory.get_int("CarrotSpeed") + if button_type != 0: + self.params_memory.put_int_nonblocking("CarrotSpeed", 0) + carrot_speed = 0 + else: + carrot_speed = self.params_memory.get_int("CarrotSpeed") if carrot_speed != 0: if carrot_speed > 0: if self.smartSpeedControl in [1,3]: @@ -454,7 +458,7 @@ class VCruiseCarrot: # v_cruise_kph = max(-carrot_speed, v_cruise_kph) elif self.smartSpeedControl == 2: v_cruise_kph = min(-carrot_speed, v_cruise_kph) - self.params_memory.put_int_nonblocking("CarrotSpeed", 0) + #self.params_memory.put_int_nonblocking("CarrotSpeed", 0) self._add_log(f"Carrot speed set to {v_cruise_kph}") if self.carrot_cmd_index_last != self.carrot_cmd_index: self.carrot_cmd_index_last = self.carrot_cmd_index @@ -598,9 +602,12 @@ class VCruiseCarrot: self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled") if self._paddle_mode > 0 and button_type in [ButtonType.paddleLeft, ButtonType.paddleRight]: # paddle button - self._cruise_control(-2, -1, "Cruise off & Ready (paddle)") - if self._paddle_mode == 2: - self._paddle_decel_active = True + if self._paddle_mode == 3: + self.carrot_cruise_active = True + else: + self._cruise_control(-2, -1, "Cruise off & Ready (paddle)") + if self._paddle_mode == 2: + self._paddle_decel_active = True elif self._paddle_decel_active: if not CC.enabled: self._cruise_control(1, -1, "Cruise on (paddle decel)") @@ -726,10 +733,10 @@ class VCruiseCarrot: elif self.v_ego_kph_set < 30: self._cruise_control(-1, 0, "Cruise off (gas speed)") elif self.xState == 3: - v_cruise_kph = self.v_ego_kph_set + v_cruise_kph = min(self.v_ego_kph_set, v_cruise_kph) self._cruise_control(-1, 3, "Cruise off (traffic sign)") elif self.v_ego_kph_set >= self.autoGasTokSpeed and not CC.enabled: - v_cruise_kph = self.v_ego_kph_set + v_cruise_kph = min(self.v_ego_kph_set, v_cruise_kph) self._cruise_control(1, -1 if self.aTarget > 0.0 else 0, "Cruise on (gas pressed)") elif self._brake_pressed_count == -1 and self._soft_hold_active == 0: if self.v_ego_kph_set > self.autoGasTokSpeed: diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index a32a729c2..f136744eb 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -101,6 +101,8 @@ class CarrotPlanner: self.dynamicTFollow = 0.0 self.dynamicTFollowLC = 0.0 + self.enableSpeedTF = 0 + self.personality = 1 self.cruiseMaxVals0 = 1.6 self.cruiseMaxVals1 = 1.6 @@ -133,6 +135,8 @@ class CarrotPlanner: self.atcType = "" self.atc_active = False + self._stop_x_rl = None + def _params_update(self): self.frame += 1 @@ -159,6 +163,7 @@ class CarrotPlanner: self.tFollowGap4 = self.params.get_float("TFollowGap4") / 100. self.dynamicTFollow = self.params.get_float("DynamicTFollow") / 100. self.dynamicTFollowLC = self.params.get_float("DynamicTFollowLC") / 100. + self.enableSpeedTF = self.params.get_int("EnableSpeedTF") elif self.params_count == 30: self.cruiseMaxVals0 = self.params.get_float("CruiseMaxVals0") / 100. self.cruiseMaxVals1 = self.params.get_float("CruiseMaxVals1") / 100. @@ -172,7 +177,7 @@ class CarrotPlanner: self.j_lead_factor = self.params.get_float("JLeadFactor3") / 100. self.eco_over_speed = self.params.get_int("CruiseEcoControl") self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01 - self.aChangeCostStaring = self.params.get_float("AChangeCostStarting") + self.aChangeCostStarting = self.params.get_float("AChangeCostStarting") self.trafficStopDistanceAdjust = self.params.get_float("TrafficStopDistanceAdjust") / 100. elif self.params_count >= 100: @@ -183,21 +188,73 @@ class CarrotPlanner: factor = self.myHighModeFactor if self.myDrivingMode == DrivingMode.High else self.mySafeFactor return np.interp(v_ego, A_CRUISE_MAX_BP_CARROT, cruiseMaxVals) * factor - def get_T_FOLLOW(self, personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.moreRelaxed: - self.jerk_factor = 1.0 - return self.tFollowGap4 - elif personality==log.LongitudinalPersonality.relaxed: - self.jerk_factor = 1.0 - return self.tFollowGap3 - elif personality==log.LongitudinalPersonality.standard: - self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.7 - return self.tFollowGap2 - elif personality==log.LongitudinalPersonality.aggressive: - self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.5 - return self.tFollowGap1 + def get_T_FOLLOW(self, personality=log.LongitudinalPersonality.standard, v_ego=0.0, a_ego=0.0): + # ------------------------------------------------------------ + # 1) Compute tf_target (your existing logic, unchanged behavior) + # ------------------------------------------------------------ + if self.enableSpeedTF < 0: + TF_SPEED_BPS = { + -1: [0, 30, 60, 90], + -2: [0, 40, 80, 120], + -3: [0, 50, 100, 150], + } + v_kph = v_ego * CV.MS_TO_KPH + bp = TF_SPEED_BPS.get(self.enableSpeedTF, [0, 30, 60, 90]) + + tf_target = float(np.interp(v_kph, bp, + [self.tFollowGap1, self.tFollowGap2, self.tFollowGap3, self.tFollowGap4])) + + self.jerk_factor = float(np.interp(v_kph, bp, [1.0, 0.7, 0.5, 0.5])) + personality = int(np.clip(np.digitize(v_kph, bp[1:], right=False), 0, 3)) + + if self.params_count % 100 == 0: + self.params.put_int_nonblocking("LongitudinalPersonality", personality) + self.personality = personality + else: - raise NotImplementedError("Longitudinal personality not supported") + tf_target = 1.0 + if personality == log.LongitudinalPersonality.moreRelaxed: + self.jerk_factor = 1.0 + tf_target = self.tFollowGap4 + elif personality == log.LongitudinalPersonality.relaxed: + self.jerk_factor = 1.0 + tf_target = self.tFollowGap3 + elif personality == log.LongitudinalPersonality.standard: + self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.7 + tf_target = self.tFollowGap2 + elif personality == log.LongitudinalPersonality.aggressive: + self.jerk_factor = 1.0 if self.myDrivingMode == DrivingMode.Safe else 0.5 + tf_target = self.tFollowGap1 + else: + raise NotImplementedError("Longitudinal personality not supported") + + if self.enableSpeedTF > 0: + reduce = self.enableSpeedTF * 0.01 + s = float(np.clip(v_ego * CV.MS_TO_KPH / 100.0, 0.0, 1.0)) + scale = (1.0 - reduce) + reduce * s + tf_target *= scale + + # ------------------------------------------------------------ + # 2) Decel-hold only (no smoothing constants) + # ------------------------------------------------------------ + if not hasattr(self, "_tf_applied") or self._tf_applied <= 0.0: + self._tf_applied = float(tf_target) + + DECEL_HOLD_A = -0.2 # m/s^2 + + # Only block TF shrink while decelerating strongly + if a_ego <= DECEL_HOLD_A and tf_target < self._tf_applied: + tf_applied = self._tf_applied + else: + tf_applied = tf_target + + # clamp to sensible range (using your configured gaps) + tf_min = float(min(self.tFollowGap1, self.tFollowGap2, self.tFollowGap3, self.tFollowGap4)) + tf_max = float(max(self.tFollowGap1, self.tFollowGap2, self.tFollowGap3, self.tFollowGap4)) + tf_applied = float(np.clip(tf_applied, tf_min, tf_max)) + + self._tf_applied = tf_applied + return tf_applied def _update_model_desire(self, sm): meta = sm['modelV2'].meta @@ -280,7 +337,7 @@ class CarrotPlanner: if sm.alive['carrotMan']: carrot_man = sm['carrotMan'] atc_turn_left = carrot_man.atcType in ["turn left", "atc left"] - trigger_start = self.carrot_staty_stop = False + trigger_start = self.carrot_stay_stop = False if atc_turn_left or sm['carState'].leftBlinker: if self.trafficState_carrot == 1 and carrot_man.trafficState == 3: # red -> left triggered trigger_start = True @@ -353,23 +410,13 @@ class CarrotPlanner: leadOne = radarstate.leadOne self.mySafeFactor = 1.0 - if leadOne.status and leadOne.radar and leadOne.vLead < 5 and leadOne.aLead < 0.2 and v_ego > 1.0: # �븵李④�� 留ㅼ슦 �뒓由ш굅�굹 �젙吏��븳寃쎌슦 - self.myDrivingMode = DrivingMode.Safe if self.myDrivingMode == DrivingMode.Eco: # eco self.mySafeFactor = self.myEcoModeFactor elif self.myDrivingMode == DrivingMode.Safe: #safe self.mySafeFactor = self.mySafeModeFactor - if self.frame % 20 == 0: # every 1 sec - vLead = 0 - aLead = 0 - dRel = 200 - if leadOne.status: - vLead = leadOne.vLead * CV.MS_TO_KPH - aLead = leadOne.aLead - dRel = leadOne.dRel - self.drivingModeDetector.update_data(v_ego_kph, vLead, carstate.aEgo, aLead, dRel) + self.drivingModeDetector.update_data(carstate, leadOne) v_cruise_kph = self.cruise_eco_control(v_ego_cluster_kph, v_cruise_kph) v_cruise_kph, atc_active = self._update_carrot_man(sm, v_ego_kph, v_cruise_kph) @@ -391,7 +438,18 @@ class CarrotPlanner: lead_detected = radarstate.leadOne.status # & radarstate.leadOne.radar self.xStop = self.update_stop_dist(x[31]) - stop_model_x = self.xStop + stop_model_x_raw = self.xStop + if self._stop_x_rl is None: + self._stop_x_rl = stop_model_x_raw + else: + max_close = v_ego * DT_MDL + 0.5 + if stop_model_x_raw > self._stop_x_rl: + self._stop_x_rl = stop_model_x_raw + else: + self._stop_x_rl = max(self._stop_x_rl - max_close, stop_model_x_raw) + + stop_model_x = self._stop_x_rl + stop_model_x_rl = self._stop_x_rl trafficState_last = self.trafficState #self.check_model_stopping(v, v_ego, self.xStop, y) @@ -403,7 +461,6 @@ class CarrotPlanner: self.trafficState = TrafficState.off #self.update_user_control() - if carstate.gasPressed or carstate.brakePressed: self.user_stop_distance = -1 @@ -414,7 +471,7 @@ class CarrotPlanner: elif self.xState == XState.e2eStopped: if carstate.gasPressed: self.xState = XState.e2eCruise #XState.e2ePrepare - elif lead_detected and (radarstate.leadOne.dRel - stop_model_x) < 2.0: + elif lead_detected and (radarstate.leadOne.dRel - stop_model_x_raw) < 2.0: self.xState = XState.lead elif self.stopping_count == 0: if self.trafficState == TrafficState.green and not self.carrot_stay_stop and not carstate.leftBlinker and self.trafficLightDetectMode != 1: @@ -429,7 +486,7 @@ class CarrotPlanner: #self.xState = XState.e2ePrepare self.xState = XState.e2eCruise self.traffic_starting_count = 10.0 / DT_MDL - elif lead_detected and (radarstate.leadOne.dRel - stop_model_x) < 2.0: + elif lead_detected and (radarstate.leadOne.dRel - stop_model_x_raw) < 2.0: self.xState = XState.lead else: if self.trafficState == TrafficState.green: @@ -439,7 +496,7 @@ class CarrotPlanner: self.comfort_brake = self.comfortBrake * 0.9 #self.comfort_brake = COMFORT_BRAKE self.trafficStopAdjustRatio = np.interp(v_ego_kph, [0, 100], [1.0, 0.7]) - stop_dist = self.xStop * np.interp(self.xStop, [0, 50], [1.0, self.trafficStopAdjustRatio]) ##占쏙옙占쏙옙占신몌옙占쏙옙 占쏙옙占쏙옙 占쏙옙占쏙옙占신몌옙 占쏙옙占쏙옙占쏙옙占쏙옙 + stop_dist = stop_model_x_rl * np.interp(stop_model_x_rl, [0, 50], [1.0, self.trafficStopAdjustRatio]) ##占쏙옙占쏙옙占신몌옙占쏙옙 占쏙옙占쏙옙 占쏙옙占쏙옙占신몌옙 占쏙옙占쏙옙占쏙옙占쏙옙 if stop_dist > 10.0: ### 10M占싱삼옙占싹띰옙占쏙옙, self.actual_stop_distance占쏙옙 占쏙옙占쏙옙占쏙옙트占쏙옙. self.actual_stop_distance = stop_dist stop_model_x = 0 @@ -465,7 +522,7 @@ class CarrotPlanner: elif self.trafficState == TrafficState.red and abs(carstate.steeringAngleDeg) < 30 and self.traffic_starting_count == 0: self.events.add(EventName.trafficStopping) self.xState = XState.e2eStop - self.actual_stop_distance = self.xStop + self.actual_stop_distance = stop_model_x_rl else: self.xState = XState.e2eCruise @@ -488,6 +545,10 @@ class CarrotPlanner: elif self.actual_stop_distance > 0: ## e2eStop, e2eStopped占싸곤옙占�.. stop_model_x = 0.0 + stopping_active = self.xState not in [XState.e2eStop, XState.e2eStopped] + if not stopping_active: + self._stop_x_rl = stop_model_x_raw + # self.debugLongText = ( # f"XState({str(self.xState)})," + # f"stop_x={stop_x:.1f}," + @@ -497,7 +558,13 @@ class CarrotPlanner: #占쏙옙호占쏙옙 占쏙옙占쏙옙占쏙옙占쏙옙 self.xState.value stop_dist = stop_model_x + self.actual_stop_distance - stop_dist = max(stop_dist, v_ego ** 2 / (self.comfort_brake * 2)) + stop_dist = max(stop_dist, 0.0) + + stopping_active = (self.xState in [XState.e2eStop, XState.e2eStopped]) + if stopping_active and stop_dist < 300.0: + stop_dist_soft = max(stop_dist - 1.0, 0.0) + v_soft = float(np.sqrt(max(0.0, 2.0 * self.comfort_brake * stop_dist_soft))) + v_cruise = min(v_cruise, v_soft) self.v_cruise = v_cruise self.stop_dist = stop_dist @@ -506,23 +573,55 @@ class CarrotPlanner: return v_cruise_kph - class DrivingModeDetector: def __init__(self): self.congested = False - self.speed_threshold = 2 # (km/h) - self.accel_threshold = 1.5 # (m/s^2) - self.distance_threshold = 12 # (m) - self.lead_speed_exit_threshold = 35 # (km/h) - def update_data(self, my_speed, lead_speed, my_accel, lead_accel, distance): - # 1. �젙泥� 議곌굔: �븵李④�� 媛�源뚯씠 �엳怨� �젙吏��맂 �긽�솴 - if distance <= self.distance_threshold and lead_speed <= self.speed_threshold: - self.congested = True + self.counter = 0 + self.enter_needed = 5 + self.exit_needed = 5 - # 2. 二쇳뻾 議곌굔: �븵李④�� 媛��냽�븯嫄곕굹 鍮좊Ⅴ寃� �씠�룞 - if lead_accel > self.accel_threshold or my_speed > self.lead_speed_exit_threshold or distance >= 200: - self.congested = False + self.distance_threshold = 12 + self.speed_threshold = 2 + self.accel_threshold = 1.5 + self.lead_speed_exit_threshold = 35 + + def update_data(self, carstate, leadOne): + my_speed = carstate.vEgo * CV.MS_TO_KPH + my_accel = carstate.aEgo + lead_speed = 0 + lead_accel = 0 + distance = 200 + if leadOne.status: + lead_speed = leadOne.vLead * CV.MS_TO_KPH + lead_accel = leadOne.aLead + distance = leadOne.dRel + + # ---- 吏꾩엯 議곌굔(OR濡� 臾띔린) ---- + enter = ( + (distance <= self.distance_threshold and lead_speed <= self.speed_threshold) or + (lead_speed < 5 and lead_accel < 0.2 and my_speed > 1.0 and distance < 200) + ) + + # ---- �깉異� 議곌굔(�뜑 蹂댁닔�쟻�쑝濡�) ---- + exit_ = ( + (lead_accel > self.accel_threshold) or + (my_speed > self.lead_speed_exit_threshold) or + (distance >= 200) + ) + + # ---- �뵒諛붿슫�뒪 濡쒖쭅 ---- + if enter: + self.counter += 1 + elif exit_: + self.counter -= 1 + + if self.counter >= self.enter_needed: + self.congested = True + self.counter = self.enter_needed + elif self.counter <= - self.exit_needed: + self.congested = False + self.counter = - self.exit_needed def get_mode(self): return DrivingMode.Safe if self.congested else DrivingMode.Normal diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index 50525c1e5..28f803121 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/selfdrive/carrot/carrot_man.py @@ -263,7 +263,7 @@ class CarrotMan: frame = 0 self.save_toggle_values() - carrot_speed = CarrotSpeed(neighbor_ring=2) + carrot_speed = CarrotSpeed(neighbor_ring=3) self.params_memory.put_int_nonblocking("CarrotSpeed", 0) rk = Ratekeeper(20, print_delay_threshold=None) @@ -276,6 +276,7 @@ class CarrotMan: self.v_cruise_change = 0 self._last_vt = 0.0 self.gas_pressed_count = 0 + self.brake_pressed_count = 0 self._last_viz_t = 0.0 while self.is_running: @@ -352,6 +353,8 @@ class CarrotMan: CC = self.sm['carControl'] v_ego = CS.vEgo a_ego = CS.aEgo + if CS.brakePressed: + self.brake_pressed_count = 200 gas_pressed = CS.gasPressed v_ego_kph = v_ego * 3.6 if gas_pressed: @@ -380,12 +383,18 @@ class CarrotMan: else: self.v_cruise_change = 0 return + v_cruise_apply = max(min(CS.vCruise, v_ego_kph), 20) + vt_last = self.params_memory.get_int("CarrotSpeed") + if vt_last != 0: + self.v_cruise_change = 0 + self.params_memory.put_int("CarrotSpeed", 0) now = time.monotonic() heading = self.carrot_serv.bearing #nPosAnglePhone lat, lon = self.carrot_serv.vpPosPointLat, self.carrot_serv.vpPosPointLon #self.carrot_serv.estimate_position(self.carrot_serv.phone_latitude, self.carrot_serv.phone_longitude, heading, v_ego, now - self.carrot_serv.last_update_gps_time_phone) - vt = carrot_speed.query_target_dist(lat, lon, heading, 0.0) + #vt = carrot_speed.query_target_dist(lat, lon, heading, 0.0) + viz_json, vt = carrot_speed.export_cells_around_with_here(lat, lon, heading, ring=4, max_points=64, lateral_m = 6.0) if self.v_cruise_change != 0: carrot_speed.add_sample(lat, lon, heading, v_cruise_apply if self.v_cruise_change > 0 else (- v_cruise_apply)) if self.v_cruise_change > 0: @@ -393,20 +402,22 @@ class CarrotMan: if self.v_cruise_change < 0: self.v_cruise_change += 1 else: - if self.gas_pressed_count > 0: + if self.brake_pressed_count > 0: + pass + elif self.gas_pressed_count > 0: vt = max(vt, v_cruise_apply) carrot_speed.add_sample(lat, lon, heading, vt) - - self.params_memory.put_int_nonblocking("CarrotSpeed", int(vt)) + else: + self.params_memory.put_int_nonblocking("CarrotSpeed", int(vt)) self._last_vt = vt if gas_pressed and a_ego < -0.5: #self._last_vt < 0.0: carrot_speed.invalidate_last_hit(window_s=2.0, action="clear") self.gas_pressed_count = max(0, self.gas_pressed_count - 1) + self.brake_pressed_count = max(0, self.brake_pressed_count - 1) if now - self._last_viz_t > 0.5: # 2Hz self._last_viz_t = now - viz_json = carrot_speed.export_cells_around(lat, lon, heading, ring=2, max_points=64) # 硫붾え由� Params�뿉 �벐�뒗 寃� 醫뗭쓬 (�뵒�뒪�겕 留먭퀬) self.params_memory.put_nonblocking("CarrotSpeedViz", viz_json) @@ -522,7 +533,7 @@ class CarrotMan: def make_send_message(self): msg = {} - msg['Carrot2'] = self.params.get("Version").decode('utf-8') + msg['Carrot2'] = self.params.get("Version") isOnroad = self.params.get_bool("IsOnroad") msg['IsOnroad'] = isOnroad msg['CarrotRouteActive'] = self.navi_points_active @@ -726,16 +737,16 @@ class CarrotMan: if car_selected is None: car_selected = "none" else: - car_selected = car_selected.decode('utf-8') + car_selected = car_selected - git_branch = Params().get("GitBranch").decode('utf-8') + git_branch = Params().get("GitBranch") try: ftp.mkd(git_branch) except Exception as e: print(f"Directory creation failed: {e}") ftp.cwd(git_branch) - directory = car_selected + " " + Params().get("DongleId").decode('utf-8') + directory = car_selected + " " + Params().get("DongleId") current_time = datetime.now().strftime("%Y%m%d-%H%M%S") filename = tmux_why + "-" + current_time + "-" + git_branch + ".txt" diff --git a/selfdrive/carrot/carrot_serv.py b/selfdrive/carrot/carrot_serv.py index bb784ba61..414fc6a18 100644 --- a/selfdrive/carrot/carrot_serv.py +++ b/selfdrive/carrot/carrot_serv.py @@ -219,12 +219,12 @@ class CarrotServ: # 瑥삣룚瑥�鼇�溫양쉰竊싦폍�뀍鵝욜뵪 LanguageSetting竊뚥툗 UI 岳앮똻訝��눜竊쎾썮���瑥삣룚�룾�꺗耶섇쑉�쉪 "lang" try: - lang_val = self.params.get('LanguageSetting', encoding='utf8') or self.params.get('lang', encoding='utf8') + lang_val = self.params.get('LanguageSetting') or self.params.get('lang') except Exception: lang_val = None if isinstance(lang_val, bytes): try: - lang_val = lang_val.decode('utf8') + lang_val = lang_val except Exception: lang_val = None if lang_val == "main_ko": @@ -659,15 +659,15 @@ class CarrotServ: gps_updated_navi = (now - self.last_update_gps_time_navi) < 3 bearing = self.nPosAngle - if gps_updated_phone: - self.bearing_offset = 0.0 + if gps_updated_navi: + bearing = self.nPosAngle + elif gps_updated_phone: + bearing = self.nPosAnglePhone elif self.gps_valid: bearing = self.nPosAngle = gps.bearingDeg - if self.gps_valid: - self.bearing_offset = 0.0 - elif self.active_carrot > 0: - bearing = self.nPosAnglePhone - self.bearing_offset = 0.0 + + self.bearing_offset = 0.0 + # TODO: �뿬湲곗꽌 bearing 蹂댁젙濡쒖쭅 異붽�� �븘�슂�븿. CC.orientationNED[2]瑜� �씠�슜�븯�뿬. #print(f"bearing = {bearing:.1f}, posA=={self.nPosAngle:.1f}, posP=={self.nPosAnglePhone:.1f}, offset={self.bearing_offset:.1f}, {gps_updated_phone}, {gps_updated_navi}") gpsDelayTimeAdjust = 0.0 @@ -676,7 +676,7 @@ class CarrotServ: external_gps_update_timedout = not (gps_updated_phone or gps_updated_navi) #print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}") - if self.gps_valid and external_gps_update_timedout: # �궡遺�GPS媛� �옄�룞�븯怨� carrotman�쑝濡쒕���꽣 gps�떊�샇媛� �뾾�뒗寃쎌슦 + if self.gps_valid and external_gps_update_timedout: # �궡遺�GPS媛� �옉�룞�븯怨� carrotman�쑝濡쒕���꽣 gps�떊�샇媛� �뾾�뒗寃쎌슦 self.vpPosPointLatNavi = gps.latitude self.vpPosPointLonNavi = gps.longitude self.last_calculate_gps_time = now #sm.recv_time[llk] diff --git a/selfdrive/carrot/carrot_speed.py b/selfdrive/carrot/carrot_speed.py index da6c1425e..fc0df231c 100644 --- a/selfdrive/carrot/carrot_speed.py +++ b/selfdrive/carrot/carrot_speed.py @@ -1,17 +1,23 @@ # -*- coding: utf-8 -*- """ -CarrotSpeedTable v2.1 (Params backend, JSON+gzip, 1e-4째 grid, 8 buckets) -- ����옣 �궎: "CarrotSpeedTable" -- �룷留�(JSON): {"format":"v5","dir_buckets":8,"cells":{"gy,gx":[[v,ts],...]} } -- gzip ����옣/濡쒕뱶 吏��썝 (湲곕낯 on). 湲곗〈 鍮꾩븬異� v2�룄 濡쒕뱶 媛��뒫. -- 寃⑹옄: �쐞/寃쎈룄 媛� 1e-4째 �뒪�깄(�븳援� �쐞�룄�뿉�꽌 �빟 9~11m) -- ����옣: �떒�씪 speed(遺��샇 �룷�븿)留� �빐�떦 ��� 1怨녹뿉 湲곕줉 - * �엯�젰 > 0: 湲곗〈 None/�쓬�닔/�뜑 �옉��� �뼇�닔硫� 媛깆떊(�뜑 �겙 +) - * �엯�젰 < 0: 湲곗〈 None/�뼇�닔/�뜙 �쓬�닔硫� 媛깆떊(�뜑 �옉��� -) -- 議고쉶: �쟾諛� lookahead ��� �넂 �뾾�쑝硫� �씠�썐 �깘�깋(ring=1) - * 蹂몄��: �떆媛� �븘�꽣 �뾾�쓬 - * �씠�썐: �삤�옒�맂 �뜲�씠�꽣留� �궗�슜(age �돟 120s) -- �젙由�(泥��냼) �뾾�쓬: �삤�옒�맂 �뜲�씠�꽣�룄 �쑀吏� +CarrotSpeedTable v6 (Params backend, JSON+gzip, 1e-4째 grid, 8 buckets) [EVENT-BASED, NO TS] +- KEY: "CarrotSpeedTable" +- FORMAT(v6): + { + "format":"v6", + "dir_buckets":8, + "cells":{"gy,gx":[eid,...]}, + "events":{ + "eid":{ + "cy": float, "cx": float, # center grid coords (float, averaging) + "b": int, # heading bucket (����몴) + "v": float, # speed (signed) + "n": int # merge count + }, ... + } + } +- v5 �뜲�씠�꽣�뒗 濡쒕뱶 �떆 �궘�젣(珥덇린�솕) +- �떆媛꾩젙蹂�(ts) ����옣/�궗�슜 �븞 �븿 """ import json, math, threading, time, gzip @@ -19,383 +25,409 @@ from typing import Optional, Tuple, Dict, List from openpilot.common.params import Params -# ---------- 吏��삤/�룄�슦誘� ---------- - def quantize_1e4(lat: float, lon: float) -> Tuple[int, int]: - gy = int(math.floor(lat * 1e4 + 0.5)) - gx = int(math.floor(lon * 1e4 + 0.5)) - return gy, gx + gy = int(math.floor(lat * 1e4 + 0.5)) + gx = int(math.floor(lon * 1e4 + 0.5)) + return gy, gx -def heading_to_bucket(heading_deg: float) -> int: - # 8 踰꾪궥 怨좎젙 - step = 45.0 # 360/8 - i = int((heading_deg % 360.0) // step) - if i < 0: return 0 - if i > 7: return 7 - return i +def heading_to_bucket(heading_deg: float, buckets: int = 8) -> int: + step = 360.0 / float(buckets) + i = int((heading_deg % 360.0) // step) + if i < 0: return 0 + if i >= buckets: return buckets - 1 + return i -DIR_8 = { - 0: ( 1, 0), # 遺� - 1: ( 1, 1), # 遺곷룞 - 2: ( 0, 1), # �룞 - 3: (-1, 1), # �궓�룞 - 4: (-1, 0), # �궓 - 5: (-1, -1), # �궓�꽌 - 6: ( 0, -1), # �꽌 - 7: ( 1, -1), # 遺곸꽌 -} +def bucket_diff(a: int, b: int, buckets: int = 8) -> int: + d = abs(int(a) - int(b)) % buckets + return min(d, buckets - d) def project_point(lat: float, lon: float, heading_deg: float, distance_m: float) -> Tuple[float, float]: - if distance_m <= 0.0: - return lat, lon - R = 6_371_000.0 - h = math.radians(heading_deg) - dlat = (distance_m * math.cos(h)) / R - dlon = (distance_m * math.sin(h)) / (R * math.cos(math.radians(lat))) - return lat + math.degrees(dlat), lon + math.degrees(dlon) + if distance_m <= 0.0: + return lat, lon + R = 6_371_000.0 + h = math.radians(heading_deg) + dlat = (distance_m * math.cos(h)) / R + dlon = (distance_m * math.sin(h)) / (R * math.cos(math.radians(lat))) + return lat + math.degrees(dlat), lon + math.degrees(dlon) def _is_gzip(data: bytes) -> bool: - return len(data) >= 2 and data[0] == 0x1F and data[1] == 0x8B + return len(data) >= 2 and data[0] == 0x1F and data[1] == 0x8B -# ---------- 硫붿씤 �겢�옒�뒪 ---------- - class CarrotSpeed: - KEY = "CarrotSpeedTable" + KEY = "CarrotSpeedTable" - def __init__(self, - neighbor_ring: int = 1, - neighbor_old_threshold_s: int = 120, - use_gzip: bool = True, - gzip_level: int = 5): - # 怨좎젙 �궗�뼇 - self.buckets = 8 + def __init__(self, + neighbor_ring: int = 1, + neighbor_old_threshold_s: int = 120, # v6: 誘몄궗�슜(�샇�솚) + use_gzip: bool = True, + gzip_level: int = 5): + self.buckets = 8 - # �뙆�씪誘명꽣 - self.neighbor_ring = max(0, int(neighbor_ring)) - self.neighbor_old_threshold_s = int(neighbor_old_threshold_s) - self.use_gzip = bool(use_gzip) - self.gzip_level = int(gzip_level) + # 湲곗〈 �씤�꽣�럹�씠�뒪 �샇�솚 �뙆�씪誘명꽣 + self.neighbor_ring = max(0, int(neighbor_ring)) + self.neighbor_old_threshold_s = int(neighbor_old_threshold_s) # v6: 誘몄궗�슜 + self.use_gzip = bool(use_gzip) + self.gzip_level = int(gzip_level) - # �궡遺� �긽�깭 - self._lock = threading.RLock() - # _cells[(gy,gx)] = [[value or None, ts(int seconds) or None] * 8] - self._cells: Dict[Tuple[int, int], List[List[Optional[float]]]] = {} - self._dirty = False - self._last_save = 0 - self._params = Params() + # v6 �씠踰ㅽ듃/癒몄�� �뙆�씪誘명꽣 + self.merge_ring = 2 + self.bucket_tol = 1 - self._load_from_params_if_exists() + self._lock = threading.RLock() + self._params = Params() + self._dirty = False + self._last_save = 0 - self._last_hit = None # (gy, gx, b, ts_when_read) - self._last_hit_read_ms = 0 # 諛�由ъ큹 + # v6 data + self._events: Dict[str, Dict] = {} + self._cells: Dict[Tuple[int, int], List[str]] = {} - # ----- �궡遺� �쑀�떥 ----- + self._load_from_params_if_exists() - def _ensure_cell(self, gy: int, gx: int) -> List[List[Optional[float]]]: - arr = self._cells.get((gy, gx)) - if arr is None: - arr = [[None, None] for _ in range(self.buckets)] # [v, ts] - self._cells[(gy, gx)] = arr - return arr + # invalidate�슜(�떆媛꾩갹 �뾾�씠 "留덉��留됱쑝濡� 諛섑솚�븳 �씠踰ㅽ듃"留� 湲곗뼲) + self._last_hit_eid: Optional[str] = None - def _now(self) -> int: - # int 珥� - return int(time.time()) + # ---------------- internals ---------------- - def _age(self, ts: Optional[float]) -> Optional[int]: - if ts is None: - return None - return self._now() - int(ts) + def _now(self) -> int: + return int(time.time()) - def _neighbor_indices(self, gy: int, gx: int) -> List[Tuple[int, int]]: - r = self.neighbor_ring - if r <= 0: - return [] - out = [] - for dy in range(-r, r + 1): - for dx in range(-r, r + 1): - if dy == 0 and dx == 0: - continue - out.append((gy + dy, gx + dx)) - return out + def _make_eid(self, gy: int, gx: int, b: int, nonce: int) -> str: + # ts瑜� �벐吏� �븡�쑝誘�濡� nonce濡� 異⑸룎留� �뵾�븿 + return f"{gy},{gx},{b},{nonce}" - def _neighbors_8(self, gy, gx): - for dy in (-1, 0, 1): - for dx in (-1, 0, 1): - if dy == 0 and dx == 0: - continue - yield gy + dy, gx + dx + def _index_add(self, eid: str, gy: int, gx: int) -> None: + lst = self._cells.get((gy, gx)) + if lst is None: + self._cells[(gy, gx)] = [eid] + else: + if eid not in lst: + lst.append(eid) - def _try_cell_bucket_old(self, arr, b): - v, ts = arr[b] - if v is None or ts is None: - return None, None - if self._now() - int(ts) < self.neighbor_old_threshold_s: - return None, None - return float(v), b - # ----- 怨듭슜 API ----- - def export_cells_around(self, lat: float, lon: float, - heading_deg: float, - ring: int = 1, max_points: int = 64) -> str: - """ - �쁽�옱 lat, lon 湲곗�� 二쇰�� 洹몃━�뱶(ring 踰붿쐞)�뿉�꽌 - 媛믪씠 �엳�뒗 ����뱾�쓣 (lat, lon, speed) 由ъ뒪�듃濡� JSON�쑝濡� 諛섑솚. - Params("CarrotSpeedViz")�뿉 洹몃��濡� �꽔�쓣 �슜�룄. - """ - gy0, gx0 = quantize_1e4(lat, lon) - b0 = heading_to_bucket(heading_deg) - pts = [] + def _index_remove(self, eid: str, gy: int, gx: int) -> None: + lst = self._cells.get((gy, gx)) + if not lst: + return + try: + lst.remove(eid) + except ValueError: + pass + if not lst: + self._cells.pop((gy, gx), None) - with self._lock: - for dy in range(-ring, ring + 1): - for dx in range(-ring, ring + 1): - gy = gy0 + dy - gx = gx0 + dx - arr = self._cells.get((gy, gx)) - if not arr: - continue + def _near_ids_in_ring(self, gy0: int, gx0: int, ring: int) -> List[str]: + out: List[str] = [] + r = max(0, int(ring)) + for dy in range(-r, r + 1): + for dx in range(-r, r + 1): + lst = self._cells.get((gy0 + dy, gx0 + dx)) + if lst: + out.extend(lst) + return out - # 癒쇱�� exact bucket(b0) - v, ts = arr[b0] - if v is not None: - cell_lat = (gy + 0.5) * 1e-4 - cell_lon = (gx + 0.5) * 1e-4 - pts.append([cell_lat, cell_lon, float(v)]) - if len(pts) >= max_points: - return json.dumps({"pts": pts}, separators=(",",":")) + def _grid_dist(self, gy: int, gx: int, cy: float, cx: float) -> float: + return abs(float(gy) - float(cy)) + abs(float(gx) - float(cx)) - # �뾾�떎硫� 醫�/�슦 - for b in ((b0 - 1) % self.buckets, (b0 + 1) % self.buckets): - v, ts = arr[b] - if v is None: - continue - cell_lat = (gy + 0.5) * 1e-4 - cell_lon = (gx + 0.5) * 1e-4 - pts.append([cell_lat, cell_lon, float(v)]) - if len(pts) >= max_points: - return json.dumps({"pts": pts}, separators=(",",":")) + # ---------------- public ---------------- + def export_cells_around_with_here(self, lat: float, lon: float, + heading_deg: float, + ring: int = 1, max_points: int = 64, + lateral_m: float = 6.0) -> Tuple[str, float]: + gy0, gx0 = quantize_1e4(lat, lon) + b0 = heading_to_bucket(heading_deg, self.buckets) + pts = [] - return json.dumps({"pts": pts}, separators=(",",":")) + def _pick_speed_at(lat0: float, lon0: float) -> float: + """�빐�떦 醫뚰몴�쓽 ���(quantize)�뿉�꽌留�(speed control�슜) speed 1媛� �꽑�깮. �뾾�쑝硫� 0.""" + gy, gx = quantize_1e4(lat0, lon0) + best_v = 0.0 + best_d = 1e18 + best_bd = 999 - def add_sample(self, lat: float, lon: float, heading_deg: float, speed_signed: float): - """ - �떒�씪 speed(遺��샇 �룷�븿) ����옣. - - 湲곗�� ���(�쁽�옱 �쐞移�) + heading 湲곗�� 醫�/�슦 1���, 2���源뚯�� �룞�씪 speed 湲곕줉 - - 媛� ��� �븞�뿉�꽌�뒗 heading 踰꾪궥 b��� b짹1 �꽭 媛� 踰꾪궥 紐⑤몢 媛숈�� 媛믪쑝濡� 媛깆떊. - - >0: 湲곗〈 �쓬�닔/None�룄 援먯껜, 湲곗〈 �뼇�닔硫� �룊洹좎쑝濡� �셿留뚰븯寃� 媛깆떊. - - <0: �빆�긽 �깉 �쓬�닔濡� �뜮�뼱�벐湲�(�룎諛� 媛먯냽 �슦�꽑). - ==0: 臾댁떆 - """ - v_in = round(float(speed_signed), 1) - if v_in == 0.0: - return + cand_ids = self._near_ids_in_ring(gy, gx, 0) # �쐟 �쁽�옱 ���留� + for eid in cand_ids: + ev = self._events.get(eid) + if not ev: + continue + bd = bucket_diff(b0, int(ev["b"]), self.buckets) + if bd > self.bucket_tol: + continue - # �쁽�옱 �쐞移섎�� 洹몃━�뱶濡� - gy0, gx0 = quantize_1e4(lat, lon) - b = heading_to_bucket(heading_deg) - now = self._now() + dd = self._grid_dist(gy, gx, float(ev["cy"]), float(ev["cx"])) + if (dd < best_d) or (dd == best_d and bd < best_bd): + best_d = dd + best_bd = bd + best_v = float(ev["v"]) - # bucket�뿉 �빐�떦�븯�뒗 �쟾吏� 諛⑺뼢 洹몃━�뱶 踰≫꽣 - dy_f, dx_f = DIR_8[b] + return best_v - # heading 湲곗�� 醫�/�슦 1���, 2��� (project_point �궗�슜 X) - # 醫� = �쟾吏꾨깹�꽣瑜� 90째 �쉶�쟾 (dy,dx) -> (dx,-dy) - # �슦 = �쟾吏꾨깹�꽣瑜� -90째 �쉶�쟾 (dy,dx) -> (-dx,dy) - dy_l1, dx_l1 = dx_f, -dy_f - dy_r1, dx_r1 = -dx_f, dy_f + with self._lock: + # 1) �몴�떆�슜 pts (二쇰�� ring) + cand_ids = self._near_ids_in_ring(gy0, gx0, ring) + seen = set() + for eid in cand_ids: + if eid in seen: + continue + seen.add(eid) - dy_l2, dx_l2 = 2 * dy_l1, 2 * dx_l1 - dy_r2, dx_r2 = 2 * dy_r1, 2 * dx_r1 + ev = self._events.get(eid) + if not ev: + continue + if bucket_diff(b0, int(ev["b"]), self.buckets) > self.bucket_tol: + continue - # 湲곕줉�븷 ����뱾: 以묒븰 + 醫�/�슦 1移� + 醫�/�슦 2移� - target_cells = { - (gy0, gx0), - (gy0 + dy_l1, gx0 + dx_l1), - (gy0 + dy_r1, gx0 + dx_r1), - (gy0 + dy_l2, gx0 + dx_l2), - (gy0 + dy_r2, gx0 + dx_r2), + # �몴�떆�슜(湲곗〈 洹몃��濡�) + cell_lat = (float(ev["cy"]) + 0.5) * 1e-4 + cell_lon = (float(ev["cx"]) + 0.5) * 1e-4 + pts.append([cell_lat, cell_lon, float(ev["v"])]) + if len(pts) >= max_points: + break + + # 2) speed: �쁽�옱 �쐞移� ��� + speed = _pick_speed_at(lat, lon) + + # 3) speed媛� 0�씠硫� 醫�/�슦 ���(heading 湲곗�� lateral)�뿉�꽌留� 媛��졇�삤湲� + if speed == 0.0 and lateral_m > 0.0: + # 醫� 癒쇱��, 洹� �떎�쓬 �슦 + yL, xL = project_point(lat, lon, heading_deg - 90.0, lateral_m) + sL = _pick_speed_at(yL, xL) + if sL != 0.0: + speed = sL + else: + yR, xR = project_point(lat, lon, heading_deg + 90.0, lateral_m) + sR = _pick_speed_at(yR, xR) + if sR != 0.0: + speed = sR + + return {"pts": pts}, float(speed) + + def export_cells_around1(self, lat: float, lon: float, + heading_deg: float, + ring: int = 1, max_points: int = 64) -> str: + gy0, gx0 = quantize_1e4(lat, lon) + b0 = heading_to_bucket(heading_deg, self.buckets) + pts = [] + + with self._lock: + cand_ids = self._near_ids_in_ring(gy0, gx0, ring) + seen = set() + for eid in cand_ids: + if eid in seen: + continue + seen.add(eid) + ev = self._events.get(eid) + if not ev: + continue + if bucket_diff(b0, int(ev["b"]), self.buckets) > self.bucket_tol: + continue + cell_lat = (float(ev["cy"]) + 0.5) * 1e-4 + cell_lon = (float(ev["cx"]) + 0.5) * 1e-4 + pts.append([cell_lat, cell_lon, float(ev["v"])]) + if len(pts) >= max_points: + break + + return json.dumps({"pts": pts}, separators=(",", ":")) + + def add_sample(self, lat: float, lon: float, heading_deg: float, speed_signed: float): + v_in = round(float(speed_signed), 1) + if v_in == 0.0: + return + + gy, gx = quantize_1e4(lat, lon) + b = heading_to_bucket(heading_deg, self.buckets) + + with self._lock: + cand_ids = self._near_ids_in_ring(gy, gx, self.merge_ring) + + best_eid = None + best_score = 1e18 + + for eid in cand_ids: + ev = self._events.get(eid) + if not ev: + continue + if bucket_diff(b, int(ev["b"]), self.buckets) > self.bucket_tol: + continue + + d = self._grid_dist(gy, gx, float(ev["cy"]), float(ev["cx"])) + # 嫄곕━ �슦�꽑. (speed 李⑥씠�뒗 援녹씠 �븞 �꽔�뼱�룄 �맖) + score = d + if score < best_score: + best_score = score + best_eid = eid + + if best_eid is None: + eid = self._make_eid(gy, gx, b, self._now()) + self._events[eid] = { + "cy": float(gy), + "cx": float(gx), + "b": int(b), + "v": float(v_in), + "n": 1, } + self._index_add(eid, gy, gx) + self._dirty = True + return - with self._lock: - for gy, gx in target_cells: - arr = self._ensure_cell(gy, gx) + # merge (以묒떖留� �닔�졃) + ev = self._events[best_eid] + n = int(ev.get("n", 1)) - # b, b-1, b+1 �꽭 踰꾪궥 紐⑤몢 媛숈�� �젙梨낆쑝濡� �뾽�뜲�씠�듃 - for off in (0, -1, +1): - bi = (b + off) % self.buckets - v_old, ts_old = arr[bi] + cy_old, cx_old = float(ev["cy"]), float(ev["cx"]) + cy_new = (cy_old * n + float(gy)) / (n + 1) + cx_new = (cx_old * n + float(gx)) / (n + 1) - if v_old is None: - # 泥섏쓬 �벐�뒗 踰꾪궥 - arr[bi] = [v_in, now] - else: - if v_in > 0.0: - # 媛��냽 �젙蹂�: 湲곗〈 �뼇�닔硫� �룊洹�, �쓬�닔硫� 援먯껜 - if v_old < 0.0: - # �쓬�닔 -> �뼇�닔濡� 諛붾�뚮㈃ �깉 �뼇�닔濡� 援먯껜 (ts�뒗 湲곗〈 �쑀吏�) - arr[bi] = [v_in, ts_old] - else: - new_val = round((v_old + v_in) / 2.0, 1) - arr[bi] = [new_val, ts_old] - else: - # 媛먯냽 �젙蹂�: �빆�긽 �깉 �쓬�닔濡� �뜮�뼱�벐湲�, ts�뒗 湲곗〈 �쑀吏� - arr[bi] = [v_in, ts_old] + old_cell = (int(round(cy_old)), int(round(cx_old))) + new_cell = (int(round(cy_new)), int(round(cx_new))) + if old_cell != new_cell: + self._index_remove(best_eid, old_cell[0], old_cell[1]) + self._index_add(best_eid, new_cell[0], new_cell[1]) - self._dirty = True + ev["cy"], ev["cx"] = cy_new, cx_new + ev["n"] = n + 1 - - def query_target(self, lat: float, lon: float, heading_deg: float, v_ego: float, - lookahead_s: float = 2.0) -> float: - dist = max(0.0, float(v_ego) * float(lookahead_s)) - return self.query_target_dist(lat, lon, heading_deg, dist) - - def query_target_dist(self, lat: float, lon: float, heading_deg: float, dist: float) -> float: - b = heading_to_bucket(heading_deg) + # �쐟 �빑�떖: 媛믪�� �빆�긽 理쒖떊�쑝濡� overwrite + ev["v"] = float(v_in) - cand_ds = [dist] - for off in (3.0, -3.0): - d2 = dist + off - if d2 >= 0.0: - cand_ds.append(d2) + # (�꽑�깮) ����몴 諛⑺뼢�룄 理쒖떊�쑝濡� 媛깆떊�븯怨� �떢�쑝硫� �븘�옒 1以� 耳쒕룄 �맖 + # ev["b"] = int(b) - with self._lock: - for d in cand_ds: - y, x = project_point(lat, lon, heading_deg, d) - gy, gx = quantize_1e4(y, x) + self._dirty = True - arr = self._cells.get((gy, gx)) - if not arr: - continue + def invalidate_last_hit(self, window_s: float = 2.0, action: str = "clear") -> bool: + """ + ts ����옣�쓣 �븞 �븯誘�濡� window_s�뒗 臾댁떆. + - clear: �씠踰ㅽ듃 �궘�젣 + - age_bump: �씠踰ㅽ듃 �셿�솕(0�뿉 媛�源앷쾶) + """ + eid = self._last_hit_eid + if not eid: + return False - v, b_sel = self._try_cell_bucket_old(arr, b) - if v is not None: - now_sec = int(time.time()) - self._last_hit = (gy, gx, b_sel, now_sec) - self._last_hit_read_ms = int(time.time() * 1000) - return v + with self._lock: + ev = self._events.get(eid) + if not ev: + return False - return 0.0 - - def invalidate_last_hit(self, window_s: float = 2.0, action: str = "clear") -> bool: - if self._last_hit is None: - return False - gy, gx, b, read_ts = self._last_hit - now = int(time.time()) - if (now - int(read_ts)) > window_s: - return False + if action == "clear": + cell = (int(round(float(ev["cy"]))), int(round(float(ev["cx"])))) + self._index_remove(eid, cell[0], cell[1]) + self._events.pop(eid, None) + else: + v = float(ev["v"]) + if v < 0.0: + ev["v"] = min(0.0, v + 1.0) + else: + ev["v"] = v + 1.0 - with self._lock: - arr = self._cells.get((gy, gx)) - if not arr: - return False + self._dirty = True + return True - # b, b-1, b+1 紐⑤몢 invalidate - for off in (0, -1, +1): - bi = (b + off) % self.buckets - v, ts = arr[bi] + def maybe_save(self, interval_s: int = 60) -> None: + now = self._now() + if (not self._dirty) or (now - self._last_save < int(interval_s)): + return + self.save() - if action == "clear": - if v is not None and v < 0.0: - arr[bi] = [None, None] - else: # "age_bump" - if v is not None: - arr[bi] = [v, now] - else: - # 媛믪씠 �뾾�쑝硫� �꽆�뼱媛�湲곕쭔 (洹� 踰꾪궥留� skip) - pass + def save(self) -> None: + payload = self._encode_payload() + self._params.put_nonblocking(self.KEY, payload) + self._last_save = self._now() + self._dirty = False - self._dirty = True - - return True - - def maybe_save(self, interval_s: int = 60) -> None: - now = self._now() - if (not self._dirty) or (now - self._last_save < interval_s): - return + def close(self) -> None: + try: + if self._dirty: self.save() + except Exception: + pass - def save(self) -> None: - payload = self._encode_payload() - self._params.put_nonblocking(self.KEY, payload) - self._last_save = self._now() + # ---------------- serialization ---------------- + + def _encode_payload(self) -> bytes: + with self._lock: + obj = { + "format": "v6", + "dir_buckets": self.buckets, + "cells": {f"{gy},{gx}": lst for (gy, gx), lst in self._cells.items()}, + "events": self._events, + } + raw = json.dumps(obj, separators=(",", ":")).encode("utf-8") + return gzip.compress(raw, compresslevel=self.gzip_level) if self.use_gzip else raw + + def _load_from_params_if_exists(self) -> None: + raw = self._params.get(self.KEY) + if not raw: + return + try: + b = raw + if _is_gzip(b): + b = gzip.decompress(b) + data = json.loads(b.decode("utf-8")) + + if data.get("format") != "v6": + # v5 �룷�븿 紐⑤몢 �궘�젣 + try: + self._params.remove(self.KEY) + except Exception: + pass + with self._lock: + self._events = {} + self._cells = {} + self._dirty = False + return + + buckets = int(data.get("dir_buckets", 8)) + if buckets != 8: + try: + self._params.remove(self.KEY) + except Exception: + pass + with self._lock: + self._events = {} + self._cells = {} + self._dirty = False + return + + events_in = data.get("events", {}) + events: Dict[str, Dict] = {} + if isinstance(events_in, dict): + for eid, ev in events_in.items(): + if not isinstance(ev, dict): + continue + if "cy" not in ev or "cx" not in ev or "b" not in ev or "v" not in ev: + continue + events[str(eid)] = { + "cy": float(ev["cy"]), + "cx": float(ev["cx"]), + "b": int(ev["b"]), + "v": float(ev["v"]), + "n": int(ev.get("n", 1)), + } + + # �씤�뜳�뒪 rebuild(�젙�빀�꽦 �슦�꽑) + rebuilt_cells: Dict[Tuple[int, int], List[str]] = {} + for eid, ev in events.items(): + gy = int(round(float(ev["cy"]))) + gx = int(round(float(ev["cx"]))) + lst = rebuilt_cells.get((gy, gx)) + if lst is None: + rebuilt_cells[(gy, gx)] = [eid] + else: + lst.append(eid) + + with self._lock: + self._events = events + self._cells = rebuilt_cells self._dirty = False - def close(self) -> None: - try: - if self._dirty: - self.save() - except Exception: - pass - - # ----- 吏곷젹�솕 ----- - - def _encode_payload(self) -> bytes: - with self._lock: - cells = {} - for (gy, gx), arr in self._cells.items(): - key = f"{gy},{gx}" - # arr: [[v, ts], ...] (ts�뒗 int �삉�뒗 None) - cells[key] = [[None if v is None else float(v), - None if ts is None else int(ts)] for (v, ts) in arr] - obj = {"format": "v5", "dir_buckets": self.buckets, "cells": cells} - raw = json.dumps(obj, separators=(",", ":")).encode("utf-8") - if self.use_gzip: - return gzip.compress(raw, compresslevel=self.gzip_level) - return raw - - def _load_from_params_if_exists(self) -> None: - raw = self._params.get(self.KEY) - if not raw: - return - try: - data_bytes = raw - if _is_gzip(data_bytes): - data_bytes = gzip.decompress(data_bytes) - data = json.loads(data_bytes.decode("utf-8")) - - # v3 �븘�땲硫� �궘�젣/珥덇린�솕 - if data.get("format") != "v5": - self._params.remove(self.KEY) - with self._lock: - self._cells = {} - self._dirty = False - return - - buckets = int(data.get("dir_buckets", 8)) - if buckets != 8: - # 踰꾪궥 遺덉씪移섎룄 �궘�젣/珥덇린�솕 - self._params.remove(self.KEY) - with self._lock: - self._cells = {} - self._dirty = False - return - - restored: Dict[Tuple[int, int], List[List[Optional[float]]]] = {} - for key, arr in data.get("cells", {}).items(): - gy, gx = map(int, key.split(",")) - fixed: List[List[Optional[float]]] = [] - if isinstance(arr, list) and len(arr) == 8: - for pair in arr: - if isinstance(pair, list) and len(pair) == 2: - v, ts = pair - v2 = None if v is None else float(v) - # ts�뒗 int濡� 媛뺤젣 - ts2 = None if ts is None else int(ts) - fixed.append([v2, ts2]) - else: - fixed.append([None, None]) - else: - fixed = [[None, None] for _ in range(8)] - restored[(gy, gx)] = fixed - - with self._lock: - self._cells = restored - self._dirty = False - - except Exception: - # �뙆�떛 �떎�뙣 �떆 �븞�쟾 珥덇린�솕 - self._params.delete(self.KEY) - with self._lock: - self._cells = {} - self._dirty = False + except Exception: + try: + self._params.remove(self.KEY) + except Exception: + pass + with self._lock: + self._events = {} + self._cells = {} + self._dirty = False diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index b9818b8ac..77935ee33 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -837,12 +837,12 @@ "group": "踰꾪듉�꽕�젙", "name": "PaddleMode", "title": "�뙣�뱾�떆�봽�듃紐⑤뱶(0)", - "descr": "�뙣�뱾�돩�봽�듃 �옉�룞 �떆 �룞�옉\n 0:�겕猷⑥쫰ON\n 1:�겕猷⑥쫰���湲�\n 2: �옄�룞媛먯냽", + "descr": "0:�겕猷⑥쫰ON, 1:�겕猷⑥쫰���湲�, 2:�옄�룞媛먯냽, 3:CarrotCruise ", "egroup": "BUTN", "etitle": "PaddleShift Mode(0)", - "edescr": "After Regen paddle, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready", + "edescr": "After Regen paddle, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready, 3: CarrotCruise", "min": 0, - "max": 2, + "max": 3, "default": 1, "unit": 1 }, @@ -1410,10 +1410,10 @@ "group": "�떆�옉", "name": "EnableRadarTracks", "title": "�젅�씠�뜑�듃�옓,SCC�젅�씠�뜑�꽕�젙(0)", - "descr": "-1: SCC�젅�씠�뜑�궗�슜(SCC �빆�떆�궗�슜), 0: SCC�젅�씠�뜑�궗�슜\n 1: �젅�씠�뜑�듃�옓 �궗�슜, 2: �젅�씠�뜑�듃�옓 �궗�슜(SCC �빆�떆�궗�슜)\n 3:�젅�씠�뜑�듃�옓 �궗�슜(�겮�뼱�뱾湲�,�뒪�뀛�뒪李⑤웾寃�異�)", + "descr": "-1: SCC�궗�슜(�젙吏�李�), 0: SCC�젅�씠�뜑�궗�슜\n 1: �젅�씠�뜑�듃�옓, 2: �젅�씠�뜑�듃�옓(SCC�궗�슜)\n 3:�젅�씠�뜑�듃�옓(�겮�뼱�뱾湲�,�뒪�뀛�뒪李⑤웾寃�異�)", "egroup": "START", "etitle": "Enable radar tracks(0)", - "edescr": "-1: SCC Radar(SCC Always On), 0: SCC Radar\n, 1: Radar Track Enabled, 2: Radar Track Enabled(SCC Always On)\n 3:Radar Track Eanbled(Cutin detect and etc)", + "edescr": "-1: use SCC(Stopped car), 0: SCC Radar\n, 1: RadarTrack, 2: RadarTrack(use SCC)\n 3:RadarTrack(Cutin detect and etc)", "min": -1, "max": 3, "default": 0, @@ -1436,12 +1436,12 @@ "group": "�떆�옉", "name": "EnableCornerRadar", "title": "肄붾꼫�젅�씠�뜑 �궗�슜(0)", - "descr": "�겮�뼱�뱶�뒗 李⑤웾 寃�異�(hda2 李⑤웾)", + "descr": "1:肄붾꼫�젅�씠�뜑�솢�꽦�솕,2:�겮�뼱�뱶�뒗 李⑤웾 寃�異�", "egroup": "START", "etitle": "Enable Corner radar(0)", - "edescr": "Cutin Detection for CCNC car", + "edescr": "1:Enable Corner radar, 2: Cutin Detection for CCNC car", "min": 0, - "max": 1, + "max": 2, "default": 0, "unit": 1 }, @@ -1562,6 +1562,19 @@ "default": 0, "unit": 1 }, + { + "group": "李⑤웾媛꾧꺽", + "name": "EnableSpeedTF", + "title": "�냽�룄蹂� 李④컙嫄곕━ �꽕�젙(0)", + "descr": "-1: �옄�룞�떒怨꾩꽑�깮, >0: ����냽 李④컙嫄곕━ 鍮꾩쑉 媛먯냼", + "egroup": "FDIST", + "etitle": "EnableSpeedTF(0)", + "edescr": "-1:Auto select by speed, >0: Reduce TFs by ratio", + "min": -3, + "max": 50, + "default": 0, + "unit": 1 + }, { "group": "李⑤웾媛꾧꺽", "name": "DynamicTFollowLC", diff --git a/selfdrive/controls/beep.py b/selfdrive/controls/beep.py index c6d2a9b32..95b1f1ac9 100755 --- a/selfdrive/controls/beep.py +++ b/selfdrive/controls/beep.py @@ -1,14 +1,16 @@ #!/usr/bin/env python3 import subprocess import time -from cereal import car, messaging -from openpilot.common.realtime import Ratekeeper import threading +from cereal import car, messaging +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper AudibleAlert = car.CarControl.HUDControl.AudibleAlert class Beepd: def __init__(self): + self.params = Params() self.current_alert = AudibleAlert.none self.enable_gpio() self.startup_beep() @@ -30,6 +32,8 @@ class Beepd: encoding='utf8') def _beep(self, on): + if self.params.get_int("SoundVolumeAdjust") <= 5: + on = False val = "1" if on else "0" subprocess.run(f"echo \"{val}\" | sudo tee /sys/class/gpio/gpio42/value", shell=True, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index de692775a..5093c3246 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -151,21 +151,20 @@ class Controls: if steer_actuator_delay == 0.0: steer_actuator_delay = self.sm['liveDelay'].lateralDelay + def smooth_value(val, prev_val, tau): + alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val + if not CC.latActive: new_desired_curvature = self.curvature elif self.lanefull_mode_enabled: if len(lat_plan.curvatures) == 0: new_desired_curvature = self.curvature else: - def smooth_value(val, prev_val, tau): - alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1 - return alpha * val + (1 - alpha) * prev_val - curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds, lat_plan.distances) - new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds) - else: - new_desired_curvature = model_v2.action.desiredCurvature + else: + new_desired_curvature = smooth_value(model_v2.action.desiredCurvature, self.desired_curvature, 0.1) self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) @@ -188,41 +187,6 @@ class Controls: return CC, lac_log - - def _update_side(self, side: str, leads2, road_edge, bsd_state, hudControl): - def ema(prev, curr, a=0.02): - return curr if prev is None else prev * (1 - a) + curr * a - - def set_hud(side_cap, name, val): - setattr(hudControl, f"lead{side_cap}{name}", float(val if val is not None else 0.0)) - - st = self.side_state[side] - if road_edge <= 2.0 or not leads2: - st["main"] = {"dRel": None, "lat": None} - st["sub"] = {"dRel": None, "lat": None} - if not bsd_state: - return - - lead_main = leads2[0] if len(leads2) > 0 else None - side_cap = side.capitalize() - - if bsd_state: - set_hud(side_cap, "Dist2", 1) - set_hud(side_cap, "Lat2", 3.2) - # 泥� 踰덉㎏媛� 10m �씠�궡�씪硫� sub �뾽�뜲�씠�듃 + �몢 踰덉㎏瑜� main�쑝濡� - elif len(leads2) > 1 and lead_main.dRel < 10: - st["sub"]["dRel"] = ema(st["sub"]["dRel"], lead_main.dRel) - st["sub"]["lat"] = ema(st["sub"]["lat"], abs(lead_main.dPath)) - set_hud(side_cap, "Dist2", st["sub"]["dRel"]) - set_hud(side_cap, "Lat2", st["sub"]["lat"]) - lead_main = leads2[1] - - if len(leads2) > 0: - st["main"]["dRel"] = ema(st["main"]["dRel"], lead_main.dRel) - st["main"]["lat"] = ema(st["main"]["lat"], abs(lead_main.dPath)) - set_hud(side_cap, "Dist", st["main"]["dRel"]) - set_hud(side_cap, "Lat", st["main"]["lat"]) - def publish(self, CC, lac_log): CS = self.sm['carState'] @@ -282,10 +246,26 @@ class Controls: hudControl.leadDPath = leadOne.dPath meta = self.sm['modelV2'].meta - hudControl.modelDesire = 1 if meta.desire == log.Desire.turnLeft else 2 if meta.desire == log.Desire.turnRight else 0 - - self._update_side("left", radarState.leadsLeft2, meta.distanceToRoadEdgeLeft, CS.leftBlindspot, hudControl) - self._update_side("right", radarState.leadsRight2, meta.distanceToRoadEdgeRight, CS.rightBlindspot, hudControl) + if False: # command + desire_map = { + log.Desire.turnLeft: 1, + log.Desire.turnRight: 2, + log.Desire.laneChangeLeft: 3, + log.Desire.laneChangeRight: 4, + } + hudControl.modelDesire = desire_map.get(meta.desire, 0) + else: # model. + hud_desire = 0 + if len(meta.desireState) > 4: + if meta.desireState[1] > 0.1: + hud_desire = 1 # turnLeft + elif meta.desireState[2] > 0.1: + hud_desire = 2 # turnRight + elif meta.desireState[3] > 0.1: + hud_desire = 3 # laneChangeLeft + elif meta.desireState[4] > 0.1: + hud_desire = 4 # laneChangeRight + hudControl.modelDesire = hud_desire hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 9794aa443..21698acef 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -257,6 +257,8 @@ class DesireHelper: def _check_desire_state(self, modeldata, carstate, maneuver_type): desire_state = modeldata.meta.desireState + orientation_rate = abs(modeldata.orientationRate.z[5]) + orientation_rate_future = abs(modeldata.orientationRate.z[15]) # turnLeft + turnRight �솗瑜� self.turn_desire_state = (desire_state[1] + desire_state[2]) > 0.1 @@ -266,7 +268,7 @@ class DesireHelper: # self.desire_disable_count = max(0, self.desire_disable_count - 1) # steeringAngle �꼫臾� �겕硫� turn �옄泥대�� �씪�젙 �떆媛� 留됯린 - if abs(carstate.steeringAngleDeg) > 80: + if maneuver_type == "turn" and abs(carstate.steeringAngleDeg) > 80 and orientation_rate_future < orientation_rate: self.turn_disable_count = int(10.0 / DT_MDL) else: self.turn_disable_count = max(0, self.turn_disable_count - 1) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a1611c005..d9eb9c513 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -15,7 +15,7 @@ MAX_VEL_ERR = 5.0 # m/s # EU guidelines MAX_LATERAL_JERK = 5.0 # m/s^3 MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 -MAX_CURVATURE_DELTA_FRAME = 0.03 #0.019 # about 3 degree / DT_CTRL +MAX_LATERAL_ACCEL_NO_ROLL_LOW_SPEED = 4.5 # m/s^2 def apply_deadzone(error, deadzone): if error > deadzone: @@ -84,21 +84,15 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll): prev_curvature + max_curvature_rate * DT_CTRL) roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY - max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation - min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation + max_lateral_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL + if v_ego < 100 / 3.6: # 100 km/h + max_lateral_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_LOW_SPEED + max_lat_accel = max_lateral_accel_no_roll + roll_compensation + min_lat_accel = -max_lateral_accel_no_roll + roll_compensation new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) - - new_curvature = np.clip( - new_curvature, - prev_curvature - MAX_CURVATURE_DELTA_FRAME, - prev_curvature + MAX_CURVATURE_DELTA_FRAME - ) - - was_limited = limited_accel or limited_max_curv or (abs(new_curvature - prev_curvature) >= MAX_CURVATURE_DELTA_FRAME) - - return float(new_curvature), was_limited + return float(new_curvature), limited_accel or limited_max_curv def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: # ToDo: Try relative error, and absolute speed diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d061f4a38..9cf3ee3fa 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -362,9 +362,9 @@ class LongitudinalMpc: self.max_a = max_a def update(self, carrot, reset_state, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - t_follow = carrot.get_T_FOLLOW(personality) v_ego = self.x0[1] a_ego = self.x0[2] + t_follow = carrot.get_T_FOLLOW(personality, v_ego, a_ego) self.status = radarstate.leadOne.status or radarstate.leadTwo.status if radarstate.leadOne.status: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index aa7346545..d1e92df3d 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -54,32 +54,36 @@ def limit_accel_in_turns_org(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -def limit_accel_in_turns(v_ego, curvature, a_target, a_lat_max): +def limit_accel_in_turns(v_ego, curvature, a_target, a_lat_max, + safety_ratio=0.70, # 0.60~0.85 (�옉�쓣�닔濡� �뜑 �뼂�쟾) + min_v=0.1): """ v_ego : m/s - curvature: 1/m (議고뼢�뿉�꽌 �씠誘� �굹�삩 怨〓쪧, sign �룷�븿) - a_target : [a_min, a_max] - a_lat_max: �뿀�슜 理쒕�� �슒媛��냽 (�삁: 6.0 ~ 8.0 m/s^2) + curvature: 1/m (sign �룷�븿) + a_target : [a_min, a_max] (m/s^2) + a_lat_max: �뿀�슜 理쒕�� �슒媛��냽 (m/s^2) + + safety_ratio: + a_lat_max�뿉 �냼�봽�듃 留덉쭊�쓣 二쇰뒗 鍮꾩쑉. + �삁) a_lat_max=4, safety_ratio=0.7 -> �떎�궗�슜 �븳怨� 2.8濡� 怨꾩궛. return : [a_min, �젣�븳�맂 a_max] """ - # �븘二� ����냽�씠硫� 援녹씠 �젣�븳 �븞 嫄몄뼱�룄 �맖 - if v_ego < 0.1 or a_lat_max <= 0.0: + if v_ego < min_v or a_lat_max <= 0.0: return a_target - # �슒媛��냽 - a_y = v_ego * v_ego * curvature # m/s^2 - a_y_abs = abs(a_y) - a_lat_max_abs = abs(a_lat_max) + a_lat_eff = abs(a_lat_max) * float(safety_ratio) - # a_total^2 = a_x^2 + a_y^2 <= a_lat_max^2 �씪怨� 蹂닿퀬, - # �궓��� a_x �뿬�쑀瑜� 怨꾩궛 - if a_y_abs >= a_lat_max_abs: + # �슒媛��냽 + a_y_abs = abs((v_ego * v_ego) * curvature) + + # �궓��� 醫낃���냽 �뿬�쑀 (�썝�삎 寃쎄퀎) + if a_y_abs >= a_lat_eff: a_x_allowed = 0.0 else: - a_x_allowed = math.sqrt(a_lat_max_abs**2 - a_y_abs**2) + a_x_allowed = math.sqrt(a_lat_eff * a_lat_eff - a_y_abs * a_y_abs) - # a_target = [min, max] 以묒뿉�꽌 max留� 以꾩뿬以� + # a_target = [min, max] 以� max留� �젣�븳 return [a_target[0], min(a_target[1], a_x_allowed)] class LongitudinalPlanner: @@ -167,7 +171,7 @@ class LongitudinalPlanner: accel_limits = [A_CRUISE_MIN, carrot.get_carrot_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg #accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) - a_lat_max = 4.0 + a_lat_max = 3.0 accel_limits_turns = limit_accel_in_turns(v_ego, sm['controlsState'].desiredCurvature, accel_limits, a_lat_max) else: accel_limits = [ACCEL_MIN, ACCEL_MAX] diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a3c445e6a..5a91db941 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -28,6 +28,14 @@ RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +def laplacian_pdf(x: float, mu: float, b: float): + diff = abs(x - mu) / max(b, 1e-4) + return 0.0 if diff > 50.0 else math.exp(-diff) + +def clamp(x: float, lo: float, hi: float) -> float: + return float(np.clip(x, lo, hi)) + + class Track: def __init__(self, identifier: int): self.identifier = identifier @@ -42,9 +50,14 @@ class Track: self.in_lane_prob = 0.0 self.in_lane_prob_future = 0.0 - def update(self, md, pt, ready, radar_reaction_factor, radar_lat_factor): + self.dPath = 0.0 - #pt_yRel = -leads_v3[0].y[0] if track_id in [0, 1] and pt.yRel == 0 and self.ready and leads_v3[0].prob > 0.5 else pt.yRel + # ---- noise filter state (new) ---- + self._vLead_last = 0.0 + self._vLead_filt = 0.0 + self._vLead_filt_init = False + + def update(self, md, pt, ready, radar_reaction_factor, radar_lat_factor): self.dRel = pt.dRel self.yRel = pt.yRel self.vRel = pt.vRel @@ -53,15 +66,17 @@ class Track: self.aLead = self.aLeadK = pt.aLead self.jLead = pt.jLead self.yvLead = pt.yvRel - - self.measured = pt.measured # measured or estimate + + self.measured = pt.measured if not self.measured: self.cnt = 0 + # optional: also reset filter init when track is not measured + self._vLead_filt_init = False self.yRel_future = self.yRel + self.yvLead * radar_lat_factor self.dRel_future = self.dRel + self.vLead * radar_lat_factor if ready: - self.d_path(md) #self.yRel + np.interp(self.dRel, md.position.x, md.position.y) + self.d_path(md) a_lead_threshold = 0.5 * radar_reaction_factor if abs(self.aLead) < a_lead_threshold and abs(self.jLead) < 0.5: @@ -75,22 +90,49 @@ class Track: lane_xs = md.laneLines[1].x left_ys = md.laneLines[1].y right_ys = md.laneLines[2].y + def d_path_interp(dRel, yRel): left_lane_y = np.interp(dRel, lane_xs, left_ys) right_lane_y = np.interp(dRel, lane_xs, right_ys) center_y = (left_lane_y + right_lane_y) / 2.0 - lane_half_width = abs(right_lane_y - left_lane_y) / 2.0 + lane_half_width = max(0.1, abs(right_lane_y - left_lane_y) / 2.0) dist_from_center = yRel + center_y in_lane_prob = max(0.0, 1.0 - (abs(dist_from_center) / lane_half_width)) return dist_from_center, in_lane_prob + self.dPath, self.in_lane_prob = d_path_interp(self.dRel, self.yRel) self.dPath_future, self.in_lane_prob_future = d_path_interp(self.dRel_future, self.yRel_future) - def get_RadarState(self, model_prob: float = 0.0, vision_y_rel = 0.0): + # ---- noise suppression only when cnt>=2 ---- + def vlead_for_matching(self, dv_max: float = 4.0, alpha: float = 0.35) -> float: + """ + Returns vLead to be used in matching score. + - If cnt < 2: raw vLead (no filtering) + - If cnt >= 2: clamp spike + IIR smooth + """ + v = float(self.vLead) + + if self.cnt < 2: + return v + + if not self._vLead_filt_init: + self._vLead_last = v + self._vLead_filt = v + self._vLead_filt_init = True + return v + + v_last = self._vLead_last + self._vLead_last = v + + v_clamped = clamp(v, v_last - dv_max, v_last + dv_max) + self._vLead_filt = alpha * v_clamped + (1.0 - alpha) * self._vLead_filt + return float(self._vLead_filt) + + def get_RadarState(self, model_prob: float = 0.0, vision_y_rel=0.0): return { "dRel": float(self.dRel), "yRel": float(self.yRel) if self.yRel != 0.0 else vision_y_rel, - "dPath" : float(self.dPath), + "dPath": float(self.dPath), "vRel": float(self.vRel), "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), @@ -98,126 +140,171 @@ class Track: "aLeadK": float(self.aLeadK), "aLeadTau": float(self.aLeadTau.x), "jLead": float(self.jLead), - "vLat": float(self.yvLead), + "vLat": float(self.yvLead), "status": True, "fcw": self.is_potential_fcw(model_prob), "modelProb": model_prob, "radar": True, "radarTrackId": self.identifier, - "score": self.score # for debug purposes only + "score": self.score, } def potential_low_speed_lead(self, v_ego: float): - # stop for stuff in front of you and low speed, even without model confirmation - # Radar points closer than 0.75, are almost always glitches on toyota radars return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25) def is_potential_fcw(self, model_prob: float): return model_prob > .9 def __str__(self): - ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" - return ret + return f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" -def laplacian_pdf(x: float, mu: float, b: float): - diff = abs(x - mu) / max(b, 1e-4) - return 0.0 if diff > 50.0 else math.exp(-diff) def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): - offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA - #vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0 - max_vision_dist = max(offset_vision_dist * 1.25, 5.0) - min_vision_dist = max(offset_vision_dist * 0.8, 1.0) + if not tracks: + return None + + offset_vision_dist = float(lead.x[0] - RADAR_TO_CAMERA) + + # distance gates + max_vision_dist = max(offset_vision_dist * 1.25, 5.0) + min_vision_dist = max(offset_vision_dist * 0.80, 1.0) max_vision_dist2 = max(offset_vision_dist * 1.45, 5.0) - min_vision_dist2 = 1.5 #max(offset_vision_dist * 0.3, 1.0) - max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # �솗瑜좎씠 �궙�쑝硫� �냽�룄�삤李⑤�� 以꾩엫. + min_vision_dist2 = 1.5 - def prob(c): - prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_y2 = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0] * 2) # for cut-in - prob_v = laplacian_pdf(c.vLead, lead.v[0], lead.vStd[0]) + # velocity tolerance (same intent) + vel_tol = float(max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0)) + # hard guardrail for moving-bias (prevents absurd match) + vel_guard = max(vel_tol * 3.0, 20.0) - #weight_v = np.interp(c.vLead, [0, 10], [0.3, 1]) - score = prob_d * prob_y * prob_v # * weight_v - score2 = prob_d * prob_y2 * prob_v # * weight_v + def dist_sane(t: Track, wide: bool = False) -> bool: + if wide: + return (min_vision_dist2 < t.dRel < max_vision_dist2) + return (min_vision_dist < t.dRel < max_vision_dist) - return score, score2 #prob_d * prob_y * prob_v * weight_v - - def vel_sane(c): - return (abs(c.vLead - lead.v[0]) < max_offset_vision_vel) or (c.vLead > 3) - def dist_sane(c, second=False): - if second: - return min_vision_dist2 < c.dRel < max_vision_dist2 - return min_vision_dist < c.dRel < max_vision_dist - def y_sane(c, second=False): - if second: - return abs(c.yRel + lead.y[0]) < 4.0 - return abs(c.yRel + lead.y[0]) < 2.0 - + def y_sane(t: Track, wide: bool = False) -> bool: + lim = 4.0 if wide else 2.0 + return abs(t.yRel + float(lead.y[0])) < lim + def vel_sane(t: Track) -> bool: + """ + Keep your philosophy: + - if it's moving, likely "the car we should read" + but add guardrail and (optionally) in-lane preference. + """ + v_vis = float(lead.v[0]) + v_trk = float(t.vLead) + dv = abs(v_trk - v_vis) + + # normal strict check + if dv < vel_tol: + return True + + # moving-bias: allow more mismatch for moving objects, + # but only within a reasonable guardrail. + moving = (v_trk > 3.0) + if not moving: + return False + + if dv > vel_guard: + return False + + # If in-lane probability exists (it does in your Track), use it as safety. + # When it's clearly not in our lane, don't use moving-bias. + # (This line is intentionally mild; you can tune 0.2~0.5) + if hasattr(t, "dPath") and (t.in_lane_prob < 0.25): + return False + + return True + + def score_pair(t: Track): + """ + score1: normal yStd + score2: wide yStd for cut-in + NOTE: uses t.vlead_for_matching() only for scoring (cnt>=2 only). + """ + pd = laplacian_pdf(float(t.dRel), offset_vision_dist, float(lead.xStd[0])) + py = laplacian_pdf(float(t.yRel), -float(lead.y[0]), float(lead.yStd[0])) + py2 = laplacian_pdf(float(t.yRel), -float(lead.y[0]), float(lead.yStd[0]) * 2.0) + + v_use = float(t.vlead_for_matching()) # noise suppression only if cnt>=2 + pv = laplacian_pdf(v_use, float(lead.v[0]), float(lead.vStd[0])) + + s1 = pd * py * pv + s2 = pd * py2 * pv + return s1, s2 + + # ---- pick best candidates (FIX: true 1st/2nd) ---- first_track, second_track, extra_track = None, None, None - first_score, second_score, extra_score = -1e6, -1e6, -1e6 - for c in tracks.values(): - c.score, score2 = prob(c) - if c.score > first_score: - second_score = first_score - second_track = first_track - first_score = c.score - first_track = c - if score2 > extra_score: - extra_score = score2 - extra_track = c - + first_score, second_score, extra_score = -1e18, -1e18, -1e18 - #best_track = max(tracks.values(), key=prob) + for t in tracks.values(): + s1, s2 = score_pair(t) + t.score = s1 - def select_track(track, score, track2, score2, extra_track, extra_score): - if score < 0.0001: - return None - - best_track = None - if dist_sane(track) and vel_sane(track): - if y_sane(track): - if lead.prob > 0.5: - best_track = track - elif lead.prob > 0.4 and track.selected_count > 0: # 鍮꾩졏�씠 �씗誘명븯吏�留� 吏곸쟾�뿉 �꽑�깮�맂 �듃�옓�씤寃쎌슦 - best_track = track - elif lead.prob > 0.6: - best_track = track - elif dist_sane(track) and y_sane(track, True): # stopped-car - if score2 > 0.00001 and dist_sane(track2) and y_sane(track2) and vel_sane(track2): - best_track = track2 - elif track.selected_count > 0: - best_track = track - else: - track.is_stopped_car_count += 2 - if track.is_stopped_car_count > int(1.0/DT_MDL): - best_track = track - #elif dist_sane(track) and vel_sane(track) and lead.prob > 0.5: - # best_track = track - elif offset_vision_dist < 90 and lead.prob > 0.65: - # wide y detect, for cut-in - if extra_score > score and dist_sane(extra_track, True) and vel_sane(extra_track) and y_sane(extra_track, True): - best_track = extra_track - # wide dRel, y detect, for cut-in - elif dist_sane(track, True) and vel_sane(track) and y_sane(track, True): - best_track = track - elif score2 > 0.0001 and dist_sane(track2, True) and vel_sane(track2) and y_sane(track2, True): - best_track = track2 - return best_track - - best_track = select_track(first_track, first_score, second_track, second_score, extra_track, extra_score) + if s1 > first_score: + second_track, second_score = first_track, first_score + first_track, first_score = t, s1 + elif s1 > second_score: + second_track, second_score = t, s1 - for c in tracks.values(): - if c is best_track: - best_track.selected_count += 1 + if s2 > extra_score: + extra_track, extra_score = t, s2 + + # score floor + if first_track is None or first_score < 1e-4: + return None + + # ---- selection policy (same logic, cleaner & safer) ---- + best_track = None + + # A) normal match + if dist_sane(first_track) and vel_sane(first_track): + if y_sane(first_track): + if lead.prob > 0.5: + best_track = first_track + elif lead.prob > 0.4 and first_track.selected_count > 0: + best_track = first_track + elif lead.prob > 0.6: + best_track = first_track + + # B) stopped-car-like (only if not chosen yet) + if best_track is None and dist_sane(first_track) and y_sane(first_track, wide=True): + if (second_track is not None and second_score > 1e-5 and + dist_sane(second_track) and y_sane(second_track) and vel_sane(second_track)): + best_track = second_track + elif first_track.selected_count > 0: + best_track = first_track else: - c.selected_count = 0 - c.is_stopped_car_count = max(0, c.is_stopped_car_count - 1) - + first_track.is_stopped_car_count += 2 + if first_track.is_stopped_car_count > int(1.0 / DT_MDL): + best_track = first_track + + # C) cut-in wide matching (only if not chosen yet) + if best_track is None and offset_vision_dist < 90.0 and lead.prob > 0.65: + # wide-y winner first (cut-in) + if (extra_track is not None and extra_score > first_score and + dist_sane(extra_track, wide=True) and vel_sane(extra_track) and y_sane(extra_track, wide=True)): + best_track = extra_track + + # then allow first/second with wide gates + elif dist_sane(first_track, wide=True) and vel_sane(first_track) and y_sane(first_track, wide=True): + best_track = first_track + + elif (second_track is not None and second_score > 1e-4 and + dist_sane(second_track, wide=True) and vel_sane(second_track) and y_sane(second_track, wide=True)): + best_track = second_track + + # ---- update counters ---- + for t in tracks.values(): + if t is best_track and best_track is not None: + t.selected_count += 1 + else: + t.selected_count = 0 + t.is_stopped_car_count = max(0, t.is_stopped_car_count - 1) + return best_track + def get_RadarState_from_vision(md, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): lead_v_rel_pred = lead_msg.v[0] - model_v_ego dRel = float(lead_msg.x[0] - RADAR_TO_CAMERA) @@ -385,6 +472,12 @@ class RadarD: self.radar_detected = False + self._corner_lat_hist = { + "L": deque(maxlen=10), + "R": deque(maxlen=10), + } + self._corner_state = {"L": 0, "R": 0} # -1,0,+1 + def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] @@ -449,7 +542,7 @@ class RadarD: self.compute_leads(self.v_ego, alive_tracks, md) if self.leadTwo is not None: self.radar_state.leadTwo = self.leadTwo - if self.enable_radar_tracks == 3: + if self.enable_radar_tracks >= 3: self._pick_lead_one_from_state() def publish(self, pm: messaging.PubMaster): @@ -480,7 +573,7 @@ class RadarD: if (track is None or lead_msg.prob < .6) and track_scc is not None and track_scc.cnt > 2: #if self.enable_radar_tracks in [-1, 2] or model_v_ego < 5 or track_scc.vLead < 5.0: - if self.enable_radar_tracks in [-1, 2] or track_scc.vLead < 5.0: + if self.enable_radar_tracks == -1 or (self.enable_radar_tracks >= 2 and track_scc.vLead < 5.0): track = track_scc lead_dict = {'status': False} @@ -491,7 +584,7 @@ class RadarD: elif (track is None) and ready and (lead_msg.prob > .5): lead_dict = self.vision_tracks[index].get_lead(md) - if self.enable_corner_radar > 0: + if self.enable_corner_radar > 1: lead_dict = self.corner_radar(CS, lead_dict) if low_speed_override: @@ -522,7 +615,7 @@ class RadarD: for c in tracks.values(): y_rel_neg = - c.yRel # center - if c.in_lane_prob > 0.1: + if c.in_lane_prob > 0.3: if c.cnt > 3: ld = c.get_RadarState(lead_msg.prob, float(-lead_msg.y[0])) ld['modelProb'] = 0.01 @@ -641,19 +734,74 @@ class RadarD: self.radar_state.leadOne = chosen self.radar_detected = detected - - def corner_radar(self, CS, lead_dict): - lat_dist = 1e6 - long_dist = 1e6 - if 0 < CS.leftLatDist < 2.5: - lat_dist = CS.leftLatDist - long_dist = CS.leftLongDist - if 0 < CS.rightLatDist < 2.5 and CS.rightLongDist < long_dist: - lat_dist = -CS.rightLatDist - long_dist = CS.rightLongDist + def _corner_update_state(self, side: str, cur_lat: float, enter_lat: float = 2.8) -> int: + # �쑀�슚 踰붿쐞 諛뽰씠硫� 由ъ뀑 + if not (0.0 < cur_lat < enter_lat): + self._corner_lat_hist[side].clear() + self._corner_state[side] = 0 + return 0 - if lat_dist == 0.0 or abs(lat_dist) >= 2.5 or long_dist == 1e6: + h = self._corner_lat_hist[side] + h.append(cur_lat) + + n = len(h) + if n < 3: + # �뜲�씠�꽣 �꼫臾� �쟻�쑝硫� �씠�쟾 �긽�깭 �쑀吏� + return self._corner_state[side] + + delta = h[-1] - h[0] + th = 0.02 # 3 * (20 / n) + + if delta < -th: + self._corner_state[side] = +1 # approaching + elif delta > th: + self._corner_state[side] = -1 # leaving + else: + self._corner_state[side] = 0 # maintain + + return self._corner_state[side] + + def corner_radar(self, CS, lead_dict): + ENTER_LAT = 2.2 + KEEP_LAT = 2.0 + EXIT_LAT = 1.2 + + left_lat, right_lat = abs(CS.leftLatDist), abs(CS.rightLatDist) + left_state = self._corner_update_state("L", left_lat) + right_state = self._corner_update_state("R", right_lat) + + # 1) left usable? + left_ok = False + if left_state > 0: + left_ok = left_lat < ENTER_LAT + elif left_state == 0: + left_ok = 0 < left_lat < KEEP_LAT + else: # leaving + left_ok = left_lat <= EXIT_LAT + + # 2) right usable? + right_ok = False + if right_state > 0: + right_ok = right_lat < ENTER_LAT + elif right_state == 0: + right_ok = 0 < right_lat < KEEP_LAT + else: + right_ok = right_lat <= EXIT_LAT + + # 3) �븘臾대룄 紐� �벐硫� skip + if not left_ok and not right_ok: return lead_dict + + # 4) �몮 �떎 �릺硫� longDist濡� �꽑�깮 + if left_ok and right_ok: + if CS.leftLongDist <= CS.rightLongDist: + lat_dist, long_dist = +left_lat, CS.leftLongDist + else: + lat_dist, long_dist = -right_lat, CS.rightLongDist + elif left_ok: + lat_dist, long_dist = +left_lat, CS.leftLongDist + else: + lat_dist, long_dist = -right_lat, CS.rightLongDist if lead_dict['status']: if lead_dict['dRel'] > long_dist: diff --git a/selfdrive/frogpilot/fleetmanager/fleet_manager.py b/selfdrive/frogpilot/fleetmanager/fleet_manager.py index 9cc2af5f8..ffe4b5559 100644 --- a/selfdrive/frogpilot/fleetmanager/fleet_manager.py +++ b/selfdrive/frogpilot/fleetmanager/fleet_manager.py @@ -223,8 +223,8 @@ def upload_carrot(route, segment): local_folder = os.path.join(Paths.log_root(), f"{route}--{segment}") if not os.path.isdir(local_folder): abort(404, "Folder not found") - car_selected = Params().get("CarName", "none").decode('utf-8') - dongle_id = Params().get("DongleId", "unknown").decode('utf-8') + car_selected = Params().get("CarName", "none") + dongle_id = Params().get("DongleId", "unknown") directory = f"{car_selected} {dongle_id}" success = upload_folder_to_ftp(local_folder, directory, f"{route}--{segment}") if success: @@ -574,7 +574,7 @@ def carinfo(): # �렩�룚饔�渦녶읃�쑍岳→겘 try: - car_name = params.get("CarName", encoding='utf8') + car_name = params.get("CarName") if car_name in PLATFORMS: platform = PLATFORMS[car_name] car_fingerprint = platform.config.platform_str diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index d54a18d49..9d8838cee 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -29,7 +29,7 @@ import subprocess # otisserv conversion from pathlib import Path -from openpilot.common.params import Params +from openpilot.common.params import Params, ParamKeyType from openpilot.system.hardware import PC from openpilot.system.hardware.hw import Paths from openpilot.system.loggerd.uploader import listdir_by_creation @@ -223,26 +223,26 @@ def ffplay_mp4_wrap_process_builder(file_name): ) def get_nav_active(): - if params.get("NavDestination", encoding='utf8') is not None: + if params.get("NavDestination") is not None: return True else: return False def get_public_token(): - token = params.get("MapboxPublicKey", encoding='utf8') + token = params.get("MapboxPublicKey") return token.strip() if token is not None else None def get_app_token(): - token = params.get("MapboxSecretKey", encoding='utf8') + token = params.get("MapboxSecretKey") return token.strip() if token is not None else None def get_gmap_key(): - token = params.get("GMapKey", encoding='utf8') + token = params.get("GMapKey") return token.strip() if token is not None else None def get_amap_key(): - token = params.get("AMapKey1", encoding='utf8') - token2 = params.get("AMapKey2", encoding='utf8') + token = params.get("AMapKey1") + token2 = params.get("AMapKey2") return (token.strip() if token is not None else None, token2.strip() if token2 is not None else None) def get_SearchInput(): @@ -262,11 +262,11 @@ def get_last_lon_lat(): return l["longitude"], l["latitude"] def get_locations(): - data = params.get("ApiCache_NavDestinations", encoding='utf-8') + data = params.get("ApiCache_NavDestinations") return data def preload_favs(): - raw_json = params.get("ApiCache_NavDestinations", encoding='utf8') + raw_json = params.get("ApiCache_NavDestinations") if raw_json is None: return (None, None, None, None, None) @@ -289,7 +289,7 @@ def parse_addr(postvars, lon, lat, valid_addr, token): real_addr = None if addr != "favorites": try: - dests = json.loads(params.get("ApiCache_NavDestinations", encoding='utf8')) + dests = json.loads(params.get("ApiCache_NavDestinations")) except TypeError: dests = json.loads("[]") for item in dests: @@ -355,7 +355,7 @@ def nav_confirmed(postvars): else: new_dest["save_type"] = "favorite" new_dest["label"] = save_type - val = params.get("ApiCache_NavDestinations", encoding='utf8') + val = params.get("ApiCache_NavDestinations") if val is not None: val = val.rstrip('\x00') dests = [] if val is None else json.loads(val) @@ -447,28 +447,116 @@ def transform_lng(lng, lat): ret += (150.0 * math.sin(lng / 12.0 * pi) + 300.0 * math.sin(lng / 30.0 * pi)) * 2.0 / 3.0 return ret -from openpilot.system.manager.manager import get_default_params_key -def get_all_toggle_values(): - all_keys = get_default_params_key() +def get_keys_with_default(exclude_types=(ParamKeyType.BYTES, ParamKeyType.JSON)): + keys = [] + for k in params.all_keys(): # raw keys + if isinstance(k, (bytes, bytearray, memoryview)): + try: + k_str = k.decode("utf-8") + except Exception: + continue + else: + k_str = str(k) - toggle_values = {} - for key in all_keys: try: - value = params.get(key) + t = params.get_type(k_str) # = getKeyType except Exception: - value = b"0" - toggle_values[key] = value.decode('utf-8') if value is not None else "0" + continue + if t in exclude_types: + continue + + # default 議댁옱 �뿬遺� + try: + dv = params.get_default_value(k_str) # optional �뾾�쑝硫� None + except Exception: + continue + + if dv is None: + continue + + keys.append(k_str) + + return keys + +def get_all_toggle_values(): + toggle_values = {} + + for k in params.all_keys(): + # key �젙由� + if isinstance(k, (bytes, bytearray, memoryview)): + try: + key = k.decode("utf-8") + except Exception: + continue + else: + key = str(k) + + # ����엯 �솗�씤 + �젣�쇅 + try: + t = params.get_type(key) + except Exception: + continue + if t in (ParamKeyType.BYTES, ParamKeyType.JSON): + continue + + # default �뾾�뒗 �궎 �젣�쇅 + try: + dv = params.get_default_value(key) + except Exception: + continue + if dv is None: + continue + + # 媛� �씫湲� (�씠誘� Params.get()�씠 ����엯 蹂��솚源뚯�� �빐以�) + try: + v = params.get(key, block=False, return_default=False) + except Exception: + v = None + + # v媛� None�씠硫� default濡� 梨꾩슦怨� �떢�쑝硫� dv濡� ���泥� (�꽑�깮) + if v is None: + v = dv + + # 理쒖쥌 stringify (jsonify �슜) + if isinstance(v, (dict, list)): + toggle_values[key] = json.dumps(v, ensure_ascii=False) + else: + toggle_values[key] = str(v) return toggle_values def store_toggle_values(updated_values): for key, value in updated_values.items(): try: - params.put(key, value.encode('utf-8')) - #params_storage.put(key, value.encode('utf-8')) + # key ����엯 �솗�씤 + t = params.get_type(key) + + # 臾몄옄�뿴 �넂 �떎�젣 ����엯�쑝濡� 蹂��솚 + if t == ParamKeyType.BOOL: + v = value in ("1", "true", "True", "on", "yes") + params.put_bool(key, v) + + elif t == ParamKeyType.INT: + params.put_int(key, int(value)) + + elif t == ParamKeyType.FLOAT: + params.put_float(key, float(value)) + + elif t == ParamKeyType.TIME: + # ISO format 臾몄옄�뿴�씠�씪怨� 媛��젙 + params.put(key, value) # Params �궡遺��뿉�꽌 isoformat string 泥섎━ + + elif t == ParamKeyType.STRING: + params.put(key, value) + + elif t == ParamKeyType.JSON: + # �씠誘� JSON 臾몄옄�뿴濡� �꽆�뼱�솕�떎怨� 媛��젙 + obj = json.loads(value) if isinstance(value, str) else value + params.put(key, obj) + + else: + print(f"Skip unsupported param type: {key} type={t}") + continue + except Exception as e: print(f"Failed to update {key}: {e}") - - #params_memory.put_bool("FrogPilotTogglesUpdated", True) - #time.sleep(1) - #params_memory.put_bool("FrogPilotTogglesUpdated", False) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 91866328f..5f88796ee 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -316,9 +316,9 @@ def main(demo=False): vEgoStopping = params.get_float("VEgoStopping") * 0.01 if custom_lat_delay > 0.0: - lat_delay = custom_lat_delay + lat_smooth_seconds + lat_delay = custom_lat_delay + lat_smooth_seconds + 0.1 else: - lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds + lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds + 0.1 # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 5c5198234..e42bb8ea9 100644 Binary files a/selfdrive/modeld/models/driving_policy.onnx and b/selfdrive/modeld/models/driving_policy.onnx differ diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index f70ad202b..99a7d138c 100644 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -60,11 +60,11 @@ class RouteEngine: self.mapbox_token = os.environ["MAPBOX_TOKEN"] self.mapbox_host = "https://api.mapbox.com" elif self.params.get_int("PrimeType") == 0: - self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8') + self.mapbox_token = self.params.get("MapboxPublicKey") self.mapbox_host = "https://api.mapbox.com" else: try: - self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) + self.mapbox_token = Api(self.params.get("DongleId")).get_token(expiry_hours=4 * 7 * 24) except FileNotFoundError: cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") self.mapbox_token = "" @@ -137,7 +137,7 @@ class RouteEngine: print(f"############## Calculating route {self.last_position} -> {destination}") self.nav_destination = destination - lang = self.params.get('LanguageSetting', encoding='utf8') + lang = self.params.get('LanguageSetting') if lang is not None: lang = lang.replace('main_', '') @@ -153,7 +153,7 @@ class RouteEngine: } # TODO: move waypoints into NavDestination param? - waypoints = self.params.get('NavDestinationWaypoints', encoding='utf8') + waypoints = self.params.get('NavDestinationWaypoints') waypoint_coords = [] if waypoints is not None and len(waypoints) > 0: waypoint_coords = json.loads(waypoints) diff --git a/selfdrive/selfdrived/alertmanager.py b/selfdrive/selfdrived/alertmanager.py index ac1006ff2..385c276a9 100644 --- a/selfdrive/selfdrived/alertmanager.py +++ b/selfdrive/selfdrived/alertmanager.py @@ -13,11 +13,11 @@ with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as OFFROAD_ALERTS = json.load(f) -def set_offroad_alert(alert: str, show_alert: bool, extra_text: str = None) -> None: +def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None: if show_alert: a = copy.copy(OFFROAD_ALERTS[alert]) a['extra'] = extra_text or '' - Params().put(alert, json.dumps(a)) + Params().put(alert, a) else: Params().remove(alert) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 06a86ede7..60b95c753 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -253,7 +253,7 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - model_name = Params().get("NNFFModelName", encoding='utf-8') + model_name = Params().get("NNFFModelName") if model_name == "": return Alert( "NNFF Torque Controller not available", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index aab8f64ec..db50f7b94 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -404,7 +404,7 @@ class SelfdriveD: if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) - if self.sm.frame == 550 and Params().get("NNFFModelName", encoding='utf-8') is not None: + if self.sm.frame == 550 and Params().get("NNFFModelName") is not None: self.events.add(EventName.torqueNNLoad) # decrement personality on distance button press #if self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 0679d079f..d72c44539 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -108,7 +108,7 @@ class Soundd: self.soundVolumeAdjust = 1.0 self.carrot_count_down = 0 - self.lang = self.params.get('LanguageSetting', encoding='utf8') + self.lang = self.params.get('LanguageSetting') self.load_sounds() self.current_alert = AudibleAlert.none diff --git a/system/athena/athenad.py b/system/athena/athenad.py index 5813bc90a..8def11540 100755 --- a/system/athena/athenad.py +++ b/system/athena/athenad.py @@ -134,7 +134,7 @@ class UploadQueueCache: try: upload_queue_json = Params().get("AthenadUploadQueue") if upload_queue_json is not None: - for item in json.loads(upload_queue_json): + for item in upload_queue_json: upload_queue.put(UploadItem.from_dict(item)) except Exception: cloudlog.exception("athena.UploadQueueCache.initialize.exception") @@ -144,7 +144,7 @@ class UploadQueueCache: try: queue: list[UploadItem | None] = list(upload_queue.queue) items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] - Params().put("AthenadUploadQueue", json.dumps(items)) + Params().put("AthenadUploadQueue", items) except Exception: cloudlog.exception("athena.UploadQueueCache.cache.exception") @@ -453,7 +453,7 @@ def setRouteViewed(route: str) -> dict[str, int | str]: # maintain a list of the last 10 routes viewed in connect params = Params() - r = params.get("AthenadRecentlyViewedRoutes", encoding="utf8") + r = params.get("AthenadRecentlyViewedRoutes") routes = [] if r is None else r.split(",") routes.append(route) @@ -475,7 +475,7 @@ def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local cloudlog.debug("athena.startLocalProxy.starting") - dongle_id = Params().get("DongleId").decode('utf8') + dongle_id = Params().get("DongleId") identity_token = Api(dongle_id).get_token() ws = create_connection(remote_ws_uri, cookie="jwt=" + identity_token, @@ -516,12 +516,12 @@ def getPublicKey() -> str | None: @dispatcher.add_method def getSshAuthorizedKeys() -> str: - return Params().get("GithubSshKeys", encoding='utf8') or '' + return Params().get("GithubSshKeys") or '' @dispatcher.add_method def getGithubUsername() -> str: - return Params().get("GithubUsername", encoding='utf8') or '' + return Params().get("GithubUsername") or '' @dispatcher.add_method def getSimInfo(): @@ -732,7 +732,7 @@ def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: recv_queue.put_nowait(data) elif opcode == ABNF.OPCODE_PING: last_ping = int(time.monotonic() * 1e9) - Params().put("LastAthenaPingTime", str(last_ping)) + Params().put("LastAthenaPingTime", last_ping) except WebSocketTimeoutException: ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9: @@ -796,7 +796,7 @@ def main(exit_event: threading.Event = None): cloudlog.exception("failed to set core affinity") params = Params() - dongle_id = params.get("DongleId", encoding='utf-8') + dongle_id = params.get("DongleId") UploadQueueCache.initialize(upload_queue) ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id diff --git a/system/athena/manage_athenad.py b/system/athena/manage_athenad.py index f5ab81720..ee63606b6 100755 --- a/system/athena/manage_athenad.py +++ b/system/athena/manage_athenad.py @@ -14,7 +14,7 @@ ATHENA_MGR_PID_PARAM = "AthenadPid" def main(): params = Params() - dongle_id = params.get("DongleId").decode('utf-8') + dongle_id = params.get("DongleId") build_metadata = get_build_metadata() cloudlog.bind_global(dongle_id=dongle_id, diff --git a/system/athena/registration.py b/system/athena/registration.py index e42139dea..335fbb590 100755 --- a/system/athena/registration.py +++ b/system/athena/registration.py @@ -21,7 +21,7 @@ DUMMY_IMEI2 = '865420071781904' def is_registered_device() -> bool: - dongle = Params().get("DongleId", encoding='utf-8') + dongle = Params().get("DongleId") return dongle not in (None, UNREGISTERED_DONGLE_ID) @@ -39,7 +39,7 @@ def register(show_spinner=False) -> str | None: #return UNREGISTERED_DONGLE_ID # for c3lite, clone - dongle_id: str | None = params.get("DongleId", encoding='utf8') + dongle_id: str | None = params.get("DongleId") if dongle_id is None and Path(Paths.persist_root()+"/comma/dongle_id").is_file(): # not all devices will have this; added early in comma 3X production (2/28/24) with open(Paths.persist_root()+"/comma/dongle_id") as f: diff --git a/system/athena/tests/test_athenad.py b/system/athena/tests/test_athenad.py index e16e73a7e..c8fd94fca 100644 --- a/system/athena/tests/test_athenad.py +++ b/system/athena/tests/test_athenad.py @@ -380,11 +380,11 @@ class TestAthenadMethods: def test_get_ssh_authorized_keys(self): keys = dispatcher["getSshAuthorizedKeys"]() - assert keys == self.default_params["GithubSshKeys"].decode('utf-8') + assert keys == self.default_params["GithubSshKeys"] def test_get_github_username(self): keys = dispatcher["getGithubUsername"]() - assert keys == self.default_params["GithubUsername"].decode('utf-8') + assert keys == self.default_params["GithubUsername"] def test_get_version(self): resp = dispatcher["getVersion"]() diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index ed5a2c5f6..b2d3e69fa 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -447,7 +447,7 @@ def hardware_thread(end_event, hw_queue) -> None: # save last one before going onroad if rising_edge_started: try: - params.put("LastOffroadStatusPacket", json.dumps(dat)) + params.put("LastOffroadStatusPacket", dat) except Exception: cloudlog.exception("failed to save offroad status") diff --git a/system/hardware/power_monitoring.py b/system/hardware/power_monitoring.py index dbfa5290a..cf53c0774 100644 --- a/system/hardware/power_monitoring.py +++ b/system/hardware/power_monitoring.py @@ -58,7 +58,7 @@ class PowerMonitoring: self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh) if now - self.last_save_time >= 10: - self.params.put_nonblocking("CarBatteryCapacity", str(int(self.car_battery_capacity_uWh))) + self.params.put_nonblocking("CarBatteryCapacity", int(self.car_battery_capacity_uWh)) self.last_save_time = now # First measurement, set integration time diff --git a/system/loggerd/loggerd.cc b/system/loggerd/loggerd.cc index 144ab9f34..b48ea3a13 100644 --- a/system/loggerd/loggerd.cc +++ b/system/loggerd/loggerd.cc @@ -275,6 +275,7 @@ void loggerd_thread() { uint64_t msg_count = 0, bytes_count = 0; double start_ts = millis_since_boot(); + printf("loggerd while() started\n"); while (!do_exit) { // poll for new messages on all sockets for (auto sock : poller->poll(1000)) { @@ -328,7 +329,7 @@ void loggerd_thread() { } } } - + printf("loggerd while() exiting\n"); LOGW("closing logger"); s.logger.setExitSignal(do_exit.signal); @@ -343,6 +344,7 @@ void loggerd_thread() { } int main(int argc, char** argv) { + printf("loggerd started\n"); if (!Hardware::PC()) { int ret; ret = util::set_core_affinity({0, 1, 2, 3}); @@ -351,8 +353,8 @@ int main(int argc, char** argv) { //ret = util::set_realtime_priority(1); //assert(ret == 0); } - + printf("loggerd runningq\n"); loggerd_thread(); - + printf("loggerd exiting\n"); return 0; } diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index b19cae564..bdb2bbd27 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -90,7 +90,7 @@ class Uploader: # self.immediate_priority.update({"rlog": 0, "rlog.zst": 0}) def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]: - r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8") + r = self.params.get("AthenadRecentlyViewedRoutes") requested_routes = [] if r is None else [route for route in r.split(",") if route] for logdir in listdir_by_creation(self.root): @@ -240,7 +240,7 @@ def main(exit_event: threading.Event = None) -> None: clear_locks(Paths.log_root()) params = Params() - dongle_id = params.get("DongleId", encoding='utf8') + dongle_id = params.get("DongleId") if dongle_id is None: cloudlog.info("uploader missing dongle_id") diff --git a/system/manager/manager.py b/system/manager/manager.py index db641c295..b4c6f0f22 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -8,7 +8,7 @@ import traceback from cereal import log import cereal.messaging as messaging import openpilot.system.sentry as sentry -from openpilot.common.params import Params, ParamKeyType +from openpilot.common.params import Params, ParamKeyFlag from openpilot.common.text_window import TextWindow from openpilot.system.hardware import HARDWARE from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog @@ -19,192 +19,19 @@ from openpilot.common.swaglog import cloudlog, add_file_handler from openpilot.system.version import get_build_metadata, terms_version, training_version from openpilot.system.hardware.hw import Paths -def get_default_params(): - default_params : list[tuple[str, str | bytes]] = [ - # kans - ("LongPitch", "1"), - ("EVTable", "1"), - ("CompletedTrainingVersion", "0"), - ("DisengageOnAccelerator", "0"), - ("GsmMetered", "1"), - ("HasAcceptedTerms", "0"), - ("LanguageSetting", "main_en"), - ("OpenpilotEnabledToggle", "1"), - ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), - ("IsMetric", "1"), - ("RecordAudio", "1"), - - ("SearchInput", "0"), - ("GMapKey", "0"), - ("MapboxStyle", "0"), - - - ("LongitudinalPersonalityMax", "3"), - ("ShowDebugUI", "0"), - ("ShowTpms", "1"), - ("ShowDateTime", "1"), - ("ShowPathEnd", "1"), - ("ShowCustomBrightness", "100"), - ("ShowLaneInfo", "1"), - ("ShowRadarInfo", "1"), - ("ShowDeviceState", "1"), - ("ShowRouteInfo", "1"), - ("ShowPathMode", "9"), - ("ShowPathColor", "13"), - ("ShowPathColorCruiseOff", "19"), - ("ShowPathModeLane", "14"), - ("ShowPathColorLane", "13"), - ("ShowPlotMode", "0"), - ("AutoCruiseControl", "0"), - ("CruiseEcoControl", "2"), - ("CarrotCruiseDecel", "-1"), - ("CarrotCruiseAtcDecel", "-1"), - ("CommaLongAcc", "0"), - ("AutoGasTokSpeed", "0"), - ("AutoGasSyncSpeed", "1"), - ("AutoEngage", "0"), - ("DisableMinSteerSpeed", "0"), - ("SoftHoldMode", "0"), - - ("AutoSpeedUptoRoadSpeedLimit", "0"), - ("AutoRoadSpeedAdjust", "50"), - ("AutoCurveSpeedLowerLimit", "30"), - ("AutoCurveSpeedFactor", "120"), - ("AutoCurveSpeedAggressiveness", "100"), - - ("AutoTurnControl", "0"), - ("AutoTurnControlSpeedTurn", "20"), - ("AutoTurnControlTurnEnd", "6"), - ("AutoTurnMapChange", "0"), - - ("AutoNaviSpeedCtrlEnd", "7"), - ("AutoNaviSpeedCtrlMode", "2"), - ("AutoNaviSpeedBumpTime", "1"), - ("AutoNaviSpeedBumpSpeed", "35"), - ("AutoNaviSpeedSafetyFactor", "105"), - ("AutoNaviSpeedDecelRate", "120"), - ("AutoRoadSpeedLimitOffset", "-1"), - ("AutoNaviCountDownMode", "2"), - ("TurnSpeedControlMode", "1"), - ("CarrotSmartSpeedControl", "0"), - ("MapTurnSpeedFactor", "90"), - ("ModelTurnSpeedFactor", "0"), - ("StoppingAccel", "0"), - ("StopDistanceCarrot", "550"), - ("JLeadFactor3", "0"), - ("CruiseButtonMode", "0"), - ("CancelButtonMode", "0"), - ("LfaButtonMode", "0"), - ("CruiseButtonTest1", "8"), - ("CruiseButtonTest2", "30"), - ("CruiseButtonTest3", "1"), - ("CruiseSpeedUnit", "10"), - ("CruiseSpeedUnitBasic", "1"), - ("CruiseSpeed1", "30"), - ("CruiseSpeed2", "50"), - ("CruiseSpeed3", "80"), - ("CruiseSpeed4", "110"), - ("CruiseSpeed5", "130"), - ("PaddleMode", "0"), - ("MyDrivingMode", "3"), - ("MyDrivingModeAuto", "0"), - ("TrafficLightDetectMode", "2"), - ("CruiseMaxVals0", "160"), - ("CruiseMaxVals1", "200"), - ("CruiseMaxVals2", "160"), - ("CruiseMaxVals3", "130"), - ("CruiseMaxVals4", "110"), - ("CruiseMaxVals5", "95"), - ("CruiseMaxVals6", "80"), - ("LongTuningKpV", "100"), - ("LongTuningKiV", "0"), - ("LongTuningKf", "100"), - ("LongActuatorDelay", "20"), - ("VEgoStopping", "50"), - ("RadarReactionFactor", "100"), - ("EnableRadarTracks", "0"), - ("RadarLatFactor", "0"), - ("EnableCornerRadar", "0"), - ("HyundaiCameraSCC", "0"), - ("IsLdwsCar", "0"), - ("CanfdHDA2", "0"), - ("CanfdDebug", "0"), - ("SoundVolumeAdjust", "100"), - ("SoundVolumeAdjustEngage", "10"), - ("TFollowGap1", "110"), - ("TFollowGap2", "120"), - ("TFollowGap3", "140"), - ("TFollowGap4", "160"), - ("DynamicTFollow", "0"), - ("AChangeCostStarting", "10"), - ("TrafficStopDistanceAdjust", "400"), - ("DynamicTFollowLC", "100"), - ("HapticFeedbackWhenSpeedCamera", "0"), - ("UseLaneLineSpeed", "0"), - ("PathOffset", "0"), - ("UseLaneLineCurveSpeed", "0"), - ("AdjustLaneOffset", "0"), - ("LaneChangeNeedTorque", "0"), - ("LaneChangeDelay", "0"), - ("LaneChangeBsd", "0"), - ("MaxAngleFrames", "89"), - ("LateralTorqueCustom", "0"), - ("LateralTorqueAccelFactor", "2500"), - ("LateralTorqueFriction", "100"), - ("LateralTorqueKpV", "100"), - ("LateralTorqueKiV", "10"), - ("LateralTorqueKf", "100"), - ("LateralTorqueKd", "0"), - ("LatMpcPathCost", "200"), - ("LatMpcMotionCost", "7"), - ("LatMpcAccelCost", "120"), - ("LatMpcJerkCost", "4"), - ("LatMpcSteeringRateCost", "7"), - ("LatMpcInputOffset", "4"), - ("CustomSteerMax", "0"), - ("CustomSteerDeltaUp", "0"), - ("CustomSteerDeltaDown", "0"), - ("CustomSteerDeltaUpLC", "0"), - ("CustomSteerDeltaDownLC", "0"), - ("SpeedFromPCM", "2"), - ("SteerActuatorDelay", "0"), - ("LatSmoothSec", "13"), - ("MaxTimeOffroadMin", "60"), - ("DisableDM", "0"), - ("EnableConnect", "0"), - ("MuteDoor", "0"), - ("MuteSeatbelt", "0"), - ("RecordRoadCam", "0"), - ("HDPuse", "0"), - ("CruiseOnDist", "400"), - ("HotspotOnBoot", "0"), - ("SoftwareMenu", "1"), - ("CustomSR", "0"), - ("SteerRatioRate", "100"), - ("NNFF", "0"), - ("NNFFLite", "0"), - ("ShareData", "0"), - ] - return default_params - def set_default_params(): params = Params() - default_params = get_default_params() - try: - default_params.remove(("GMapKey", "0")) - default_params.remove(("CompletedTrainingVersion", "0")) - default_params.remove(("LanguageSetting", "main_en")) - default_params.remove(("GsmMetered", "1")) - except ValueError: - pass - for k, v in default_params: - params.put(k, v) - print(f"SetToDefault[{k}]={v}") + for k in params.all_keys(): + default_value = params.get_default_value(k) + if default_value is not None: + params.put(k, default_value) + print(f"SetToDefault[{k}]={default_value}") def get_default_params_key(): - default_params = get_default_params() - all_keys = [key for key, _ in default_params] - return all_keys + return Params().all_keys() + #default_params = get_default_params() + #all_keys = [key for key, _ in default_params] + #return all_keys def manager_init() -> None: save_bootlog() @@ -212,21 +39,21 @@ def manager_init() -> None: build_metadata = get_build_metadata() params = Params() - params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) - params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) - params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) + params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION) + params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) + params.clear_all(ParamKeyFlag.CLEAR_ON_IGNITION_ON) if build_metadata.release_channel: - params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) - - default_params = get_default_params() + params.clear_all(ParamKeyFlag.DEVELOPMENT_ONLY) if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) - # set unset params - for k, v in default_params: - if params.get(k) is None: - params.put(k, v) + # set unset params to their default value + for k in params.all_keys(): + default_value = params.get_default_value(k) + if default_value is not None and params.get(k) is None: + params.put(k, default_value) # Create folders needed for msgq try: @@ -298,25 +125,27 @@ def manager_thread() -> None: params = Params() ignore: list[str] = [] - if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): + if params.get("DongleId") in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] - if params.get("HardwareC3xLite"): + if params.get_bool("HardwareC3xLite"): ignore += ["micd", "soundd", "loggerd"] - params.put("RecordAudio", "0") + params.put_bool("RecordAudio", False) - sm = messaging.SubMaster(['deviceState', 'carParams'], poll='deviceState') + sm = messaging.SubMaster(['deviceState', 'carParams', 'pandaStates'], poll='deviceState') pm = messaging.PubMaster(['managerState']) write_onroad_params(False, params) + print(f"################# ignore process list: {ignore} #################") ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) print_timer = 0 started_prev = False + ignition_prev = False while True: sm.update(1000) @@ -324,15 +153,20 @@ def manager_thread() -> None: started = sm['deviceState'].started if started and not started_prev: - params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) + params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION) elif not started and started_prev: - params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) + + ignition = any(ps.ignitionLine or ps.ignitionCan for ps in sm['pandaStates'] if ps.pandaType != log.PandaState.PandaType.unknown) + if ignition and not ignition_prev: + params.clear_all(ParamKeyFlag.CLEAR_ON_IGNITION_ON) # update onroad params, which drives pandad's safety setter thread if started != started_prev: write_onroad_params(started, params) started_prev = started + ignition_prev = ignition ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) diff --git a/system/manager/process.py b/system/manager/process.py index d360be857..7185e0b7c 100644 --- a/system/manager/process.py +++ b/system/manager/process.py @@ -197,7 +197,6 @@ class NativeProcess(ManagerProcess): self.watchdog_seen = False self.shutting_down = False - class PythonProcess(ManagerProcess): def __init__(self, name, module, should_run, enabled=True, sigkill=False, watchdog_max_dt=None): self.name = name @@ -256,7 +255,7 @@ class DaemonProcess(ManagerProcess): if self.params is None: self.params = Params() - pid = self.params.get(self.param_name, encoding='utf-8') + pid = self.params.get(self.param_name) if pid is not None: try: os.kill(int(pid), 0) @@ -275,7 +274,7 @@ class DaemonProcess(ManagerProcess): stderr=open('/dev/null', 'w'), preexec_fn=os.setpgrp) - self.params.put(self.param_name, str(proc.pid)) + self.params.put(self.param_name, proc.pid) def stop(self, retry=True, block=True, sig=None) -> None: pass diff --git a/system/sentry.py b/system/sentry.py index 3a53ff7c3..75ec22f1c 100644 --- a/system/sentry.py +++ b/system/sentry.py @@ -50,7 +50,7 @@ def init(project: SentryProject) -> bool: return False env = "release" if build_metadata.tested_channel else "master" - dongle_id = Params().get("DongleId", encoding='utf-8') + dongle_id = Params().get("DongleId") integrations = [] if project == SentryProject.SELFDRIVE: diff --git a/system/statsd.py b/system/statsd.py index b8a7f2c66..d60064fc9 100755 --- a/system/statsd.py +++ b/system/statsd.py @@ -61,7 +61,7 @@ class StatLog: def main() -> NoReturn: - dongle_id = Params().get("DongleId", encoding='utf-8') + dongle_id = Params().get("DongleId") def get_influxdb_line(measurement: str, value: float | dict[str, float], timestamp: datetime, tags: dict) -> str: res = f"{measurement}" for k, v in tags.items(): diff --git a/system/updated/tests/test_base.py b/system/updated/tests/test_base.py index 52287c58f..46b7d766f 100644 --- a/system/updated/tests/test_base.py +++ b/system/updated/tests/test_base.py @@ -132,8 +132,8 @@ class TestBaseUpdate: class ParamsBaseUpdateTest(TestBaseUpdate): def _test_finalized_update(self, branch, version, agnos_version, release_notes): - assert self.params.get("UpdaterNewDescription", encoding="utf-8").startswith(f"{version} / {branch}") - assert self.params.get("UpdaterNewReleaseNotes", encoding="utf-8") == f"{release_notes}\n" + assert self.params.get("UpdaterNewDescription").startswith(f"{version} / {branch}") + assert self.params.get("UpdaterNewReleaseNotes") == f"{release_notes}\n" super()._test_finalized_update(branch, version, agnos_version, release_notes) def send_check_for_updates_signal(self, updated: ManagerProcess): @@ -143,7 +143,7 @@ class ParamsBaseUpdateTest(TestBaseUpdate): updated.signal(signal.SIGHUP.value) def _test_params(self, branch, fetch_available, update_available): - assert self.params.get("UpdaterTargetBranch", encoding="utf-8") == branch + assert self.params.get("UpdaterTargetBranch") == branch assert self.params.get_bool("UpdaterFetchAvailable") == fetch_available assert self.params.get_bool("UpdateAvailable") == update_available @@ -151,8 +151,8 @@ class ParamsBaseUpdateTest(TestBaseUpdate): self.wait_for_condition(lambda: self.params.get("UpdaterState", encoding="utf-8") == "idle") def wait_for_failed(self): - self.wait_for_condition(lambda: self.params.get("UpdateFailedCount", encoding="utf-8") is not None and \ - int(self.params.get("UpdateFailedCount", encoding="utf-8")) > 0) + self.wait_for_condition(lambda: self.params.get("UpdateFailedCount") is not None and \ + int(self.params.get("UpdateFailedCount")) > 0) def wait_for_fetch_available(self): self.wait_for_condition(lambda: self.params.get_bool("UpdaterFetchAvailable")) diff --git a/system/updated/updated.py b/system/updated/updated.py index d8879b4e8..68a2cdb20 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -61,10 +61,10 @@ class WaitTimeHelper: def write_time_to_param(params, param) -> None: t = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - params.put(param, t.isoformat().encode('utf8')) + params.put(param, t) def read_time_from_param(params, param) -> datetime.datetime | None: - t = params.get(param, encoding='utf8') + t = params.get(param) try: return datetime.datetime.fromisoformat(t) except (TypeError, ValueError): @@ -242,7 +242,7 @@ class Updater: @property def target_branch(self) -> str: - b: str | None = self.params.get("UpdaterTargetBranch", encoding='utf-8') + b: str | None = self.params.get("UpdaterTargetBranch") if b is None: b = self.get_branch(BASEDIR) return b @@ -272,7 +272,7 @@ class Updater: return run(["git", "rev-parse", "HEAD"], path).rstrip() def set_params(self, update_success: bool, failed_count: int, exception: str | None) -> None: - self.params.put("UpdateFailedCount", str(failed_count)) + self.params.put("UpdateFailedCount", failed_count) self.params.put("UpdaterTargetBranch", self.target_branch) self.params.put_bool("UpdaterFetchAvailable", self.update_available) @@ -429,8 +429,8 @@ def main() -> None: cloudlog.event("update installed") if not params.get("InstallDate"): - t = datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat() - params.put("InstallDate", t.encode('utf8')) + t = datetime.datetime.now(datetime.UTC).replace(tzinfo=None) + params.put("InstallDate", t) updater = Updater() update_failed_count = 0 # TODO: Load from param? diff --git a/system/version.py b/system/version.py index 2d8a387bf..cd0e2d7f3 100755 --- a/system/version.py +++ b/system/version.py @@ -15,8 +15,8 @@ TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging', 'nightly-dev'] BUILD_METADATA_FILENAME = "build.json" -training_version: bytes = b"0.2.0" -terms_version: bytes = b"2" +training_version: str = "0.2.0" +terms_version: str = "2" def get_version(path: str = BASEDIR) -> str: diff --git a/tools/camerastream/best.onnx b/tools/camerastream/best.onnx new file mode 100644 index 000000000..2c387a4e4 Binary files /dev/null and b/tools/camerastream/best.onnx differ diff --git a/tools/camerastream/best.pt b/tools/camerastream/best.pt new file mode 100644 index 000000000..1fb5d7b10 Binary files /dev/null and b/tools/camerastream/best.pt differ diff --git a/tools/camerastream/compressed_vipc.py b/tools/camerastream/compressed_vipc.py index 8c773f9df..6c4320fc5 100755 --- a/tools/camerastream/compressed_vipc.py +++ b/tools/camerastream/compressed_vipc.py @@ -112,8 +112,15 @@ class CompressedVipc: os.environ["ZMQ"] = "1" messaging.reset_context() sm = messaging.SubMaster([ENCODE_SOCKETS[s] for s in vision_streams], addr=addr) + print("waiting for:", [ENCODE_SOCKETS[s] for s in vision_streams]) + print("initial recv_frame:", sm.recv_frame) + import time + t0 = time.time() while min(sm.recv_frame.values()) == 0: sm.update(100) + if time.time() - t0 > 1.0: + print("still waiting, recv_frame:", sm.recv_frame) + t0 = time.time() os.environ.pop("ZMQ") messaging.reset_context() diff --git a/tools/camerastream/orin_yolo.py b/tools/camerastream/orin_yolo.py new file mode 100755 index 000000000..061cc8873 --- /dev/null +++ b/tools/camerastream/orin_yolo.py @@ -0,0 +1,373 @@ +#!/usr/bin/env python3 +import os +import time +import argparse +import numpy as np +import av + +import cereal.messaging as messaging + +# openpilot encoderd EncodeData uses this flag for keyframe +V4L2_BUF_FLAG_KEYFRAME = 8 + +def ensure_dir(p): + os.makedirs(p, exist_ok=True) + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("addr", help="C3X IP (where bridge.cc is publishing roadEncodeData)") + parser.add_argument("--model", default="yolo11n.pt", help="yolo11n.pt or yolo11s.pt etc") + parser.add_argument("--conf", type=float, default=0.25) + parser.add_argument("--imgsz", type=int, default=640) + parser.add_argument("--device", default="0", help="0 for cuda:0 (if torch sees it), or 'cpu'") + parser.add_argument("--skip", type=int, default=2, help="run YOLO every N frames (reduce load)") + parser.add_argument("--conflate", action="store_true", help="ZMQ conflate (latest-only) - good for realtime") + parser.add_argument("--print_every", type=int, default=1, help="print every N yolo runs") + + # NEW: dump frames for verification + parser.add_argument("--dump_dir", default="/tmp/orin_dump", help="where to save jpg frames") + parser.add_argument("--dump_every", type=int, default=30, help="save a raw decoded frame every N decoded frames (0 disables)") + parser.add_argument("--dump_max", type=int, default=50, help="max number of saved raw frames") + parser.add_argument("--dump_yolo", action="store_true", help="also save YOLO overlay frames when YOLO runs") + parser.add_argument("--dump_first", action="store_true", help="save the first decoded frame immediately (recommended)") + + args = parser.parse_args() + + # OpenCV only for saving jpg + overlay text/boxes + import cv2 + + # Ultralytics YOLO + from ultralytics import YOLO + model = YOLO(args.model) + + # Subscribe to roadEncodeData from C3X via ZMQ + os.environ["ZMQ"] = "1" + messaging.reset_context() + sock = messaging.sub_sock("roadEncodeData", None, addr=args.addr, conflate=args.conflate) + + # HEVC decoder (PyAV/FFmpeg) + codec = av.CodecContext.create("hevc", "r") + + seen_iframe = False + last_encode_id = -1 + frame_cnt = 0 + yolo_run_cnt = 0 + + ensure_dir(args.dump_dir) + raw_dump_cnt = 0 + yolo_dump_cnt = 0 + first_dump_done = False + + print("[orin] subscribing roadEncodeData from", args.addr) + print("[orin] model:", args.model, "conf:", args.conf, "imgsz:", args.imgsz, "device:", args.device) + print("[orin] dump_dir:", args.dump_dir, "dump_every:", args.dump_every, "dump_max:", args.dump_max, + "dump_yolo:", args.dump_yolo, "dump_first:", args.dump_first) + + def save_jpg(path, img_bgr, quality=90): + ok = cv2.imwrite(path, img_bgr, [int(cv2.IMWRITE_JPEG_QUALITY), int(quality)]) + if not ok: + print("[orin] WARN: failed to write", path) + return ok + + def draw_boxes(img_bgr, r): + # r is results[0] + boxes = r.boxes + if boxes is None or len(boxes) == 0: + return 0 + names = r.names + xyxy = boxes.xyxy.cpu().numpy() + confs = boxes.conf.cpu().numpy() + clss = boxes.cls.cpu().numpy().astype(int) + + n = 0 + for (x1, y1, x2, y2), cf, ci in zip(xyxy, confs, clss): + x1i, y1i, x2i, y2i = int(x1), int(y1), int(x2), int(y2) + cls_name = names.get(ci, str(ci)) if isinstance(names, dict) else str(ci) + label = f"{cls_name} {cf:.2f}" + + cv2.rectangle(img_bgr, (x1i, y1i), (x2i, y2i), (0, 255, 0), 2) + cv2.putText(img_bgr, label, (x1i, max(0, y1i - 5)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2, cv2.LINE_AA) + n += 1 + return n + + while True: + msgs = messaging.drain_sock(sock, wait_for_one=True) + for evt in msgs: + evta = getattr(evt, evt.which()) # EncodeData + + encode_id = int(evta.idx.encodeId) + if last_encode_id != -1 and encode_id != last_encode_id + 1: + print(f"[orin] DROP? encodeId {last_encode_id} -> {encode_id}") + last_encode_id = encode_id + + # need first keyframe to init decoder correctly + if not seen_iframe: + if not (evta.idx.flags & V4L2_BUF_FLAG_KEYFRAME): + continue + try: + codec.decode(av.packet.Packet(evta.header)) + except Exception as e: + print("[orin] header decode error:", e) + continue + seen_iframe = True + print("[orin] got first iframe/header, start decoding...") + + # decode this frame + try: + frames = codec.decode(av.packet.Packet(evta.data)) + except Exception as e: + print("[orin] decode error:", e) + continue + if len(frames) < 1: + continue + + frame = frames[0] + img_bgr = frame.to_ndarray(format="bgr24") + frame_cnt += 1 + + # dump first frame immediately (for sanity) + if args.dump_first and (not first_dump_done): + ts = int(time.time() * 1000) + path = os.path.join(args.dump_dir, f"raw_first_eid{encode_id}_t{ts}.jpg") + save_jpg(path, img_bgr) + first_dump_done = True + print("[orin] saved first frame:", path, "shape=", img_bgr.shape) + + # periodic raw dump (decoded frame, no yolo) + if args.dump_every > 0 and raw_dump_cnt < args.dump_max and (frame_cnt % args.dump_every) == 0: + ts = int(time.time() * 1000) + path = os.path.join(args.dump_dir, f"raw_f{frame_cnt:07d}_eid{encode_id}_t{ts}.jpg") + if save_jpg(path, img_bgr): + raw_dump_cnt += 1 + print("[orin] saved raw frame:", path, "shape=", img_bgr.shape) + + # YOLO skip + if args.skip > 1 and (frame_cnt % args.skip) != 0: + continue + + # YOLO inference + results = model.predict( + source=img_bgr, + imgsz=args.imgsz, + conf=args.conf, + device=args.device, + verbose=False + ) + + yolo_run_cnt += 1 + if args.print_every > 1 and (yolo_run_cnt % args.print_every) != 0: + continue + + r = results[0] + boxes = r.boxes + det_n = 0 if (boxes is None) else len(boxes) + + print(f"[orin] encodeId={encode_id} det={det_n}") + + # print detections + if boxes is not None and det_n > 0: + names = r.names + xyxy = boxes.xyxy.cpu().numpy() + confs = boxes.conf.cpu().numpy() + clss = boxes.cls.cpu().numpy().astype(int) + for (x1, y1, x2, y2), cf, ci in zip(xyxy, confs, clss): + cls_name = names.get(ci, str(ci)) if isinstance(names, dict) else str(ci) + print(f" - {cls_name:>12s} conf={cf:.2f} box=({x1:.1f},{y1:.1f})-({x2:.1f},{y2:.1f})") + + # optional: dump yolo overlay frame + if args.dump_yolo: + img_vis = img_bgr.copy()#!/usr/bin/env python3 +import os +import time +import argparse +import numpy as np +import av +import cv2 + +import cereal.messaging as messaging + +# openpilot encoderd EncodeData uses this flag for keyframe +V4L2_BUF_FLAG_KEYFRAME = 8 + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("addr", help="C3X IP (where bridge.cc is publishing roadEncodeData)") + parser.add_argument("--model", default="yolo11n.pt", help="yolo11n.pt or yolo11s.pt etc") + parser.add_argument("--conf", type=float, default=0.25) + parser.add_argument("--imgsz", type=int, default=640) + parser.add_argument("--device", default="0", help="0 for cuda:0 (if torch sees it), or 'cpu'") + parser.add_argument("--skip", type=int, default=2, help="run YOLO every N frames (reduce load)") + parser.add_argument("--conflate", action="store_true", help="ZMQ conflate (latest-only) - good for realtime") + parser.add_argument("--print_every", type=int, default=1, help="print every N yolo runs") + + # Save annotated images (for verification) + parser.add_argument("--save_dir", default="/tmp/orin_yolo", help="save annotated jpg here") + parser.add_argument("--save_every", type=int, default=1, help="save every N yolo runs") + parser.add_argument("--save_max", type=int, default=200, help="max saved images") + parser.add_argument("--save_raw_first", action="store_true", help="save first decoded raw frame too") + parser.add_argument("--jpg_quality", type=int, default=90, help="jpeg quality 0~100") + + args = parser.parse_args() + + os.makedirs(args.save_dir, exist_ok=True) + save_cnt = 0 + raw_first_saved = False + + # Ultralytics YOLO + from ultralytics import YOLO + model = YOLO(args.model) + + # Subscribe to roadEncodeData from C3X via ZMQ + os.environ["ZMQ"] = "1" + messaging.reset_context() + sock = messaging.sub_sock("roadEncodeData", None, addr=args.addr, conflate=args.conflate) + + # HEVC decoder (PyAV/FFmpeg) + codec = av.CodecContext.create("hevc", "r") + + seen_iframe = False + last_encode_id = -1 + frame_cnt = 0 + yolo_run_cnt = 0 + + print("[orin] subscribing roadEncodeData from", args.addr) + print("[orin] model:", args.model, "conf:", args.conf, "imgsz:", args.imgsz, "device:", args.device) + print("[orin] save_dir:", args.save_dir, "save_every:", args.save_every, "save_max:", args.save_max) + + def save_jpg(path, img_bgr): + ok = cv2.imwrite(path, img_bgr, [int(cv2.IMWRITE_JPEG_QUALITY), int(args.jpg_quality)]) + if not ok: + print("[orin] WARN: failed to write", path) + return ok + + def draw_and_save(img_bgr, r, encode_id, frame_cnt, infer_ms): + nonlocal save_cnt + if save_cnt >= args.save_max: + return + + img = img_bgr.copy() + + det_n = 0 if (r.boxes is None) else len(r.boxes) + info = f"eid={encode_id} frame={frame_cnt} det={det_n} infer={infer_ms:.1f}ms imgsz={args.imgsz}" + cv2.putText(img, info, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2, cv2.LINE_AA) + + # Draw boxes + if r.boxes is not None and det_n > 0: + names = r.names + xyxy = r.boxes.xyxy.cpu().numpy() + confs = r.boxes.conf.cpu().numpy() + clss = r.boxes.cls.cpu().numpy().astype(int) + + for (x1, y1, x2, y2), cf, ci in zip(xyxy, confs, clss): + x1i, y1i, x2i, y2i = int(x1), int(y1), int(x2), int(y2) + cls_name = names.get(ci, str(ci)) if isinstance(names, dict) else str(ci) + label = f"{cls_name} {cf:.2f}" + + cv2.rectangle(img, (x1i, y1i), (x2i, y2i), (0, 255, 0), 2) + cv2.putText(img, label, (x1i, max(0, y1i - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2, cv2.LINE_AA) + + path = os.path.join(args.save_dir, f"yolo_eid{encode_id}_f{frame_cnt:07d}_d{det_n}.jpg") + if save_jpg(path, img): + save_cnt += 1 + print("[orin] saved:", path) + + while True: + # wait_for_one=True blocks until at least one message arrives + msgs = messaging.drain_sock(sock, wait_for_one=True) + for evt in msgs: + evta = getattr(evt, evt.which()) # EncodeData + + encode_id = int(evta.idx.encodeId) + if last_encode_id != -1 and encode_id != last_encode_id + 1: + print(f"[orin] DROP? encodeId {last_encode_id} -> {encode_id}") + last_encode_id = encode_id + + # need first keyframe to init decoder correctly + if not seen_iframe: + if not (evta.idx.flags & V4L2_BUF_FLAG_KEYFRAME): + continue + # feed header once + try: + codec.decode(av.packet.Packet(evta.header)) + except Exception as e: + print("[orin] header decode error:", e) + continue + seen_iframe = True + print("[orin] got first iframe/header, start decoding...") + + # decode this frame + try: + frames = codec.decode(av.packet.Packet(evta.data)) + except Exception as e: + print("[orin] decode error:", e) + continue + + if len(frames) < 1: + continue + + frame = frames[0] + img_bgr = frame.to_ndarray(format="bgr24") + frame_cnt += 1 + + # Optional: save first raw decoded frame + if args.save_raw_first and not raw_first_saved: + raw_path = os.path.join(args.save_dir, f"raw_first_eid{encode_id}_f{frame_cnt:07d}.jpg") + if save_jpg(raw_path, img_bgr): + raw_first_saved = True + print("[orin] saved raw first:", raw_path, "shape=", img_bgr.shape) + + # Skip inference frames + if args.skip > 1 and (frame_cnt % args.skip) != 0: + continue + + # YOLO inference timing + t0 = time.perf_counter() + results = model.predict( + source=img_bgr, + imgsz=args.imgsz, + conf=args.conf, + device=args.device, + verbose=False + ) + infer_ms = (time.perf_counter() - t0) * 1000.0 + + yolo_run_cnt += 1 + + # Print every N yolo runs + if args.print_every <= 1 or (yolo_run_cnt % args.print_every) == 0: + r = results[0] + boxes = r.boxes + det_n = 0 if (boxes is None) else len(boxes) + print(f"[orin] encodeId={encode_id} det={det_n} infer={infer_ms:.1f}ms") + + if boxes is not None and det_n > 0: + names = r.names + xyxy = boxes.xyxy.cpu().numpy() + confs = boxes.conf.cpu().numpy() + clss = boxes.cls.cpu().numpy().astype(int) + for (x1, y1, x2, y2), cf, ci in zip(xyxy, confs, clss): + cls_name = names.get(ci, str(ci)) if isinstance(names, dict) else str(ci) + print(f" - {cls_name:>12s} conf={cf:.2f} box=({x1:.1f},{y1:.1f})-({x2:.1f},{y2:.1f})") + + # Save annotated image every N yolo runs + if args.save_every > 0 and (yolo_run_cnt % args.save_every) == 0: + r = results[0] + draw_and_save(img_bgr, r, encode_id, frame_cnt, infer_ms) + + # Stop saving if reached max (still keep running inference/prints) + if save_cnt >= args.save_max: + # You can break if you want to stop entirely: + # return + pass + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass + diff --git a/tools/camerastream/vipc_road.py b/tools/camerastream/vipc_road.py new file mode 100644 index 000000000..71ef4c12d --- /dev/null +++ b/tools/camerastream/vipc_road.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import os, time, argparse +import av +from PIL import Image + +import cereal.messaging as messaging + +SOCK_NAME = "roadEncodeData" + +def atomic_write_jpeg(path: str, img: Image.Image, quality: int = 80): + tmp = path + ".tmp" + img.save(tmp, format="JPEG", quality=quality, optimize=False) + os.replace(tmp, path) + +def main(addr: str, conflate: bool, dump_path: str, debug: bool, every_n: int): + os.environ["ZMQ"] = "1" + messaging.reset_context() + sock = messaging.sub_sock(SOCK_NAME, None, addr=addr, conflate=conflate) + + codec = av.CodecContext.create("hevc", "r") + + W = H = None + seen_header = False + decoded = dropped = 0 + last_stat = time.time() + + while True: + msgs = messaging.drain_sock(sock, wait_for_one=True) + if not msgs: + continue + + for evt in msgs: + evta = getattr(evt, evt.which()) + + if W is None: + W, H = int(evta.width), int(evta.height) + if debug: + print(f"[road] stream size {W}x{H}", flush=True) + + if (not seen_header) and evta.header: + try: + codec.decode(av.packet.Packet(evta.header)) + seen_header = True + if debug: + print("[road] header decoded", flush=True) + except Exception as e: + if debug: + print("[road] header decode error:", e, flush=True) + + try: + frames = codec.decode(av.packet.Packet(evta.data)) + except Exception: + dropped += 1 + continue + + if not frames: + dropped += 1 + continue + + decoded += 1 + if dump_path and (decoded % every_n == 0): + f = frames[0] + # PyAV -> RGB numpy -> PIL + rgb = f.to_ndarray(format="rgb24") + img = Image.fromarray(rgb) + atomic_write_jpeg(dump_path, img, quality=80) + + now = time.time() + if debug and now - last_stat > 1.0: + print(f"[road] decoded={decoded} dropped={dropped} batch={len(msgs)}", flush=True) + last_stat = now + +if __name__ == "__main__": + ap = argparse.ArgumentParser() + ap.add_argument("addr") + ap.add_argument("--conflate", action="store_true") + ap.add_argument("--dump", default="/tmp/road.jpg") + ap.add_argument("--debug", action="store_true") + ap.add_argument("--every", type=int, default=3, help="write every N decoded frames") + args = ap.parse_args() + + main(args.addr, args.conflate, args.dump, args.debug, args.every) diff --git a/tools/camerastream/yolo_video.py b/tools/camerastream/yolo_video.py new file mode 100755 index 000000000..9cf51dd43 --- /dev/null +++ b/tools/camerastream/yolo_video.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +import argparse +import time +import cv2 +from ultralytics import YOLO + + +def is_traffic_light(det_name: str) -> bool: + # �뜲�씠�꽣�뀑�뿉 �뵲�씪 �씪踰⑤챸�씠 �떎瑜� �닔 �엳�뼱�꽌 �꼮�꼮�븯寃� �옟�뒿�땲�떎. + # �삁: "traffic light", "trafficlight", "tl", "signal" �벑 + n = det_name.lower().replace("_", " ").strip() + return ("traffic" in n and "light" in n) or ("trafficlight" in n) or (n in ["tl", "signal", "signal light"]) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--model", default="best.pt", help="YOLO .pt path") + parser.add_argument("--source", default=0, + help="video path OR camera index (0,1,...) OR rtsp url") + parser.add_argument("--imgsz", type=int, default=640) + parser.add_argument("--conf", type=float, default=0.25) + parser.add_argument("--iou", type=float, default=0.45) + parser.add_argument("--device", default="0", help="0,1,... or 'cpu'") + parser.add_argument("--show", action="store_true", help="show window") + parser.add_argument("--save", default="", help="output mp4 path (optional)") + parser.add_argument("--only_tl", action="store_true", help="filter traffic light only") + parser.add_argument("--max_fps", type=float, default=0.0, help="limit fps (0=unlimited)") + args = parser.parse_args() + + # source �뙆�떛: �닽�옄硫� 移대찓�씪 �씤�뜳�뒪濡� + src = args.source + if isinstance(src, str) and src.isdigit(): + src = int(src) + + cap = cv2.VideoCapture(src) + if not cap.isOpened(): + raise RuntimeError(f"Failed to open source: {args.source}") + + model = YOLO(args.model) + + w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 0) + h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 0) + in_fps = float(cap.get(cv2.CAP_PROP_FPS) or 30.0) + + writer = None + if args.save: + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + writer = cv2.VideoWriter(args.save, fourcc, in_fps, (w, h)) + + prev_t = time.time() + frame_idx = 0 + + while True: + ok, frame = cap.read() + if not ok: + break + + # FPS �젣�븳(�샃�뀡) + if args.max_fps > 0: + now = time.time() + dt = now - prev_t + min_dt = 1.0 / args.max_fps + if dt < min_dt: + time.sleep(min_dt - dt) + prev_t = time.time() + + # YOLO 異붾줎 + results = model.predict( + source=frame, + imgsz=args.imgsz, + conf=args.conf, + iou=args.iou, + device=args.device, + verbose=False, + )[0] + + names = results.names # class id -> name + annotated = frame + + # 諛뺤뒪 �닔�룞 洹몃━湲�(��쒖떊�샇�벑留뚢�� �븘�꽣 媛��뒫) + if results.boxes is not None and len(results.boxes) > 0: + print (results) + for b in results.boxes: + cls_id = int(b.cls.item()) + name = names.get(cls_id, str(cls_id)) + if args.only_tl and not is_traffic_light(name): + continue + + conf = float(b.conf.item()) + x1, y1, x2, y2 = map(int, b.xyxy[0].tolist()) + + cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.putText(annotated, f"{name} {conf:.2f}", (x1, max(0, y1 - 7)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) + + if writer is not None: + writer.write(annotated) + + if args.show: + cv2.imshow("YOLO Traffic Light", annotated) + key = cv2.waitKey(1) & 0xFF + if key == 27 or key == ord('q'): + break + + frame_idx += 1 + + cap.release() + if writer is not None: + writer.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main()