diff --git a/.github/workflows/auto_pr_review.yaml b/.github/workflows/auto_pr_review.yaml index 977460f5bc..06b7044934 100644 --- a/.github/workflows/auto_pr_review.yaml +++ b/.github/workflows/auto_pr_review.yaml @@ -40,10 +40,10 @@ jobs: runs-on: ubuntu-latest if: (github.event.pull_request.head.repo.fork && (contains(github.event_name, 'pull_request') && github.event.action == 'synchronize')) env: - PR_LABEL: 'dev-c3' + PR_LABEL: 'dev' TRUST_FORK_PR_LABEL: 'trust-fork-pr' steps: - - name: Check if PR has dev-c3 label + - name: Check if PR has dev label id: check-labels uses: actions/github-script@v7 with: @@ -62,11 +62,11 @@ jobs: console.log(`PR #${prNumber} has ${process.env.PR_LABEL} label: ${hasDevC3Label}`); console.log(`PR #${prNumber} has ${process.env.TRUST_FORK_PR_LABEL} label: ${hasTrustLabel}`); - core.setOutput('has-dev-c3', hasDevC3Label ? 'true' : 'false'); + core.setOutput('has-dev', hasDevC3Label ? 'true' : 'false'); core.setOutput('has-trust', hasTrustLabel ? 'true' : 'false'); - name: Remove trust-fork-pr label if present - if: steps.check-labels.outputs.has-dev-c3 == 'true' && steps.check-labels.outputs.has-trust == 'true' + if: steps.check-labels.outputs.has-dev == 'true' && steps.check-labels.outputs.has-trust == 'true' uses: actions/github-script@v7 with: github-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 97bcc61783..70b3f09eaa 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -107,7 +107,6 @@ jobs: build_mac: name: build macOS - if: false # temp disable since homebrew install is getting stuck runs-on: ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-macos-8x14' || 'macos-latest' }} steps: - uses: actions/checkout@v4 @@ -116,7 +115,9 @@ jobs: - run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV - name: Homebrew cache uses: ./.github/workflows/auto-cache + if: false # disabling the cache for now because it is breaking macos builds... with: + save: false # No need save here if we manually save it later conditionally path: ~/Library/Caches/Homebrew key: brew-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} restore-keys: | @@ -125,8 +126,8 @@ jobs: - name: Install dependencies run: ./tools/mac_setup.sh env: - # package install has DeprecationWarnings - PYTHONWARNINGS: default + PYTHONWARNINGS: default # package install has DeprecationWarnings + HOMEBREW_DISPLAY_INSTALL_TIMES: 1 - name: Save Homebrew cache uses: actions/cache/save@v4 if: github.ref == 'refs/heads/master-tici' @@ -137,6 +138,7 @@ jobs: - name: Getting scons cache uses: ./.github/workflows/auto-cache with: + save: false # No need save here if we manually save it later conditionally path: /tmp/scons_cache key: scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }} restore-keys: | diff --git a/.github/workflows/sunnypilot-master-dev-c3-prep.yaml b/.github/workflows/sunnypilot-master-dev-prep.yaml similarity index 100% rename from .github/workflows/sunnypilot-master-dev-c3-prep.yaml rename to .github/workflows/sunnypilot-master-dev-prep.yaml diff --git a/.gitignore b/.gitignore index 3c0dad521d..00a0533d86 100644 --- a/.gitignore +++ b/.gitignore @@ -50,7 +50,6 @@ cereal/services.h cereal/gen cereal/messaging/bridge selfdrive/mapd/default_speeds_by_region.json -system/proclogd/proclogd selfdrive/ui/translations/tmp selfdrive/test/longitudinal_maneuvers/out selfdrive/car/tests/cars_dump diff --git a/.vscode/launch.json b/.vscode/launch.json index a6b341d9ea..f090061c42 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -23,6 +23,11 @@ "id": "args", "description": "Arguments to pass to the process", "type": "promptString" + }, + { + "id": "replayArg", + "type": "promptString", + "description": "Enter route or segment to replay." } ], "configurations": [ @@ -40,7 +45,41 @@ "type": "cppdbg", "request": "launch", "program": "${workspaceFolder}/${input:cpp_process}", - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" + }, + { + "name": "Attach LLDB to Replay drive", + "type": "lldb", + "request": "attach", + "pid": "${command:pickMyProcess}", + "initCommands": [ + "script import time; time.sleep(3)" + ] + }, + { + "name": "Replay drive", + "type": "debugpy", + "request": "launch", + "program": "${workspaceFolder}/opendbc/safety/tests/safety_replay/replay_drive.py", + "args": [ + "${input:replayArg}" + ], + "console": "integratedTerminal", + "justMyCode": false, + "env": { + "PYTHONPATH": "${workspaceFolder}" + }, + "subProcess": true, + "stopOnEntry": false + } + ], + "compounds": [ + { + "name": "Replay drive + Safety LLDB", + "configurations": [ + "Replay drive", + "Attach LLDB to Replay drive" + ] } ] } \ No newline at end of file diff --git a/Jenkinsfile b/Jenkinsfile index 0905abd6da..f3a63d3dec 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -178,7 +178,7 @@ node { try { if (env.BRANCH_NAME == 'devel-staging') { - deviceStage("build release3-staging", "tici-needs-can", [], [ + deviceStage("build release3-staging", "tizi-needs-can", [], [ step("build release3-staging", "RELEASE_BRANCH=release3-staging $SOURCE_DIR/release/build_release.sh"), ]) } @@ -186,12 +186,12 @@ node { if (env.BRANCH_NAME == '__nightly') { parallel ( 'nightly': { - deviceStage("build nightly", "tici-needs-can", [], [ + deviceStage("build nightly", "tizi-needs-can", [], [ step("build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"), ]) }, 'nightly-dev': { - deviceStage("build nightly-dev", "tici-needs-can", [], [ + deviceStage("build nightly-dev", "tizi-needs-can", [], [ step("build nightly-dev", "PANDA_DEBUG_BUILD=1 RELEASE_BRANCH=nightly-dev $SOURCE_DIR/release/build_release.sh"), ]) }, @@ -200,39 +200,30 @@ node { if (!env.BRANCH_NAME.matches(excludeRegex)) { parallel ( - // tici tests 'onroad tests': { - deviceStage("onroad", "tici-needs-can", ["UNSAFE=1"], [ + deviceStage("onroad", "tizi-needs-can", ["UNSAFE=1"], [ step("build openpilot", "cd system/manager && ./build.py"), step("check dirty", "release/check-dirty.sh"), step("onroad tests", "pytest selfdrive/test/test_onroad.py -s", [timeout: 60]), ]) }, 'HW + Unit Tests': { - deviceStage("tici-hardware", "tici-common", ["UNSAFE=1"], [ + deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [ step("build", "cd system/manager && ./build.py"), step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]), step("test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.py"), step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py", [diffPaths: ["system/loggerd/"]]), - step("test pigeond", "pytest system/ubloxd/tests/test_pigeond.py", [diffPaths: ["system/ubloxd/"]]), step("test manager", "pytest system/manager/test/test_manager.py"), ]) }, 'loopback': { - deviceStage("loopback", "tici-loopback", ["UNSAFE=1"], [ + deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [ step("build openpilot", "cd system/manager && ./build.py"), step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"), ]) }, - 'camerad AR0231': { - deviceStage("AR0231", "tici-ar0231", ["UNSAFE=1"], [ - step("build", "cd system/manager && ./build.py"), - step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]), - step("test exposure", "pytest system/camerad/test/test_exposure.py"), - ]) - }, 'camerad OX03C10': { - deviceStage("OX03C10", "tici-ox03c10", ["UNSAFE=1"], [ + deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [ step("build", "cd system/manager && ./build.py"), step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]), step("test exposure", "pytest system/camerad/test/test_exposure.py"), @@ -246,17 +237,13 @@ node { ]) }, 'sensord': { - deviceStage("LSM + MMC", "tici-lsmc", ["UNSAFE=1"], [ - step("build", "cd system/manager && ./build.py"), - step("test sensord", "pytest system/sensord/tests/test_sensord.py"), - ]) - deviceStage("BMX + LSM", "tici-bmx-lsm", ["UNSAFE=1"], [ + deviceStage("LSM + MMC", "tizi-lsmc", ["UNSAFE=1"], [ step("build", "cd system/manager && ./build.py"), step("test sensord", "pytest system/sensord/tests/test_sensord.py"), ]) }, 'replay': { - deviceStage("model-replay", "tici-replay", ["UNSAFE=1"], [ + deviceStage("model-replay", "tizi-replay", ["UNSAFE=1"], [ step("build", "cd system/manager && ./build.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]), step("model replay", "selfdrive/test/process_replay/model_replay.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]), ]) @@ -266,7 +253,6 @@ node { step("build openpilot", "cd system/manager && ./build.py"), step("test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"), step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"), - step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]), step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"), // TODO: enable once new AGNOS is available // step("test esim", "pytest system/hardware/tici/tests/test_esim.py"), diff --git a/RELEASES.md b/RELEASES.md index dacf0eaa17..189aa7ad54 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,7 +1,13 @@ Version 0.10.1 (2025-09-08) ======================== -* Record driving feedback using LKAS button -* Honda City 2023 support thanks to drFritz! +* New driving model + * World Model: removed global localization inputs + * World Model: 2x the number of parameters + * World Model: trained on 4x the number of segments + * Driving Vision Model: trained on 4x the number of segments +* Honda City 2023 support thanks to vanillagorillaa and drFritz! +* Honda N-Box 2018 support thanks to miettal! +* Honda Odyssey 2021-25 support thanks to csouers and MVL! Version 0.10.0 (2025-08-05) ======================== diff --git a/SConstruct b/SConstruct index 192f83a0f6..12d879b350 100644 --- a/SConstruct +++ b/SConstruct @@ -359,11 +359,6 @@ SConscript([ 'system/ubloxd/SConscript', 'system/loggerd/SConscript', ]) -if arch != "Darwin": - SConscript([ - 'system/logcatd/SConscript', - 'system/proclogd/SConscript', - ]) if arch == "larch64": SConscript(['system/camerad/SConscript']) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8d4bc3cb82..44b8bd69fa 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -25,6 +25,26 @@ struct ModularAssistiveDrivingSystem { } } +struct IntelligentCruiseButtonManagement { + state @0 :IntelligentCruiseButtonManagementState; + sendButton @1 :SendButtonState; + vTarget @2 :Float32; + + enum IntelligentCruiseButtonManagementState { + inactive @0; # No button press or default state + preActive @1; # Pre-active state before transitioning to increasing or decreasing + increasing @2; # Increasing speed + decreasing @3; # Decreasing speed + holding @4; # Holding steady speed + } + + enum SendButtonState { + none @0; + increase @1; + decrease @2; + } +} + # Same struct as Log.RadarState.LeadData struct LeadData { dRel @0 :Float32; @@ -48,6 +68,7 @@ struct LeadData { struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; + intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement; } struct ModelManagerSP @0xaedffd8f31e7b55d { @@ -122,6 +143,13 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { dec @0 :DynamicExperimentalControl; + longitudinalPlanSource @1 :LongitudinalPlanSource; + smartCruiseControl @2 :SmartCruiseControl; + speedLimit @3 :SpeedLimit; + vTarget @4 :Float32; + aTarget @5 :Float32; + events @6 :List(OnroadEventSP.Event); + e2eAlerts @7 :E2eAlerts; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -133,6 +161,97 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { blended @1; } } + + struct SmartCruiseControl { + vision @0 :Vision; + map @1 :Map; + + struct Vision { + state @0 :VisionState; + vTarget @1 :Float32; + aTarget @2 :Float32; + currentLateralAccel @3 :Float32; + maxPredictedLateralAccel @4 :Float32; + enabled @5 :Bool; + active @6 :Bool; + } + + struct Map { + state @0 :MapState; + vTarget @1 :Float32; + aTarget @2 :Float32; + enabled @3 :Bool; + active @4 :Bool; + } + + enum VisionState { + disabled @0; # System disabled or inactive. + enabled @1; # No predicted substantial turn on vision range. + entering @2; # A substantial turn is predicted ahead, adapting speed to turn comfort levels. + turning @3; # Actively turning. Managing acceleration to provide a roll on turn feeling. + leaving @4; # Road ahead straightens. Start to allow positive acceleration. + overriding @5; # System overriding with manual control. + } + + enum MapState { + disabled @0; # System disabled or inactive. + enabled @1; # No predicted substantial turn on map range. + turning @2; # Actively turning. Managing acceleration to provide a roll on turn feeling. + overriding @3; # System overriding with manual control. + } + } + + struct SpeedLimit { + resolver @0 :Resolver; + assist @1 :Assist; + + struct Resolver { + speedLimit @0 :Float32; + distToSpeedLimit @1 :Float32; + source @2 :Source; + speedLimitOffset @3 :Float32; + speedLimitLast @4 :Float32; + speedLimitFinal @5 :Float32; + speedLimitFinalLast @6 :Float32; + speedLimitValid @7 :Bool; + speedLimitLastValid @8 :Bool; + } + + struct Assist { + state @0 :AssistState; + enabled @1 :Bool; + active @2 :Bool; + vTarget @3 :Float32; + aTarget @4 :Float32; + } + + enum Source { + none @0; + car @1; + map @2; + } + + enum AssistState { + disabled @0; + inactive @1; # No speed limit set or not enabled by parameter. + preActive @2; + pending @3; # Awaiting new speed limit. + adapting @4; # Reducing speed to match new speed limit. + active @5; # Cruising at speed limit. + } + } + + enum LongitudinalPlanSource { + cruise @0; + sccVision @1; + sccMap @2; + speedLimitAssist @3; + } + + struct E2eAlerts { + greenLightAlert @0 :Bool; + leadDepartAlert @1 :Bool; + } } struct OnroadEventSP @0xda96579883444c35 { @@ -174,12 +293,20 @@ struct OnroadEventSP @0xda96579883444c35 { pedalPressedAlertOnly @16; laneTurnLeft @17; laneTurnRight @18; + speedLimitPreActive @19; + speedLimitActive @20; + speedLimitChanged @21; + speedLimitPending @22; + e2eChime @23; } } struct CarParamsSP @0x80ae746ee2596b11 { flags @0 :UInt32; # flags for car specific quirks in sunnypilot safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags + pcmCruiseSpeed @3 :Bool; + intelligentCruiseButtonManagementAvailable @4 :Bool; + enableGasInterceptor @5 :Bool; neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; @@ -199,6 +326,7 @@ struct CarControlSP @0xa5cd762cd951a455 { params @1 :List(Param); leadOne @2 :LeadData; leadTwo @3 :LeadData; + intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; struct Param { key @0 :Text; @@ -207,7 +335,7 @@ struct CarControlSP @0xa5cd762cd951a455 { valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility. } - + enum ParamType { string @0; bool @1; @@ -262,6 +390,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { } struct CarStateSP @0xb86e6369214c01c8 { + speedLimit @0 :Float32; } struct LiveMapDataSP @0xf416ec09499d9d19 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 474b4777cb..8925bca8ae 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -585,7 +585,6 @@ struct PandaState @0xa7649e2575e4591e { heartbeatLost @22 :Bool; interruptLoad @25 :Float32; fanPower @28 :UInt8; - fanStallCount @34 :UInt8; spiErrorCount @33 :UInt16; @@ -714,6 +713,7 @@ struct PandaState @0xa7649e2575e4591e { usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; safetyParamDEPRECATED @20 :Int16; safetyParam2DEPRECATED @26 :UInt32; + fanStallCountDEPRECATED @34 :UInt8; } struct PeripheralState { @@ -2684,7 +2684,7 @@ struct Event { lateralPlanDEPRECATED @64 :LateralPlan; navModelDEPRECATED @104 :NavModelData; uiPlanDEPRECATED @106 :UiPlan; - liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; + liveLocationKalman @72 :LiveLocationKalman; liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED); } diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc index 1a7a48980e..7f7e2795c4 100644 --- a/cereal/messaging/socketmaster.cc +++ b/cereal/messaging/socketmaster.cc @@ -33,7 +33,7 @@ MessageContext message_context; struct SubMaster::SubMessage { std::string name; SubSocket *socket = nullptr; - int freq = 0; + float freq = 0.0f; bool updated = false, alive = false, valid = false, ignore_alive; uint64_t rcv_time = 0, rcv_frame = 0; void *allocated_msg_reader = nullptr; diff --git a/cereal/services.py b/cereal/services.py index b48290bc77..67548bc79e 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -89,6 +89,7 @@ _services: dict[str, tuple] = { "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), "modelDataV2SP": (True, 20.), + "liveLocationKalman": (True, 20.), # debug "uiDebug": (True, 0., 1), @@ -121,12 +122,12 @@ def build_header(): h += "#include \n" h += "#include \n" - h += "struct service { std::string name; bool should_log; int frequency; int decimation; };\n" + h += "struct service { std::string name; bool should_log; float frequency; int decimation; };\n" h += "static std::map services = {\n" for k, v in SERVICE_LIST.items(): should_log = "true" if v.should_log else "false" decimation = -1 if v.decimation is None else v.decimation - h += ' { "%s", {"%s", %s, %d, %d}},\n' % \ + h += ' { "%s", {"%s", %s, %f, %d}},\n' % \ (k, k, should_log, v.frequency, decimation) h += "};\n" diff --git a/common/model.h b/common/model.h index a984f55e8d..a1e034c06f 100644 --- a/common/model.h +++ b/common/model.h @@ -1 +1 @@ -#define DEFAULT_MODEL "Steam Powered (Default)" +#define DEFAULT_MODEL "Firehose (Default)" diff --git a/common/params_keys.h b/common/params_keys.h index fc7842720b..3b5d02a429 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -94,7 +94,6 @@ inline static std::unordered_map keys = { {"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, JSON}}, {"Offroad_TemperatureTooHigh", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_UnregisteredHardware", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_UpdateFailed", {CLEAR_ON_MANAGER_START, JSON}}, @@ -149,17 +148,29 @@ inline static std::unordered_map keys = { {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}}, {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}}, {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}}, + {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}}, + {"HideVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}}, {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}}, + {"LastGPSPositionLLK", {PERSISTENT, STRING}}, + {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}}, {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}}, {"Offroad_TiciSupport", {CLEAR_ON_MANAGER_START, JSON}}, + {"OnroadScreenOffBrightness", {PERSISTENT | BACKUP, INT, "100"}}, + {"OnroadScreenOffControl", {PERSISTENT | BACKUP, BOOL}}, + {"OnroadScreenOffTimer", {PERSISTENT | BACKUP, INT, "0"}}, + {"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}}, {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}}, {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, // MADS params {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}}, @@ -185,7 +196,8 @@ inline static std::unordered_map keys = { {"SunnylinkCache_Users", {PERSISTENT, STRING}}, {"SunnylinkDongleId", {PERSISTENT, STRING}}, {"SunnylinkdPid", {PERSISTENT, INT}}, - {"SunnylinkEnabled", {PERSISTENT, BOOL}}, + {"SunnylinkEnabled", {PERSISTENT, BOOL, "1"}}, + {"SunnylinkTempFault", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0"}}, // Backup Manager params {"BackupManager_CreateBackup", {PERSISTENT, BOOL}}, @@ -223,4 +235,25 @@ inline static std::unordered_map keys = { {"OsmStateTitle", {PERSISTENT, STRING}}, {"OsmWayTest", {PERSISTENT, STRING}}, {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, + {"RoadNameToggle", {PERSISTENT, STRING}}, + + // Speed Limit + {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}}, + {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}}, + {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}}, + {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}}, + + // Smart Cruise Control + {"MapTargetVelocities", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, + {"SmartCruiseControlMap", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}}, + + // Torque lateral control custom params + {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}}, + {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}}, + {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}}, + {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}}, + {"TorqueParamsOverrideEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"TorqueParamsOverrideFriction", {PERSISTENT | BACKUP, FLOAT, "0.1"}}, + {"TorqueParamsOverrideLatAccelFactor", {PERSISTENT | BACKUP, FLOAT, "2.5"}}, }; diff --git a/common/util.h b/common/util.h index f46db4d9fa..6d28e3ccbc 100644 --- a/common/util.h +++ b/common/util.h @@ -36,6 +36,7 @@ const double MS_TO_KPH = 3.6; const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +const double METER_TO_KM = 1. / 1000.0; #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1)) diff --git a/docs/CARS.md b/docs/CARS.md index dbc1dbcd74..25f84b389e 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 334 Supported Cars +# 337 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video|Setup Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -83,7 +83,7 @@ A supported vehicle is one that just works when you install a comma device. All |Honda|Civic Hatchback 2017-18|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback 2019-21|All|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback 2022-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Honda|Civic Hatchback Hybrid 2025|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|Civic Hatchback Hybrid 2025-26|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback Hybrid (Europe only) 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hybrid 2025|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Clarity 2018-21|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Nidec connector + Honda Clarity Proxy Board
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| @@ -99,7 +99,9 @@ A supported vehicle is one that just works when you install a comma device. All |Honda|HR-V 2023-25|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|N-Box 2018|All|openpilot available[1](#footnotes)|0 mph|11 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Odyssey 2018-20|Honda Sensing|openpilot|26 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|Odyssey 2021-25|All|openpilot available[1](#footnotes)|0 mph|43 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Passport 2019-25|All|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Pilot 2016-22|Honda Sensing|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Pilot 2023-25|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch C connector
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| @@ -163,6 +165,7 @@ A supported vehicle is one that just works when you install a comma device. All |Kia|EV6 (without HDA II) 2022-24[6](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai L connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Hyundai G connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai E connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Kia|Forte Non-SCC 2019|No Smart Cruise Control (Non-SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai G connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Kia|K8 Hybrid (with HDA II) 2023[6](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai Q connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| @@ -259,7 +262,7 @@ A supported vehicle is one that just works when you install a comma device. All |Tesla[11](#footnotes)|Model 3 (with HW3) 2019-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Tesla[11](#footnotes)|Model 3 (with HW4) 2024-25[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Tesla[11](#footnotes)|Model Y (with HW3) 2020-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Tesla[11](#footnotes)|Model Y (with HW4) 2024[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Tesla[11](#footnotes)|Model Y (with HW4) 2024-25[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md index 154734b7fc..7583095eaf 100644 --- a/docs/CONTRIBUTING.md +++ b/docs/CONTRIBUTING.md @@ -39,7 +39,7 @@ All of these are examples of good PRs: ### First contribution [Projects / openpilot bounties](https://github.com/orgs/commaai/projects/26/views/1?pane=info) is the best place to get started and goes in-depth on what's expected when working on a bounty. -There's lot of bounties that don't require a comma 3/3X or a car. +There's lot of bounties that don't require a comma 3X or a car. ## Pull Requests diff --git a/docs/DEBUGGING_SAFETY.md b/docs/DEBUGGING_SAFETY.md new file mode 100644 index 0000000000..cd0a46b446 --- /dev/null +++ b/docs/DEBUGGING_SAFETY.md @@ -0,0 +1,30 @@ +# Debugging Panda Safety with Replay Drive + LLDB + +## 1. Start the debugger in VS Code + +* Select **Replay drive + Safety LLDB**. +* Enter the route or segment when prompted. +[](https://github.com/user-attachments/assets/b0cc320a-083e-46a7-a9f8-ca775bbe5604) + +## 2. Attach LLDB + +* When prompted, pick the running **`replay_drive` process**. +* ⚠️ Attach quickly, or `replay_drive` will start consuming messages. + +> [!TIP] +> Add a Python breakpoint at the start of `replay_drive.py` to pause execution and give yourself time to attach LLDB. + +## 3. Set breakpoints in VS Code +Breakpoints can be set directly in `modes/xxx.h` (or any C file). +No extra LLDB commands are required — just place breakpoints in the editor. + +## 4. Resume execution +Once attached, you can step through both Python (on the replay) and C safety code as CAN logs are replayed. + +> [!NOTE] +> * Use short routes for quicker iteration. +> * Pause `replay_drive` early to avoid wasting log messages. + +## Video + +View a demo of this workflow on the PR that added it: https://github.com/commaai/openpilot/pull/36055#issue-3352911578 \ No newline at end of file diff --git a/docs/SAFETY.md b/docs/SAFETY.md index 18a450a395..25815e3372 100644 --- a/docs/SAFETY.md +++ b/docs/SAFETY.md @@ -16,7 +16,7 @@ industry standards of safety for Level 2 Driver Assistance Systems. In particula ISO26262 guidelines, including those from [pertinent documents](https://www.nhtsa.gov/sites/nhtsa.dot.gov/files/documents/13498a_812_573_alcsystemreport.pdf) released by NHTSA. In addition, we impose strict coding guidelines (like [MISRA C : 2012](https://www.misra.org.uk/what-is-misra/)) on parts of openpilot that are safety relevant. We also perform software-in-the-loop, -hardware-in-the-loop and in-vehicle tests before each software release. +hardware-in-the-loop, and in-vehicle tests before each software release. Following Hazard and Risk Analysis and FMEA, at a very high level, we have designed openpilot ensuring two main safety requirements. @@ -29,8 +29,18 @@ ensuring two main safety requirements. For additional safety implementation details, refer to [panda safety model](https://github.com/commaai/panda#safety-model). For vehicle specific implementation of the safety concept, refer to [opendbc/safety/safety](https://github.com/commaai/opendbc/tree/master/opendbc/safety/safety). -**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or - not fully meeting the above requirements. +[^1]: For these actuator limits we observe ISO11270 and ISO15622. Lateral limits described there translate to 0.9 seconds of maximum actuation to achieve a 1m lateral deviation. -[^1]: For these actuator limits we observe ISO11270 and ISO15622. Lateral limits described there translate to 0.9 seconds of maximum actuation to achieve a 1m lateral deviation. +--- +### Forks of openpilot + +* Do not disable or nerf [driver monitoring](https://github.com/commaai/openpilot/tree/master/selfdrive/monitoring) +* Do not disable or nerf [excessive actuation checks](https://github.com/commaai/openpilot/tree/master/selfdrive/selfdrived/helpers.py) +* If your fork modifies any of the code in `opendbc/safety/`: + * your fork cannot use the openpilot trademark + * your fork must preserve the full [safety test suite](https://github.com/commaai/opendbc/tree/master/opendbc/safety/tests) and all tests must pass, including any new coverage required by the fork's changes + +Failure to comply with these standards will get you and your users banned from comma.ai servers. + +**comma.ai strongly discourages the use of openpilot forks with safety code either missing or not fully meeting the above requirements.** diff --git a/docs/how-to/connect-to-comma.md b/docs/how-to/connect-to-comma.md index cbaccaae6a..5f02e11599 100644 --- a/docs/how-to/connect-to-comma.md +++ b/docs/how-to/connect-to-comma.md @@ -1,11 +1,11 @@ -# connect to a comma 3/3X +# connect to a comma 3X -A comma 3/3X is a normal [Linux](https://github.com/commaai/agnos-builder) computer that exposes [SSH](https://wiki.archlinux.org/title/Secure_Shell) and a [serial console](https://wiki.archlinux.org/title/Working_with_the_serial_console). +A comma 3X is a normal [Linux](https://github.com/commaai/agnos-builder) computer that exposes [SSH](https://wiki.archlinux.org/title/Secure_Shell) and a [serial console](https://wiki.archlinux.org/title/Working_with_the_serial_console). ## Serial Console On both the comma three and 3X, the serial console is accessible from the main OBD-C port. -Connect the comma 3/3X to your computer with a normal USB C cable, or use a [comma serial](https://comma.ai/shop/comma-serial) for steady 12V power. +Connect the comma 3X to your computer with a normal USB C cable, or use a [comma serial](https://comma.ai/shop/comma-serial) for steady 12V power. On the comma three, the serial console is exposed through a UART-to-USB chip, and `tools/scripts/serial.sh` can be used to connect. @@ -45,7 +45,7 @@ In order to use ADB on your device, you'll need to perform the following steps u * Here's an example command for connecting to your device using its tethered connection: `adb connect 192.168.43.1:5555` > [!NOTE] -> The default port for ADB is 5555 on the comma 3/3X. +> The default port for ADB is 5555 on the comma 3X. For more info on ADB, see the [Android Debug Bridge (ADB) documentation](https://developer.android.com/tools/adb). diff --git a/docs/how-to/replay-a-drive.md b/docs/how-to/replay-a-drive.md index 084b6bf825..b0db36a46f 100644 --- a/docs/how-to/replay-a-drive.md +++ b/docs/how-to/replay-a-drive.md @@ -8,7 +8,7 @@ Replaying is a critical tool for openpilot development and debugging. Just run `tools/replay/replay --demo`. ## Replaying CAN data -*Hardware required: jungle and comma 3/3X* +*Hardware required: jungle and comma 3X* 1. Connect your PC to a jungle. 2. diff --git a/docs/how-to/turn-the-speed-blue.md b/docs/how-to/turn-the-speed-blue.md index 13b3b03e80..eb6e75afa2 100644 --- a/docs/how-to/turn-the-speed-blue.md +++ b/docs/how-to/turn-the-speed-blue.md @@ -3,7 +3,7 @@ In 30 minutes, we'll get an openpilot development environment set up on your computer and make some changes to openpilot's UI. -And if you have a comma 3/3X, we'll deploy the change to your device for testing. +And if you have a comma 3X, we'll deploy the change to your device for testing. ## 1. Set up your development environment diff --git a/launch_env.sh b/launch_env.sh index 4c011c6ac0..67dd5ee795 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="12.8" + export AGNOS_VERSION="13.1" fi export STAGING_ROOT="/data/safe_staging" diff --git a/mkdocs.yml b/mkdocs.yml index a66d1c76d4..550f807aca 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -21,7 +21,7 @@ nav: - What is openpilot?: getting-started/what-is-openpilot.md - How-to: - Turn the speed blue: how-to/turn-the-speed-blue.md - - Connect to a comma 3/3X: how-to/connect-to-comma.md + - Connect to a comma 3X: how-to/connect-to-comma.md # - Make your first pull request: how-to/make-first-pr.md #- Replay a drive: how-to/replay-a-drive.md - Concepts: diff --git a/opendbc_repo b/opendbc_repo index 1a1fd1af7f..c8034c5640 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 1a1fd1af7f1dbdcaaef733adf4a5901f37cf4858 +Subproject commit c8034c5640e66c5ec02abfae7c4acad026f97ea5 diff --git a/panda b/panda index 7eab6fd61b..d5eaa73282 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 7eab6fd61bae085e0fd44cccb79dc6451163029e +Subproject commit d5eaa7328247d58a9fa47eb9eeff5c1d0a31c887 diff --git a/pyproject.toml b/pyproject.toml index 0968cd4ed3..2d4505c744 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -36,6 +36,9 @@ dependencies = [ "pyopenssl < 24.3.0", "pyaudio", + # ubloxd (TODO: just use struct) + "kaitaistruct", + # panda "libusb1", "spidev; platform_system == 'Linux'", @@ -101,8 +104,9 @@ dev = [ "av", "azure-identity", "azure-storage-blob", - "dbus-next", + "dbus-next", # TODO: remove once we moved everything to jeepney "dictdiffer", + "jeepney", "matplotlib", "opencv-python-headless", "parameterized >=0.8, <0.9", @@ -120,6 +124,7 @@ dev = [ tools = [ "metadrive-simulator @ https://github.com/commaai/metadrive/releases/download/MetaDrive-minimal-0.4.2.4/metadrive_simulator-0.4.2.4-py3-none-any.whl ; (platform_machine != 'aarch64')", + "dearpygui>=2.1.0", ] [project.urls] @@ -157,7 +162,6 @@ testpaths = [ "system/camerad", "system/hardware", "system/loggerd", - "system/proclogd", "system/tests", "system/ubloxd", "system/webrtc", @@ -173,7 +177,7 @@ quiet-level = 3 # if you've got a short variable name that's getting flagged, add it here ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead,TGE,abl,lite" builtin = "clear,rare,informal,code,names,en-GB_to_en-US" -skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*" +skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*, tools/plotjuggler/layouts/*" [tool.mypy] python_version = "3.11" @@ -247,6 +251,7 @@ exclude = [ "teleoprtc_repo", "third_party", "*.ipynb", + "generated", ] lint.flake8-implicit-str-concat.allow-multiline = false diff --git a/release/ci/squash_and_merge_prs.py b/release/ci/squash_and_merge_prs.py index 0f20f4f900..24922288be 100755 --- a/release/ci/squash_and_merge_prs.py +++ b/release/ci/squash_and_merge_prs.py @@ -14,7 +14,7 @@ def setup_argument_parser(): parser.add_argument('--pr-data', type=str, help='PR data in JSON format') parser.add_argument('--source-branch', type=str, default='master', help='Source branch for merging') - parser.add_argument('--target-branch', type=str, default='master-dev-c3-new-test', + parser.add_argument('--target-branch', type=str, default='master-dev-test', help='Target branch for merging') parser.add_argument('--squash-script-path', type=str, required=True, help='Path to the squash_and_merge.py script') diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 820c68e895..4c44e43281 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -71,7 +71,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) self.can_rcv_cum_timeout_counter = 0 @@ -124,7 +124,7 @@ class Car: self.CP.alternativeExperience = 0 # mads - set_alternative_experience(self.CP, self.params) + set_alternative_experience(self.CP, self.CP_SP, self.params) set_car_specific_params(self.CP, self.CP_SP, self.params) # Dynamic Experimental Control @@ -179,7 +179,7 @@ class Car: self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes) self.mock_carstate = MockCarState() - self.v_cruise_helper = VCruiseHelper(self.CP) + self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") @@ -216,6 +216,7 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP']) self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 5ffb43f9e5..9973862b85 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -30,8 +30,8 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper(VCruiseHelperSP): - def __init__(self, CP): - VCruiseHelperSP.__init__(self) + def __init__(self, CP, CP_SP): + VCruiseHelperSP.__init__(self, CP, CP_SP) self.CP = CP self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -46,10 +46,14 @@ class VCruiseHelper(VCruiseHelperSP): def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph + self.get_minimum_set_speed(is_metric) + if CS.cruiseState.available: - if not self.CP.pcmCruise: + _enabled = self.update_enabled_state(CS, enabled) + if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric) + self._update_v_cruise_non_pcm(CS, _enabled, is_metric) + self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -101,6 +105,12 @@ class VCruiseHelper(VCruiseHelperSP): if not self.button_change_states[button_type]["enabled"]: return + # Speed Limit Assist for Non PCM long cars. + # True: Disallow set speed changes when user confirmed the target set speed during preActive state + # False: Allow set speed changes as SLA is not requesting user confirmation + if self.update_speed_limit_assist_pre_active_confirmed(button_type): + return + long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta) if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta @@ -111,7 +121,7 @@ class VCruiseHelper(VCruiseHelperSP): if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) - self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX) + self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX) def update_button_timers(self, CS, enabled): # increment timer for buttons still pressed diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 275c84479f..384152c71b 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -57,8 +57,11 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {}))) - struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])] + # struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])] struct_dataclass.leadOne = structs.LeadData(**remove_deprecated(struct_dict.get('leadOne', {}))) struct_dataclass.leadTwo = structs.LeadData(**remove_deprecated(struct_dict.get('leadTwo', {}))) + struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement( + **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {})) + ) return struct_dataclass diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 7d136b824c..821767ee46 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -1,15 +1,12 @@ import os -import math import hypothesis.strategies as st from hypothesis import Phase, given, settings from parameterized import parameterized from cereal import car, custom from opendbc.car import DT_CTRL -from opendbc.car.car_helpers import interfaces from opendbc.car.structs import CarParams -from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface_args -from opendbc.car.fw_versions import FW_VERSIONS, FW_QUERY_CONFIGS +from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface from opendbc.car.mock.values import CAR as MOCK from opendbc.car.values import PLATFORMS from openpilot.selfdrive.car.helpers import convert_carControlSP @@ -21,11 +18,6 @@ from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces -ALL_ECUS = {ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()} -ALL_ECUS |= {ecu for config in FW_QUERY_CONFIGS.values() for ecu in config.extra_ecus} - -ALL_REQUESTS = {tuple(r.request) for config in FW_QUERY_CONFIGS.values() for r in config.requests} - MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '60')) @@ -37,43 +29,10 @@ class TestCarInterfaces: phases=(Phase.reuse, Phase.generate, Phase.shrink)) @given(data=st.data()) def test_car_interfaces(self, car_name, data): - CarInterface = interfaces[car_name] - - args = get_fuzzy_car_interface_args(data.draw) - - car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], - alpha_long=args['alpha_long'], is_release=False, docs=False) - car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'], - alpha_long=args['alpha_long'], docs=False) - car_params = car_params.as_reader() - car_interface = CarInterface(car_params, car_params_sp) + car_interface = get_fuzzy_car_interface(car_name, data.draw) + car_params = car_interface.CP.as_reader() + car_params_sp = car_interface.CP_SP sunnypilot_interfaces.setup_interfaces(car_interface) - assert car_params - assert car_params_sp - assert car_interface - - assert car_params.mass > 1 - assert car_params.wheelbase > 0 - # centerToFront is center of gravity to front wheels, assert a reasonable range - assert car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7 - assert car_params.maxLateralAccel > 0 - - # Longitudinal sanity checks - assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP) - assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP) - - # Lateral sanity checks - if car_params.steerControlType != CarParams.SteerControlType.angle: - tune = car_params.lateralTuning - if tune.which() == 'pid': - if car_name != MOCK.MOCK: - assert not math.isnan(tune.pid.kf) and tune.pid.kf > 0 - assert len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP) - assert len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP) - - elif tune.which() == 'torque': - assert not math.isnan(tune.torque.kf) and tune.torque.kf > 0 - assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0 cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) cc_sp_msg = FuzzyGenerator.get_random_msg(data.draw, custom.CarControlSP, real_floats=True) @@ -101,7 +60,7 @@ class TestCarInterfaces: # Test controller initialization # TODO: wait until card refactor is merged to run controller a few times, # hypothesis also slows down significantly with just one more message draw - LongControl(car_params) + LongControl(car_params, car_params_sp) if car_params.steerControlType == CarParams.SteerControlType.angle: LatControlAngle(car_params, car_params_sp, car_interface) elif car_params.lateralTuning.which() == 'pid': diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 4f9444d4bb..61bae68931 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -5,7 +5,7 @@ import numpy as np from parameterized import parameterized_class from cereal import log from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT -from cereal import car +from cereal import car, custom from openpilot.common.constants import CV from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -44,12 +44,13 @@ class TestCruiseSpeed: assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' -# TODO: test pcmCruise -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestVCruiseHelper: def setup_method(self): self.CP = car.CarParams(pcmCruise=self.pcm_cruise) - self.v_cruise_helper = VCruiseHelper(self.CP) + self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed) + self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.reset_cruise_speed_state() def reset_cruise_speed_state(self): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9e88a37690..3601ae310e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -58,7 +58,7 @@ class Controls(ControlsExt, ModelStateBase): self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None - self.LoC = LongControl(self.CP) + self.LoC = LongControl(self.CP, self.CP_SP) self.VM = VehicleModel(self.CP) self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: @@ -116,7 +116,8 @@ class Controls(ControlsExt, ModelStateBase): CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.CP.steerAtStandstill) - CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl + CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and \ + (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed) actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -168,7 +169,7 @@ class Controls(ControlsExt, ModelStateBase): CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() - CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl + CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed) CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 93088182f9..e72d464d06 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -51,6 +51,10 @@ class DesireHelper: self.lane_turn_controller = LaneTurnController(self) self.lane_turn_direction = custom.TurnDirection.none + @staticmethod + def get_lane_change_direction(CS): + return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right + def update(self, carstate, lateral_active, lane_change_prob): self.alc.update_params() self.lane_turn_controller.update_params() @@ -71,12 +75,13 @@ class DesireHelper: if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 + # Initialize lane change direction to prevent UI alert flicker + self.lane_change_direction = self.get_lane_change_direction(carstate) # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: - # Set lane change direction - self.lane_change_direction = LaneChangeDirection.left if \ - carstate.leftBlinker else LaneChangeDirection.right + # Update lane change direction + self.lane_change_direction = self.get_lane_change_direction(carstate) torque_applied = carstate.steeringPressed and \ ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e4554ae464..917fb233f4 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -48,6 +48,10 @@ class LatControlTorque(LatControl): self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): + # Override torque params from extension + if self.extension.update_override_torque_params(self.torque_params): + self.update_limits() + pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -56,10 +60,8 @@ class LatControlTorque(LatControl): actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 @@ -71,6 +73,8 @@ class LatControlTorque(LatControl): # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(setpoint - measurement) ff = gravity_adjusted_lateral_accel + # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll + ff -= self.torque_params.latAccelOffset ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6d4f922461..b8b00e2fb9 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -10,8 +10,11 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] LongCtrlState = car.CarControl.Actuators.LongControlState -def long_control_state_trans(CP, active, long_control_state, v_ego, +def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego, should_stop, brake_pressed, cruise_standstill): + # Gas Interceptor + cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor + stopping_condition = should_stop starting_condition = (not should_stop and not cruise_standstill and @@ -45,8 +48,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, return long_control_state class LongControl: - def __init__(self, CP): + def __init__(self, CP, CP_SP): self.CP = CP + self.CP_SP = CP_SP self.long_control_state = LongCtrlState.off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), @@ -61,7 +65,7 @@ class LongControl: self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, + self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, CS.cruiseState.standstill) if self.long_control_state == LongCtrlState.off: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 96248b7132..53eca38356 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -146,6 +146,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP): clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) + # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist + v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise) + if force_slow_decel: v_cruise = 0.0 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 60036c3082..4565a9c047 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 from cereal import car +from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog @@ -16,14 +17,18 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("plannerd got CarParams: %s", CP.brand) + gps_location_service = get_gps_location_service(params) + ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], - poll='modelV2') + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', + 'liveMapDataSP', 'carStateSP', gps_location_service], + poll='carState') while True: sm.update() + longitudinal_planner.sla.update_car_state(sm['carState']) if sm.updated['modelV2']: longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index ab50810d89..cf0ab24e0b 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans @@ -8,49 +8,52 @@ class TestLongControlStateTransition: def test_stay_stopped(self): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid active = False - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.off def test_engage(): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.off - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid def test_starting(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid diff --git a/selfdrive/controls/tests/test_torqued_lat_accel_offset.py b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py new file mode 100644 index 0000000000..84389856b6 --- /dev/null +++ b/selfdrive/controls/tests/test_torqued_lat_accel_offset.py @@ -0,0 +1,70 @@ +import numpy as np +from cereal import car, messaging +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY +from opendbc.car import structs +from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS + +np.random.seed(0) + +LA_ERR_STD = 1.0 +INPUT_NOISE_STD = 0.08 +V_EGO = 30.0 + +WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int) +STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03) + +ROLL_BIAS_DEG = 2.0 +ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG))) +TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2) +TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2) + +def generate_inputs(torque_tune, la_err_std, input_noise_std=None): + rng = np.random.default_rng(0) + steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)]) + la_errs = rng.normal(scale=la_err_std, size=steer_torques.size) + frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs]) + lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions + if input_noise_std is not None: + steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size) + lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size) + return steer_torques, lat_accels + +def get_warmed_up_estimator(steer_torques, lat_accels): + est = TorqueEstimator(car.CarParams()) + for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True): + est.filtered_points.add_point(steer_torque, lat_accel) + return est + +def simulate_straight_road_msgs(est): + carControl = messaging.new_message('carControl').carControl + carOutput = messaging.new_message('carOutput').carOutput + carState = messaging.new_message('carState').carState + livePose = messaging.new_message('livePose').livePose + carControl.latActive = True + carState.vEgo = V_EGO + carState.steeringPressed = False + ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET) + steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET))) + lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques + for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True): + carOutput.actuatorsOutput.torque = float(-steer_torque) + livePose.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG)) + livePose.angularVelocityDevice.z = float(lat_accel / V_EGO) + for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)): + est.handle_log(t, which, msg) + +def test_estimated_offset(): + steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) + est = get_warmed_up_estimator(steer_torques, lat_accels) + msg = est.get_msg() + # TODO add lataccelfactor and friction check when we have more accurate estimates + assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1 + +def test_straight_road_roll_bias(): + steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD) + est = get_warmed_up_estimator(steer_torques, lat_accels) + simulate_straight_road_msgs(est) + msg = est.get_msg() + assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index e0409957f5..57d53fdee3 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -11,6 +11,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.selfdrive.locationd.torqued_ext import TorqueEstimatorExt HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -50,9 +51,10 @@ class TorqueBuckets(PointBuckets): break -class TorqueEstimator(ParameterEstimator): +class TorqueEstimator(ParameterEstimator, TorqueEstimatorExt): def __init__(self, CP, decimated=False, track_all_points=False): - super().__init__() + ParameterEstimator.__init__(self) + TorqueEstimatorExt.__init__(self, CP) self.CP = CP self.hist_len = int(HISTORY / DT_MDL) self.lag = 0.0 @@ -82,6 +84,8 @@ class TorqueEstimator(ParameterEstimator): self.calibrator = PoseCalibrator() + TorqueEstimatorExt.initialize_custom_params(self, decimated) + self.reset() initial_params = { @@ -260,6 +264,8 @@ def main(demo=False): t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) + TorqueEstimatorExt.update_use_params(estimator) + # 4Hz driven by livePose if sm.frame % 5 == 0: pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 5ca0a86bc8..ff7e1d8600 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -13,12 +13,9 @@ class ModelConstants: META_T_IDXS = [2., 4., 6., 8., 10.] # model inputs constants - MODEL_FREQ = 20 - HISTORY_FREQ = 5 - HISTORY_LEN_SECONDS = 5 - TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ - FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS - INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS + N_FRAMES = 2 + MODEL_RUN_FREQ = 20 + MODEL_CONTEXT_FREQ = 5 # "model_trained_fps" FEATURE_LEN = 512 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index a2b54b420e..7273745c7b 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -95,8 +96,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action modelV2.action = action - # times at X_IDXS of edges and lines aren't used - LINE_T_IDXS: list[float] = [] + # times at X_IDXS of edges and lines + LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) @@ -149,7 +150,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D meta.hardBrakePredicted = hard_brake_predicted.item() # confidence - if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: + if vipc_frame_id % (2*ModelConstants.MODEL_RUN_FREQ) == 0: # any disengage prob brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE] gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 5f9ccf5072..00b49afaba 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -80,6 +80,64 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof +class InputQueues: + def __init__ (self, model_fps, env_fps, n_frames_input): + assert env_fps % model_fps == 0 + assert env_fps >= model_fps + self.model_fps = model_fps + self.env_fps = env_fps + self.n_frames_input = n_frames_input + + self.dtypes = {} + self.shapes = {} + self.q = {} + + def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None: + self.dtypes.update(input_dtypes) + if self.env_fps == self.model_fps: + self.shapes.update(input_shapes) + else: + for k in input_shapes: + shape = list(input_shapes[k]) + if 'img' in k: + n_channels = shape[1] // self.n_frames_input + shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels + else: + shape[1] = (self.env_fps // self.model_fps) * shape[1] + self.shapes[k] = tuple(shape) + + def reset(self) -> None: + self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()} + + def enqueue(self, inputs:dict[str, np.ndarray]) -> None: + for k in inputs.keys(): + if inputs[k].dtype != self.dtypes[k]: + raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}') + input_shape = list(self.shapes[k]) + input_shape[1] = -1 + single_input = inputs[k].reshape(tuple(input_shape)) + sz = single_input.shape[1] + self.q[k][:,:-sz] = self.q[k][:,sz:] + self.q[k][:,-sz:] = single_input + + def get(self, *names) -> dict[str, np.ndarray]: + if self.env_fps == self.model_fps: + return {k: self.q[k] for k in names} + else: + out = {} + for k in names: + shape = self.shapes[k] + if 'img' in k: + n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1)) + out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1) + elif 'pulse' in k: + # any pulse within interval counts + out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2) + else: + idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1] + out[k] = self.q[k][:, idxs] + return out + class ModelState(ModelStateBase): frames: dict[str, DrivingModelFrame] inputs: dict[str, np.ndarray] @@ -102,19 +160,15 @@ class ModelState(ModelStateBase): self.policy_output_slices = policy_metadata['output_slices'] policy_output_size = policy_metadata['output_shapes']['outputs'][1] - self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names} + self.frames = {name: DrivingModelFrame(context, ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ) for name in self.vision_input_names} self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) - self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32) - self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP) - # policy inputs - self.numpy_inputs = { - 'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32), - 'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32), - 'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), - } + self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes} + self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES) + for k in ['desire_pulse', 'features_buffer']: + self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape}) + self.full_input_queues.reset() # img buffers are managed in openCL transform code self.vision_inputs: dict[str, Tensor] = {} @@ -136,15 +190,10 @@ class ModelState(ModelStateBase): def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge - inputs['desire'][0] = 0 - new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) - self.prev_desire[:] = inputs['desire'] + inputs['desire_pulse'][0] = 0 + new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) + self.prev_desire[:] = inputs['desire_pulse'] - self.full_desire[0,:-1] = self.full_desire[0,1:] - self.full_desire[0,-1] = new_desire - self.numpy_inputs['desire'][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2) - - self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names} if TICI and not USBGPU: @@ -163,9 +212,10 @@ class ModelState(ModelStateBase): self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) - self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] - self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :] - self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] + self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire}) + for k in ['desire_pulse', 'features_buffer']: + self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k] + self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) @@ -223,7 +273,7 @@ def main(demo=False): params = Params() # setup filter to track dropped frames - frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) + frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ) frame_id = 0 last_vipc_frame_id = 0 run_count = 0 @@ -320,7 +370,7 @@ def main(demo=False): bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names} transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names} inputs:dict[str, np.ndarray] = { - 'desire': vec_desire, + 'desire_pulse': vec_desire, 'traffic_convention': traffic_convention, } diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 867a0d3b9b..7b87846748 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:04b763fb71efe57a8a4c4168a8043ecd58939015026ded0dc755ded6905ac251 -size 12343523 +oid sha256:ebb38a934d6472c061cc6010f46d9720ca132d631a47e585a893bdd41ade2419 +size 12343535 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index ce0dc927e7..4b4fa05df8 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e66bb8d53eced3786ed71a59b55ffc6810944cb217f0518621cc76303260a1ef +oid sha256:befac016a247b7ad5dc5b55d339d127774ed7bd2b848f1583f72aa4caee37781 size 46271991 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 26f42add6c..bcb39391c4 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -84,12 +84,10 @@ Panda *connect(std::string serial="", uint32_t index=0) { panda->set_can_fd_auto(i, true); } - bool is_deprecated_panda = std::find(DEPRECATED_PANDA_TYPES.begin(), - DEPRECATED_PANDA_TYPES.end(), - panda->hw_type) != DEPRECATED_PANDA_TYPES.end(); + bool is_supported_panda = std::find(SUPPORTED_PANDA_TYPES.begin(), SUPPORTED_PANDA_TYPES.end(), panda->hw_type) != SUPPORTED_PANDA_TYPES.end(); - if (is_deprecated_panda) { - LOGW("panda %s is deprecated (hw_type: %i), skipping firmware check...", panda->hw_serial().c_str(), static_cast(panda->hw_type)); + if (!is_supported_panda) { + LOGW("panda %s is not supported (hw_type: %i), skipping firmware check...", panda->hw_serial().c_str(), static_cast(panda->hw_type)); return panda.release(); } @@ -175,7 +173,6 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); ps.setInterruptLoad(health.interrupt_load_pkt); ps.setFanPower(health.fan_power); - ps.setFanStallCount(health.fan_stall_count); ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt)); ps.setSpiErrorCount(health.spi_error_count_pkt); ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h index cdacdc894b..954851b613 100644 --- a/selfdrive/pandad/pandad.h +++ b/selfdrive/pandad/pandad.h @@ -9,13 +9,10 @@ void pandad_main_thread(std::vector serials); // deprecated devices -static const std::vector DEPRECATED_PANDA_TYPES = { - cereal::PandaState::PandaType::WHITE_PANDA, - cereal::PandaState::PandaType::GREY_PANDA, - cereal::PandaState::PandaType::BLACK_PANDA, - cereal::PandaState::PandaType::PEDAL, - cereal::PandaState::PandaType::UNO, - cereal::PandaState::PandaType::RED_PANDA_V2 +static const std::vector SUPPORTED_PANDA_TYPES = { + cereal::PandaState::PandaType::RED_PANDA, + cereal::PandaState::PandaType::TRES, + cereal::PandaState::PandaType::CUATRO, }; diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index 265f673628..f4064ddcd4 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -29,6 +29,12 @@ def flash_panda(panda_serial: str) -> Panda: HARDWARE.recover_internal_panda() raise + # skip flashing if the detected panda is not supported + supported_panda = check_panda_support(panda) + if not supported_panda: + cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...") + return panda + fw_signature = get_expected_signature(panda) internal_panda = panda.is_internal() @@ -36,12 +42,6 @@ def flash_panda(panda_serial: str) -> Panda: panda_signature = b"" if panda.bootstub else panda.get_signature() cloudlog.warning(f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}") - # skip flashing if the detected device is deprecated from upstream - hw_type = panda.get_type() - if hw_type in Panda.DEPRECATED_DEVICES: - cloudlog.warning(f"Panda {panda_serial} is deprecated (hw_type: {hw_type}), skipping flash...") - return panda - if panda.bootstub or panda_signature != fw_signature: cloudlog.info("Panda firmware out of date, update required") panda.flash() @@ -67,6 +67,14 @@ def flash_panda(panda_serial: str) -> Panda: return panda +def check_panda_support(panda) -> bool: + hw_type = panda.get_type() + if hw_type in Panda.SUPPORTED_DEVICES: + return True + + return False + + def main() -> None: # signal pandad to close the relay and exit def signal_handler(signum, frame): @@ -91,11 +99,6 @@ def main() -> None: cloudlog.event("pandad.flash_and_connect", count=count) params.remove("PandaSignatures") - # TODO: remove this in the next AGNOS - # wait until USB is up before counting - if time.monotonic() < 60.: - no_internal_panda_count = 0 - # Handle missing internal panda if no_internal_panda_count > 0: if no_internal_panda_count == 3: @@ -145,6 +148,12 @@ def main() -> None: params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) for panda in pandas: + # skip health check if the detected panda is not supported + supported_panda = check_panda_support(panda) + if not supported_panda: + cloudlog.warning(f"Panda {panda.get_usb_serial()} is not supported (hw_type: {panda.get_type()}), skipping health check...") + continue + # check health for lost heartbeat health = panda.health() if health["heartbeat_lost"]: diff --git a/selfdrive/pandad/tests/bootstub.panda.bin b/selfdrive/pandad/tests/bootstub.panda.bin deleted file mode 100755 index 43db537061..0000000000 Binary files a/selfdrive/pandad/tests/bootstub.panda.bin and /dev/null differ diff --git a/selfdrive/pandad/tests/test_pandad.py b/selfdrive/pandad/tests/test_pandad.py index 87f30e4793..6a7359fd85 100644 --- a/selfdrive/pandad/tests/test_pandad.py +++ b/selfdrive/pandad/tests/test_pandad.py @@ -22,8 +22,6 @@ class TestPandad: if len(Panda.list()) == 0: self._run_test(60) - self.spi = HARDWARE.get_device_type() != 'tici' - def teardown_method(self): managed_processes['pandad'].stop() @@ -106,11 +104,9 @@ class TestPandad: # - 0.2s pandad -> pandad # - plus some buffer print("startup times", ts, sum(ts) / len(ts)) - assert 0.1 < (sum(ts)/len(ts)) < (0.7 if self.spi else 5.0) + assert 0.1 < (sum(ts)/len(ts)) < 0.7 def test_protocol_version_check(self): - if not self.spi: - pytest.skip("SPI test") # flash old fw fn = os.path.join(HERE, "bootstub.panda_h7_spiv0.bin") self._flash_bootstub_and_test(fn, expect_mismatch=True) diff --git a/selfdrive/pandad/tests/test_pandad_spi.py b/selfdrive/pandad/tests/test_pandad_spi.py index 9f7cc3b029..da4b181993 100644 --- a/selfdrive/pandad/tests/test_pandad_spi.py +++ b/selfdrive/pandad/tests/test_pandad_spi.py @@ -6,7 +6,6 @@ import random import cereal.messaging as messaging from cereal.services import SERVICE_LIST -from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.test.helpers import with_processes from openpilot.selfdrive.pandad.tests.test_pandad_loopback import setup_pandad, send_random_can_messages @@ -16,8 +15,6 @@ JUNGLE_SPAM = "JUNGLE_SPAM" in os.environ class TestBoarddSpi: @classmethod def setup_class(cls): - if HARDWARE.get_device_type() == 'tici': - pytest.skip("only for spi pandas") os.environ['STARTED'] = '1' os.environ['SPI_ERR_PROB'] = '0.001' if not JUNGLE_SPAM: diff --git a/selfdrive/selfdrived/alerts_offroad.json b/selfdrive/selfdrived/alerts_offroad.json index 87a007211f..6bae034766 100644 --- a/selfdrive/selfdrived/alerts_offroad.json +++ b/selfdrive/selfdrived/alerts_offroad.json @@ -29,10 +29,6 @@ "text": "Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support.", "severity": 1 }, - "Offroad_StorageMissing": { - "text": "NVMe drive not mounted.", - "severity": 1 - }, "Offroad_CarUnrecognized": { "text": "sunnypilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.", "severity": 0 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 591e54918e..b03a953a65 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -83,7 +83,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", "", AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index c56c7fe65f..4bc75021f7 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -21,12 +21,12 @@ from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck from openpilot.selfdrive.selfdrived.state import StateMachine from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert -from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP REPLAY = "REPLAY" in os.environ @@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper): 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', - 'modelDataV2SP'] + \ + 'modelDataV2SP', 'longitudinalPlanSP'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -135,13 +135,7 @@ class SelfdriveD(CruiseHelper): self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) - # some comma three with NVMe experience NVMe dropouts mid-drive that - # cause loggerd to crash on write, so ignore it only on that platform - self.ignored_processes = set() - nvme_expected = os.path.exists('/dev/nvme0n1') or (not os.path.isfile("/persist/comma/living-in-the-moment")) - if HARDWARE.get_device_type() == 'tici' and nvme_expected: - self.ignored_processes = {'loggerd', } - self.ignored_processes.update({'mapd'}) + self.ignored_processes = {'mapd', } # Determine startup event is_remote = build_metadata.openpilot.comma_remote or build_metadata.openpilot.sunnypilot_remote @@ -163,8 +157,9 @@ class SelfdriveD(CruiseHelper): self.events_sp_prev = [] self.mads = ModularAssistiveDrivingSystem(self) + self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP) - self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP) CruiseHelper.__init__(self, self.CP) @@ -210,6 +205,7 @@ class SelfdriveD(CruiseHelper): if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) + self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events) # Add car events, ignore if CAN isn't valid if CS.canValid: @@ -449,6 +445,8 @@ class SelfdriveD(CruiseHelper): self.events.add(EventName.personalityChanged) self.experimental_mode_switched = False + self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric) + def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) CS = _car_state.carState if _car_state else self.CS_prev @@ -553,6 +551,11 @@ class SelfdriveD(CruiseHelper): mads.active = self.mads.active mads.available = self.mads.enabled_toggle + icbm = ss_sp.intelligentCruiseButtonManagement + icbm.state = self.icbm.state + icbm.sendButton = self.icbm.cruise_button + icbm.vTarget = self.icbm.v_target + self.pm.send('selfdriveStateSP', ss_sp_msg) # onroadEventsSP - logged every second or on change diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index b8c6adb436..e466472432 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -67,6 +67,9 @@ class Plant: lp = messaging.new_message('liveParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') + car_state_sp = messaging.new_message('carStateSP') + live_map_data_sp = messaging.new_message('liveMapDataSP') + gps_data = messaging.new_message('gpsLocation') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead @@ -133,7 +136,10 @@ class Plant: 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, 'liveParameters': lp.liveParameters, - 'modelV2': model.modelV2} + 'modelV2': model.modelV2, + 'carStateSP': car_state_sp.carStateSP, + 'liveMapDataSP': live_map_data_sp.liveMapDataSP, + 'gpsLocation': gps_data.gpsLocation} self.planner.update(sm) self.acceleration = self.planner.output_a_target self.speed = self.speed + self.acceleration * self.ts diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 33b363cfd9..74507197b6 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -28,7 +28,8 @@ MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps] # 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr # 4. it must return a list of operations to be applied to the logreader (replace, add, delete) # 5. all migration functions must be independent of each other -def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False): +def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False, + live_location_kalman: bool = True): migrations = [ migrate_sensorEvents, migrate_carParams, @@ -37,7 +38,6 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo migrate_carOutput, migrate_controlsState, migrate_carState, - migrate_liveLocationKalman, migrate_liveTracks, migrate_driverAssistance, migrate_drivingModelData, @@ -51,6 +51,8 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo migrations.extend([migrate_pandaStates, migrate_peripheralState]) if camera_states: migrations.append(migrate_cameraStates) + if live_location_kalman: + migrations.append(migrate_liveLocationKalman) return migrate(lr, migrations) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index b1190c25e9..9b6371dbb9 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -496,7 +496,7 @@ CONFIGS = [ pubs=[ "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState" ], - subs=["livePose"], + subs=["liveLocationKalman", "livePose"], ignore=["logMonoTime"], should_recv_callback=MessageBasedRcvCallback("cameraOdometry"), tolerance=NUMPY_TOLERANCE, @@ -595,7 +595,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> if len(live_calibration) > 0: custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() if len(live_parameters) > 0: - custom_params["LiveParameters"] = live_parameters[msg_index].as_builder().to_bytes() + custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes() if len(live_torque_parameters) > 0: custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a4297096c0..a833fadb94 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6d3219bca9f66a229b38a5382d301a92b0147edb \ No newline at end of file +afcab1abb62b9d5678342956cced4712f44e909e \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 0149653c84..b4b9b9dbbe 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -18,7 +18,6 @@ from openpilot.common.timeout import Timeout from openpilot.common.params import Params from openpilot.selfdrive.selfdrived.events import EVENTS, ET from openpilot.selfdrive.test.helpers import set_params_enabled, release_only -from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.hw import Paths from openpilot.tools.lib.logreader import LogReader from openpilot.tools.lib.log_time_series import msgs_to_time_series @@ -33,7 +32,7 @@ CPU usage budget TEST_DURATION = 25 LOG_OFFSET = 8 -MAX_TOTAL_CPU = 280. # total for all 8 cores +MAX_TOTAL_CPU = 300. # total for all 8 cores PROCS = { # Baseline CPU usage by process "selfdrive.controls.controlsd": 16.0, @@ -57,30 +56,20 @@ PROCS = { "selfdrive.ui.soundd": 3.0, "selfdrive.ui.feedback.feedbackd": 1.0, "selfdrive.monitoring.dmonitoringd": 4.0, - "./proclogd": 2.0, + "system.proclogd": 3.0, "system.logmessaged": 1.0, "system.tombstoned": 0, - "./logcatd": 1.0, + "system.journald": 1.0, "system.micd": 5.0, "system.timed": 0, "selfdrive.pandad.pandad": 0, "system.statsd": 1.0, "system.loggerd.uploader": 15.0, "system.loggerd.deleter": 1.0, + "./pandad": 19.0, + "system.qcomgpsd.qcomgpsd": 1.0, } -PROCS.update({ - "tici": { - "./pandad": 5.0, - "./ubloxd": 1.0, - "system.ubloxd.pigeond": 6.0, - }, - "tizi": { - "./pandad": 19.0, - "system.qcomgpsd.qcomgpsd": 1.0, - } -}.get(HARDWARE.get_device_type(), {})) - TIMINGS = { # rtols: max/min, rsd "can": [2.5, 0.35], diff --git a/selfdrive/ui/layouts/main.py b/selfdrive/ui/layouts/main.py index 3ab8525d33..777d2f4c3f 100644 --- a/selfdrive/ui/layouts/main.py +++ b/selfdrive/ui/layouts/main.py @@ -63,14 +63,20 @@ class MainLayout(Widget): # Don't hide sidebar from interactive timeout if self._current_mode != MainState.ONROAD: self._sidebar.set_visible(False) - self._current_mode = MainState.ONROAD + self._set_current_layout(MainState.ONROAD) else: - self._current_mode = MainState.HOME + self._set_current_layout(MainState.HOME) self._sidebar.set_visible(True) + def _set_current_layout(self, layout: MainState): + if layout != self._current_mode: + self._layouts[self._current_mode].hide_event() + self._current_mode = layout + self._layouts[self._current_mode].show_event() + def open_settings(self, panel_type: PanelType): self._layouts[MainState.SETTINGS].set_current_panel(panel_type) - self._current_mode = MainState.SETTINGS + self._set_current_layout(MainState.SETTINGS) self._sidebar.set_visible(False) def _on_settings_clicked(self): diff --git a/selfdrive/ui/layouts/network.py b/selfdrive/ui/layouts/network.py deleted file mode 100644 index 856be26e97..0000000000 --- a/selfdrive/ui/layouts/network.py +++ /dev/null @@ -1,17 +0,0 @@ -import pyray as rl -from openpilot.system.ui.lib.wifi_manager import WifiManagerWrapper -from openpilot.system.ui.widgets import Widget -from openpilot.system.ui.widgets.network import WifiManagerUI - - -class NetworkLayout(Widget): - def __init__(self): - super().__init__() - self.wifi_manager = WifiManagerWrapper() - self.wifi_ui = WifiManagerUI(self.wifi_manager) - - def _render(self, rect: rl.Rectangle): - self.wifi_ui.render(rect) - - def shutdown(self): - self.wifi_manager.shutdown() diff --git a/selfdrive/ui/layouts/settings/firehose.py b/selfdrive/ui/layouts/settings/firehose.py index 74a8f317d5..b3db1fa5f0 100644 --- a/selfdrive/ui/layouts/settings/firehose.py +++ b/selfdrive/ui/layouts/settings/firehose.py @@ -2,7 +2,7 @@ import pyray as rl import time import threading -from openpilot.common.api import Api, api_get +from openpilot.common.api import api_get from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.ui.ui_state import ui_state @@ -11,7 +11,7 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel from openpilot.system.ui.lib.wrap_text import wrap_text from openpilot.system.ui.widgets import Widget - +from openpilot.selfdrive.ui.lib.api_helpers import get_token TITLE = "Firehose Mode" DESCRIPTION = ( @@ -163,7 +163,7 @@ class FirehoseLayout(Widget): dongle_id = self.params.get("DongleId") if not dongle_id or dongle_id == UNREGISTERED_DONGLE_ID: return - identity_token = Api(dongle_id).get_token() + identity_token = get_token(dongle_id) response = api_get(f"v1/devices/{dongle_id}/firehose_stats", access_token=identity_token) if response.status_code == 200: data = response.json() diff --git a/selfdrive/ui/layouts/settings/settings.py b/selfdrive/ui/layouts/settings/settings.py index 674e5005f4..a731a9158c 100644 --- a/selfdrive/ui/layouts/settings/settings.py +++ b/selfdrive/ui/layouts/settings/settings.py @@ -2,7 +2,6 @@ import pyray as rl from dataclasses import dataclass from enum import IntEnum from collections.abc import Callable -from openpilot.selfdrive.ui.layouts.network import NetworkLayout from openpilot.selfdrive.ui.layouts.settings.developer import DeveloperLayout from openpilot.selfdrive.ui.layouts.settings.device import DeviceLayout from openpilot.selfdrive.ui.layouts.settings.firehose import FirehoseLayout @@ -10,7 +9,9 @@ from openpilot.selfdrive.ui.layouts.settings.software import SoftwareLayout from openpilot.selfdrive.ui.layouts.settings.toggles import TogglesLayout from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos from openpilot.system.ui.lib.text_measure import measure_text_cached +from openpilot.system.ui.lib.wifi_manager import WifiManager from openpilot.system.ui.widgets import Widget +from openpilot.system.ui.widgets.network import WifiManagerUI # Settings close button SETTINGS_CLOSE_TEXT = "×" @@ -43,7 +44,7 @@ class PanelType(IntEnum): @dataclass class PanelInfo: name: str - instance: object + instance: Widget button_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) @@ -53,9 +54,12 @@ class SettingsLayout(Widget): self._current_panel = PanelType.DEVICE # Panel configuration + wifi_manager = WifiManager() + wifi_manager.set_active(False) + self._panels = { PanelType.DEVICE: PanelInfo("Device", DeviceLayout()), - PanelType.NETWORK: PanelInfo("Network", NetworkLayout()), + PanelType.NETWORK: PanelInfo("Network", WifiManagerUI(wifi_manager)), PanelType.TOGGLES: PanelInfo("Toggles", TogglesLayout()), PanelType.SOFTWARE: PanelInfo("Software", SoftwareLayout()), PanelType.FIREHOSE: PanelInfo("Firehose", FirehoseLayout()), @@ -149,8 +153,14 @@ class SettingsLayout(Widget): def set_current_panel(self, panel_type: PanelType): if panel_type != self._current_panel: + self._panels[self._current_panel].instance.hide_event() self._current_panel = panel_type + self._panels[self._current_panel].instance.show_event() - def close_settings(self): - if self._close_callback: - self._close_callback() + def show_event(self): + super().show_event() + self._panels[self._current_panel].instance.show_event() + + def hide_event(self): + super().hide_event() + self._panels[self._current_panel].instance.hide_event() diff --git a/selfdrive/ui/lib/api_helpers.py b/selfdrive/ui/lib/api_helpers.py new file mode 100644 index 0000000000..b83efedb60 --- /dev/null +++ b/selfdrive/ui/lib/api_helpers.py @@ -0,0 +1,14 @@ +import time +from functools import lru_cache +from openpilot.common.api import Api + +TOKEN_EXPIRY_HOURS = 2 + + +@lru_cache(maxsize=1) +def _get_token(dongle_id: str, t: int): + return Api(dongle_id).get_token(expiry_hours=TOKEN_EXPIRY_HOURS) + + +def get_token(dongle_id: str): + return _get_token(dongle_id, int(time.monotonic() / (TOKEN_EXPIRY_HOURS / 2 * 60 * 60))) diff --git a/selfdrive/ui/lib/prime_state.py b/selfdrive/ui/lib/prime_state.py index da2ff899dd..be2132c1b7 100644 --- a/selfdrive/ui/lib/prime_state.py +++ b/selfdrive/ui/lib/prime_state.py @@ -2,14 +2,12 @@ from enum import IntEnum import os import threading import time -from functools import lru_cache -from openpilot.common.api import Api, api_get +from openpilot.common.api import api_get from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID - -TOKEN_EXPIRY_HOURS = 2 +from openpilot.selfdrive.ui.lib.api_helpers import get_token class PrimeType(IntEnum): @@ -23,12 +21,6 @@ class PrimeType(IntEnum): PURPLE = 5, -@lru_cache(maxsize=1) -def get_token(dongle_id: str, t: int): - print('getting token') - return Api(dongle_id).get_token(expiry_hours=TOKEN_EXPIRY_HOURS) - - class PrimeState: FETCH_INTERVAL = 5.0 # seconds between API calls API_TIMEOUT = 10.0 # seconds for API requests @@ -58,15 +50,13 @@ class PrimeState: return try: - identity_token = get_token(dongle_id, int(time.monotonic() / (TOKEN_EXPIRY_HOURS / 2 * 60 * 60))) + identity_token = get_token(dongle_id) response = api_get(f"v1.1/devices/{dongle_id}", timeout=self.API_TIMEOUT, access_token=identity_token) if response.status_code == 200: data = response.json() is_paired = data.get("is_paired", False) prime_type = data.get("prime_type", 0) self.set_type(PrimeType(prime_type) if is_paired else PrimeType.UNPAIRED) - elif response.status_code == 401: - get_token.cache_clear() except Exception as e: cloudlog.error(f"Failed to fetch prime status: {e}") diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index 4cfa3d0e3c..540643bed5 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -47,11 +47,12 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0)); p.fillRect(0, 0, surface_rect.width(), UI_HEADER_HEIGHT, bg); - +#ifndef SUNNYPILOT if (is_cruise_available) { drawSetSpeed(p, surface_rect); } drawCurrentSpeed(p, surface_rect); +#endif p.restore(); } diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h index 9b51bb62e4..ad0603c095 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.h +++ b/selfdrive/ui/qt/onroad/onroad_home.h @@ -4,8 +4,10 @@ #ifdef SUNNYPILOT #include "selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h" +#include "selfdrive/ui/sunnypilot/qt/onroad/alerts.h" #define UIState UIStateSP #define AnnotatedCameraWidget AnnotatedCameraWidgetSP +#define OnroadAlerts OnroadAlertsSP #else #include "selfdrive/ui/qt/onroad/annotated_camera.h" #endif diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 807bf02478..be92c14261 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -24,6 +24,7 @@ qt_src = [ "sunnypilot/qt/offroad/offroad_home.cc", "sunnypilot/qt/offroad/settings/developer_panel.cc", "sunnypilot/qt/offroad/settings/device_panel.cc", + "sunnypilot/qt/offroad/settings/display_panel.cc", "sunnypilot/qt/offroad/settings/lateral_panel.cc", "sunnypilot/qt/offroad/settings/longitudinal_panel.cc", "sunnypilot/qt/offroad/settings/max_time_offroad.cc", @@ -37,6 +38,7 @@ qt_src = [ "sunnypilot/qt/offroad/settings/trips_panel.cc", "sunnypilot/qt/offroad/settings/vehicle_panel.cc", "sunnypilot/qt/offroad/settings/visuals_panel.cc", + "sunnypilot/qt/onroad/alerts.cc", "sunnypilot/qt/onroad/annotated_camera.cc", "sunnypilot/qt/onroad/buttons.cc", "sunnypilot/qt/onroad/developer_ui/developer_ui.cc", @@ -50,10 +52,14 @@ lateral_panel_qt_src = [ "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc", "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc", "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc", + "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc", + "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc", ] longitudinal_panel_qt_src = [ "sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc", + "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc", + "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc", ] network_src = [ @@ -88,9 +94,13 @@ brand_settings_qt_src = [ "sunnypilot/qt/offroad/settings/vehicle/volkswagen_settings.cc", ] +display_panel_qt_src = [ + "sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc", +] sp_widgets_src = widgets_src + network_src -sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + longitudinal_panel_qt_src + osm_panel_qt_src +sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + \ + longitudinal_panel_qt_src + osm_panel_qt_src + display_panel_qt_src sp_qt_util = qt_util Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util") diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.cc index ffd3ce1d8b..e9bd13485a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.cc @@ -25,8 +25,8 @@ const QMap Brightness::brightness_options = { Brightness::Brightness() : OptionControlSP( "Brightness", - tr("Brightness"), - tr("Overrides the brightness of the device."), + tr("Global Brightness"), + tr("Overrides the brightness of the device. This applies to both onroad and offroad screens. "), "../assets/offroad/icon_blank.png", {0, 11}, 1, true, &brightness_options) { diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc index 7e3d18bd81..43e6067451 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc @@ -23,6 +23,7 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { {"regulatoryBtn", tr("Regulatory"), ""}, {"translateBtn", tr("Language"), ""}, {"resetParams", tr("Reset Settings"), ""}, + {"onroadUploadsBtn", tr("Onroad Uploads"), "OnroadUploads"} }; int row = 0, col = 0; @@ -75,12 +76,14 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { connect(buttons["resetParams"], &PushButtonSP::clicked, this, &DevicePanelSP::resetSettings); + connect(buttons["onroadUploadsBtn"], &PushButtonSP::clicked, buttons["onroadUploadsBtn"], &PushButtonSP::updateButton); + // Max Time Offroad maxTimeOffroad = new MaxTimeOffroad(); connect(maxTimeOffroad, &OptionControlSP::updateLabels, maxTimeOffroad, &MaxTimeOffroad::refresh); addItem(maxTimeOffroad); - toggleDeviceBootMode = new ButtonParamControlSP("DeviceBootMode", tr("Wake-Up Behavior"), "", "", {"Default", "Offroad"}, 375, true); + toggleDeviceBootMode = new ButtonParamControlSP("DeviceBootMode", tr("Wake-Up Behavior"), "", "", {"Default", "Offroad"}, 375, true); addItem(toggleDeviceBootMode); connect(toggleDeviceBootMode, &ButtonParamControlSP::buttonClicked, this, [=](int index) { @@ -88,22 +91,6 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { updateState(); }); - interactivityTimeout = new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"), - tr("Apply a custom timeout for settings UI." - "\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."), - "", {0, 120}, 10, true, nullptr, false); - - connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() { - updateState(); - }); - - addItem(interactivityTimeout); - - // Brightness - brightness = new Brightness(); - connect(brightness, &OptionControlSP::updateLabels, brightness, &Brightness::refresh); - addItem(brightness); - addItem(device_grid_layout); // offroad mode and power buttons @@ -129,9 +116,8 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { offroadBtn->setFixedWidth(power_layout->sizeHint().width()); QObject::connect(offroadBtn, &PushButtonSP::clicked, this, &DevicePanelSP::setOffroadMode); - QVBoxLayout *power_group_layout = new QVBoxLayout(); + power_group_layout = new QVBoxLayout(); power_group_layout->setSpacing(25); - power_group_layout->addWidget(offroadBtn, 0, Qt::AlignHCenter); power_group_layout->addLayout(power_layout); addItem(power_group_layout); @@ -141,9 +127,10 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { poweroffBtn, offroadBtn, buttons["quietModeBtn"], + buttons["onroadUploadsBtn"], }; - QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + QObject::connect(uiState(), &UIState::offroadTransition, [=](bool _offroad) { for (auto btn : findChildren()) { bool always_enabled = std::find(always_enabled_btns.begin(), always_enabled_btns.end(), btn) != always_enabled_btns.end(); @@ -151,6 +138,8 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) { btn->setEnabled(offroad); } } + offroad = _offroad; + updateState(); }); } @@ -201,7 +190,7 @@ void DevicePanelSP::updateState() { } bool offroad_mode_param = params.getBool("OffroadMode"); - offroadBtn->setText(offroad_mode_param ? tr("Exit Always Offroad") : tr("Always Offroad")); + offroadBtn->setText(offroad_mode_param ? tr("Exit Always Offroad") : tr("Enable Always Offroad")); offroadBtn->setStyleSheet(offroad_mode_param ? alwaysOffroadStyle : autoOffroadStyle); DeviceSleepModeStatus currStatus = DeviceSleepModeStatus::DEFAULT; @@ -210,10 +199,9 @@ void DevicePanelSP::updateState() { } toggleDeviceBootMode->setDescription(deviceSleepModeDescription(currStatus)); - QString timeoutValue = QString::fromStdString(params.get("InteractivityTimeout")); - if (timeoutValue == "0" || timeoutValue.isEmpty()) { - interactivityTimeout->setLabel("Default"); + if (offroad and not offroad_mode_param) { + power_group_layout->insertWidget(0, offroadBtn, 0, Qt::AlignHCenter); } else { - interactivityTimeout->setLabel(timeoutValue + "s"); + AddWidgetAt(0, offroadBtn); } } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h index 899a942dae..ce4acfc7cf 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h @@ -8,7 +8,6 @@ #pragma once #include "selfdrive/ui/sunnypilot/qt/offroad/settings/max_time_offroad.h" -#include "selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" @@ -32,8 +31,8 @@ private: PushButtonSP *offroadBtn; MaxTimeOffroad *maxTimeOffroad; ButtonParamControlSP *toggleDeviceBootMode; - Brightness *brightness; - OptionControlSP *interactivityTimeout; + QVBoxLayout *power_group_layout; + bool offroad; const QString alwaysOffroadStyle = R"( PushButtonSP { diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc new file mode 100644 index 0000000000..f28d9e45e8 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc @@ -0,0 +1,54 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h" + +OnroadScreenBrightnessControl::OnroadScreenBrightnessControl(const QString ¶m, const QString &title, + const QString &description, const QString &icon, + QWidget *parent) + : ExpandableToggleRow(param, title, description, icon, parent) { + auto *mainFrame = new QFrame(this); + auto *mainFrameLayout = new QVBoxLayout(); + mainFrame->setLayout(mainFrameLayout); + mainFrameLayout->setSpacing(30); + mainFrameLayout->setContentsMargins(0, 0, 0, 0); + + onroadScreenOffTimer = new OptionControlSP( + "OnroadScreenOffTimer", + "Onroad Brightness Delay", + "", + "", + {0, 11}, 1, true, &onroadScreenOffTimerOptions); + + onroadScreenBrightness = new OptionControlSP( + "OnroadScreenOffBrightness", + "Onroad Brightness", + "", + "", + {0, 100}, 10, true); + + connect(onroadScreenOffTimer, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh); + connect(onroadScreenBrightness, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh); + mainFrameLayout->addWidget(onroadScreenBrightness); + mainFrameLayout->addWidget(onroadScreenOffTimer); + + addItem(mainFrame); + + refresh(); +} + +void OnroadScreenBrightnessControl::refresh() { + // Driving Screen Off Timer + int valTimer = std::atoi(params.get("OnroadScreenOffTimer").c_str()); + std::string labelTimer = (valTimer < 60 ? std::to_string(valTimer) + "s" : std::to_string(valTimer / 60) + "m"); + onroadScreenOffTimer->setLabel(QString::fromStdString(labelTimer)); + + // Driving Screen Off Brightness + std::string valBrightness = params.get("OnroadScreenOffBrightness"); + std::string labelBrightness = (valBrightness == "0" ? " Screen Off" : valBrightness + "%"); + onroadScreenBrightness->setLabel(QString::fromStdString(labelBrightness)); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h new file mode 100644 index 0000000000..02335d1ce6 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h @@ -0,0 +1,42 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h" + +static const QMap onroadScreenOffTimerOptions = { + {"0", "15"}, + {"1", "30"}, + {"2", "60"}, + {"3", "120"}, + {"4", "180"}, + {"5", "240"}, + {"6", "300"}, + {"7", "360"}, + {"8", "420"}, + {"9", "480"}, + {"10", "540"}, + {"11", "600"} +}; + +class OnroadScreenBrightnessControl : public ExpandableToggleRow { + Q_OBJECT + +public: + OnroadScreenBrightnessControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + QWidget *parent = nullptr); + void refresh(); + +private: + Params params; + OptionControlSP *onroadScreenOffTimer; + OptionControlSP *onroadScreenBrightness; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc new file mode 100644 index 0000000000..465047f178 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc @@ -0,0 +1,65 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h" + +DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) { + main_layout = new QStackedLayout(this); + ListWidgetSP *list = new ListWidgetSP(this, false); + + sunnypilotScreen = new QWidget(this); + QVBoxLayout* vlayout = new QVBoxLayout(sunnypilotScreen); + vlayout->setContentsMargins(50, 20, 50, 20); + + // Onroad Screen Off/Brightness + onroadScreenBrightnessControl = new OnroadScreenBrightnessControl( + "OnroadScreenOffControl", + tr("Onroad Screen: Reduced Brightness"), + tr("Turn off device screen or reduce brightness after driving starts. " + "It automatically brightens again when screen is touched or a visible alert is displayed."), + "", + this); + list->addItem(onroadScreenBrightnessControl); + list->addItem(horizontal_line()); + + // Global Brightness + brightness = new Brightness(); + connect(brightness, &OptionControlSP::updateLabels, brightness, &Brightness::refresh); + list->addItem(brightness); + list->addItem(horizontal_line()); + + // Interactivity Timeout + interactivityTimeout = new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"), + tr("Apply a custom timeout for settings UI." + "\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."), + "", {0, 120}, 10, true, nullptr, false); + + connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() { + refresh(); + }); + list->addItem(interactivityTimeout); + + sunnypilotScroller = new ScrollViewSP(list, this); + vlayout->addWidget(sunnypilotScroller); + main_layout->addWidget(sunnypilotScreen); +} + +void DisplayPanel::showEvent(QShowEvent *event) { + QWidget::showEvent(event); + refresh(); +} + +void DisplayPanel::refresh() { + onroadScreenBrightnessControl->refresh(); + + QString timeoutValue = QString::fromStdString(params.get("InteractivityTimeout")); + if (timeoutValue == "0" || timeoutValue.isEmpty()) { + interactivityTimeout->setLabel("Default"); + } else { + interactivityTimeout->setLabel(timeoutValue + "s"); + } +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h new file mode 100644 index 0000000000..69b440c9d3 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h @@ -0,0 +1,31 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/brightness.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" + +class DisplayPanel : public QWidget { + Q_OBJECT + +public: + explicit DisplayPanel(QWidget *parent = nullptr); + void showEvent(QShowEvent *event) override; + void refresh(); + +private: + QStackedLayout* main_layout = nullptr; + QWidget* sunnypilotScreen = nullptr; + ScrollViewSP *sunnypilotScroller = nullptr; + Params params; + OnroadScreenBrightnessControl *onroadScreenBrightnessControl = nullptr; + Brightness *brightness; + OptionControlSP *interactivityTimeout; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc index 72e7a5c8cb..749bc9e3f3 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -52,12 +52,16 @@ void MadsSettings::updateToggles(bool _offroad) { ); auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { params.remove("MadsMainCruiseAllowed"); params.putBool("MadsUnifiedEngagementMode", true); params.put("MadsSteeringMode", std::to_string(static_cast(MadsSteeringMode::DISENGAGE))); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h index af624865c7..9cf62a0ffe 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -12,7 +12,16 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" -const std::vector mads_limited_settings_brands = {"rivian", "tesla"}; +inline bool madsLimitedSettings(const cereal::CarParams::Reader &CP, const cereal::CarParamsSP::Reader &CP_SP) { + if (CP.getBrand() == "rivian") { + return true; + } + if (CP.getBrand() == "tesla") { + return !(CP_SP.getFlags() & 1); // 1 == TeslaFlagsSP.HAS_VEHICLE_BUS + } + + return false; +} enum class MadsSteeringMode { REMAIN_ACTIVE = 0, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc index 0b66b83660..02ab810c57 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc @@ -10,14 +10,15 @@ NeuralNetworkLateralControl::NeuralNetworkLateralControl() : ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"), "", "") { setConfirmation(true, false); - updateToggle(); + updateToggle(offroad); } -void NeuralNetworkLateralControl::updateToggle() { +void NeuralNetworkLateralControl::updateToggle(bool _offroad) { QString statusInitText = "" + STATUS_CHECK_COMPATIBILITY + ""; QString notLoadedText = "" + STATUS_NOT_LOADED + ""; QString loadedText = "" + STATUS_LOADED + ""; + bool allowed = true; auto cp_bytes = params.get("CarParamsPersistent"); auto cp_sp_bytes = params.get("CarParamsSPPersistent"); if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { @@ -31,7 +32,7 @@ void NeuralNetworkLateralControl::updateToggle() { if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) { params.remove("NeuralNetworkLateralControl"); setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE)); - setEnabled(false); + allowed = false; } else { QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName()); QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ? @@ -56,4 +57,11 @@ void NeuralNetworkLateralControl::updateToggle() { if (getDescription() != getBaseDescription()) { showDescription(); } + + bool enforce_torque_toggle = params.getBool("EnforceTorqueControl"); + setEnabled(_offroad && allowed && !enforce_torque_toggle); + + refresh(); + + offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h index df80fbf097..9e872b11e9 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h @@ -20,10 +20,11 @@ public: NeuralNetworkLateralControl(); public slots: - void updateToggle(); + void updateToggle(bool _offroad); private: Params params; + bool offroad; // Status messages const QString STATUS_NOT_AVAILABLE = tr("NNLC is currently not available on this platform."); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc new file mode 100644 index 0000000000..894fbc2ec4 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc @@ -0,0 +1,58 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h" + +TorqueLateralControlCustomParams::TorqueLateralControlCustomParams(const QString ¶m, const QString &title, const QString &description, const QString &icon, QWidget *parent) + : ExpandableToggleRow(param, title, description, icon, parent) { + + QFrame *frame = new QFrame(this); + QGridLayout *frame_layout = new QGridLayout(); + frame->setLayout(frame_layout); + frame_layout->setSpacing(0); + + torqueLateralControlParamsOverride = new ParamControl( + "TorqueParamsOverrideEnabled", + tr("Manual Real-Time Tuning"), + tr("Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values."), + "../assets/offroad/icon_blank.png", + this + ); + connect(torqueLateralControlParamsOverride, &ParamControl::toggleFlipped, this, &TorqueLateralControlCustomParams::refresh); + + torqueParamsOverrideLatAccelFactor = new OptionControlSP("TorqueParamsOverrideLatAccelFactor", tr("Lateral Acceleration Factor"), "", "", {1, 500}, 1, false, nullptr, true, false); + connect(torqueParamsOverrideLatAccelFactor, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh); + torqueParamsOverrideLatAccelFactor->setFixedWidth(280); + + torqueParamsOverrideFriction = new OptionControlSP("TorqueParamsOverrideFriction", tr("Friction"), "", "", {1, 100}, 1, false, nullptr, true, false); + connect(torqueParamsOverrideFriction, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh); + torqueParamsOverrideFriction->setFixedWidth(280); + + frame_layout->addWidget(torqueLateralControlParamsOverride, 0, 0, 1, 2); + QSpacerItem *spacer = new QSpacerItem(20, 40); + frame_layout->addItem(spacer, 1, 0, 1, 2); + frame_layout->addWidget(torqueParamsOverrideLatAccelFactor, 2, 0, Qt::AlignCenter); + frame_layout->addWidget(torqueParamsOverrideFriction, 2, 1, Qt::AlignCenter); + + addItem(frame); + + refresh(); +} + +void TorqueLateralControlCustomParams::refresh() { + bool torque_override_param = params.getBool("TorqueParamsOverrideEnabled"); + float laf_param = QString::fromStdString(params.get("TorqueParamsOverrideLatAccelFactor")).toFloat(); + const QString laf_unit = "m/s²"; + + float friction_param = QString::fromStdString(params.get("TorqueParamsOverrideFriction")).toFloat(); + + torqueParamsOverrideLatAccelFactor->setTitle(tr("Lateral Acceleration Factor") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")"); + torqueParamsOverrideFriction->setTitle(tr("Friction") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")"); + + torqueParamsOverrideLatAccelFactor->setLabel(QString::number(laf_param, 'f', 2) + " " + laf_unit); + torqueParamsOverrideFriction->setLabel(QString::number(friction_param, 'f', 2)); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h new file mode 100644 index 0000000000..8442bd0d48 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h @@ -0,0 +1,27 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h" + +class TorqueLateralControlCustomParams : public ExpandableToggleRow { + Q_OBJECT + +public: + TorqueLateralControlCustomParams(const QString ¶m, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr); + void refresh(); + +private: + Params params; + ParamControl *torqueLateralControlParamsOverride; + OptionControlSP *torqueParamsOverrideFriction; + OptionControlSP *torqueParamsOverrideLatAccelFactor; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc new file mode 100644 index 0000000000..914ce7a939 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc @@ -0,0 +1,81 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h" + +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" + +TorqueLateralControlSettings::TorqueLateralControlSettings(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(50, 20, 50, 20); + main_layout->setSpacing(20); + + // Back button + PanelBackButton *back = new PanelBackButton(); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + main_layout->addWidget(back, 0, Qt::AlignLeft); + + ListWidget *list = new ListWidget(this, false); + // param, title, desc, icon + std::vector> toggle_defs{ + { + "LiveTorqueParamsToggle", + tr("Self-Tune"), + tr("Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default."), + "../assets/offroad/icon_blank.png", + }, + { + "LiveTorqueParamsRelaxedToggle", + tr("Less Restrict Settings for Self-Tune (Beta)"), + tr("Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values."), + "../assets/offroad/icon_blank.png", + } + }; + + for (auto &[param, title, desc, icon] : toggle_defs) { + auto toggle = new ParamControlSP(param, title, desc, icon, this); + list->addItem(toggle); + toggles[param.toStdString()] = toggle; + } + + torqueLateralControlCustomParams = new TorqueLateralControlCustomParams( + "CustomTorqueParams", + tr("Enable Custom Tuning"), + tr("Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within \"opendbc/car/torque_data\". " + "The values will also be used live when \"Manual Real-Time Tuning\" toggle is enabled."), + "../assets/offroad/icon_blank.png", + this); + list->addItem(torqueLateralControlCustomParams); + + QObject::connect(uiState(), &UIState::offroadTransition, this, &TorqueLateralControlSettings::updateToggles); + QObject::connect(toggles["LiveTorqueParamsToggle"], &ParamControlSP::toggleFlipped, [=](bool state) { + if (!state) { + params.remove("LiveTorqueParamsRelaxedToggle"); + toggles["LiveTorqueParamsRelaxedToggle"]->refresh(); + } + + updateToggles(offroad); + }); + + main_layout->addWidget(new ScrollViewSP(list, this)); +} + +void TorqueLateralControlSettings::showEvent(QShowEvent *event) { + updateToggles(offroad); +} + +void TorqueLateralControlSettings::updateToggles(bool _offroad) { + bool live_toggle = toggles["LiveTorqueParamsToggle"]->isToggled(); + + toggles["LiveTorqueParamsToggle"]->setEnabled(_offroad); + toggles["LiveTorqueParamsRelaxedToggle"]->setEnabled(_offroad && live_toggle); + + torqueLateralControlCustomParams->setEnabled(_offroad); + torqueLateralControlCustomParams->refresh(); + + offroad = _offroad; +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h new file mode 100644 index 0000000000..6254073dfe --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h @@ -0,0 +1,36 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" + +class TorqueLateralControlSettings : public QWidget { + Q_OBJECT + +public: + explicit TorqueLateralControlSettings(QWidget *parent = nullptr); + + void showEvent(QShowEvent *event) override; + + signals: + void backPress(); + +public slots: + void updateToggles(bool _offroad); + +private: + Params params; + bool offroad; + std::map toggles; + + TorqueLateralControlCustomParams *torqueLateralControlCustomParams; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index ae954d7539..ac004e8856 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -75,6 +75,36 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { list->addItem(horizontal_line()); + // Customized Torque Lateral Control + torqueLateralControlToggle = new ParamControl( + "EnforceTorqueControl", + tr("Enforce Torque Lateral Control"), + tr("Enable this to enforce sunnypilot to steer with Torque lateral control."), + ""); + list->addItem(torqueLateralControlToggle); + + torqueLateralControlSettingsButton = new PushButtonSP(tr("Customize Params")); + torqueLateralControlSettingsButton->setObjectName("torque_btn"); + connect(torqueLateralControlSettingsButton, &QPushButton::clicked, [=]() { + sunnypilotScroller->setLastScrollPosition(); + main_layout->setCurrentWidget(torqueLateralControlWidget); + }); + QObject::connect(torqueLateralControlToggle, &ToggleControl::toggleFlipped, [=](bool state) { + torqueLateralControlSettingsButton->setEnabled(state); + nnlcToggle->updateToggle(offroad); + updateToggles(offroad); + }); + + torqueLateralControlWidget = new TorqueLateralControlSettings(this); + connect(torqueLateralControlWidget, &TorqueLateralControlSettings::backPress, [=]() { + sunnypilotScroller->restoreScrollPosition(); + main_layout->setCurrentWidget(sunnypilotScreen); + }); + list->addItem(torqueLateralControlSettingsButton); + + list->addItem(vertical_space(0)); + list->addItem(horizontal_line()); + // Neural Network Lateral Control nnlcToggle = new NeuralNetworkLateralControl(); list->addItem(nnlcToggle); @@ -86,12 +116,10 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { nnlcToggle->hideDescription(); } - nnlcToggle->updateToggle(); + nnlcToggle->updateToggle(offroad); + updateToggles(offroad); }); - toggleOffroadOnly = { - madsToggle, nnlcToggle, - }; QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles); sunnypilotScroller = new ScrollViewSP(list, this); @@ -100,6 +128,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { main_layout->addWidget(sunnypilotScreen); main_layout->addWidget(madsWidget); main_layout->addWidget(laneChangeWidget); + main_layout->addWidget(torqueLateralControlWidget); setStyleSheet(R"( #back_btn { @@ -120,7 +149,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { } void LateralPanel::showEvent(QShowEvent *event) { - nnlcToggle->updateToggle(); + nnlcToggle->updateToggle(offroad); updateToggles(offroad); } @@ -129,27 +158,40 @@ void LateralPanel::hideEvent(QHideEvent *event) { } void LateralPanel::updateToggles(bool _offroad) { - for (auto *toggle : toggleOffroadOnly) { - toggle->setEnabled(_offroad); - } - + bool torque_allowed = true; auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC)); } else { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC)); } + + if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) { + params.remove("EnforceTorqueControl"); + torque_allowed = false; + } } else { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_CHECK_COMPATIBILITY, MADS_BASE_DESC)); + + params.remove("EnforceTorqueControl"); + torque_allowed = false; } + madsToggle->setEnabled(_offroad); madsSettingsButton->setEnabled(madsToggle->isToggled()); + torqueLateralControlToggle->setEnabled(_offroad && torque_allowed && !nnlcToggle->isToggled()); + torqueLateralControlSettingsButton->setEnabled(torqueLateralControlToggle->isToggled()); + blinkerPauseLateralSettings->refresh(); offroad = _offroad; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index 7b5e73fceb..88c4d401d0 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -15,6 +15,7 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" @@ -35,7 +36,6 @@ private: QStackedLayout* main_layout = nullptr; QWidget* sunnypilotScreen = nullptr; ScrollViewSP *sunnypilotScroller = nullptr; - std::vector toggleOffroadOnly; bool offroad; ParamControl *madsToggle; @@ -45,6 +45,9 @@ private: LaneChangeSettings *laneChangeWidget = nullptr; NeuralNetworkLateralControl *nnlcToggle = nullptr; BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr; + ParamControl *torqueLateralControlToggle; + PushButtonSP *torqueLateralControlSettingsButton; + TorqueLateralControlSettings *torqueLateralControlWidget = nullptr; const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC)."); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h new file mode 100644 index 0000000000..295072d1ef --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -0,0 +1,49 @@ +/* + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ +#pragma once + +enum class SpeedLimitOffsetType { + NONE, + FIXED, + PERCENT, +}; + +inline const QString SpeedLimitOffsetTypeTexts[]{ + QObject::tr("None"), + QObject::tr("Fixed"), + QObject::tr("Percent"), +}; + +enum class SpeedLimitSourcePolicy { + CAR_ONLY, + MAP_ONLY, + CAR_FIRST, + MAP_FIRST, + COMBINED, +}; + +inline const QString SpeedLimitSourcePolicyTexts[]{ + QObject::tr("Car\nOnly"), + QObject::tr("Map\nOnly"), + QObject::tr("Car\nFirst"), + QObject::tr("Map\nFirst"), + QObject::tr("Combined\nData") +}; + +enum class SpeedLimitMode { + OFF, + INFORMATION, + WARNING, + ASSIST, +}; + +inline const QString SpeedLimitModeTexts[]{ + QObject::tr("Off"), + QObject::tr("Information"), + QObject::tr("Warning"), + QObject::tr("Assist"), +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc new file mode 100644 index 0000000000..764a8e0208 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ +#include + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" + +SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + + // Back button + PanelBackButton *back = new PanelBackButton(tr("Back")); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + main_layout->addWidget(back, 0, Qt::AlignLeft); + + main_layout->addSpacing(10); + + ListWidgetSP *list = new ListWidgetSP(this); + + std::vector speed_limit_policy_texts{ + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::CAR_ONLY)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::MAP_ONLY)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::CAR_FIRST)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::MAP_FIRST)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::COMBINED)] + }; + speed_limit_policy = new ButtonParamControlSP( + "SpeedLimitPolicy", + tr("Speed Limit Source"), + "", + "", + speed_limit_policy_texts, + 250); + list->addItem(speed_limit_policy); + connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh); + + speedLimitPolicyScroller = new ScrollViewSP(list, this); + main_layout->addWidget(speedLimitPolicyScroller); + refresh(); +}; + +void SpeedLimitPolicy::refresh() { + SpeedLimitSourcePolicy policy_param = static_cast(std::atoi(params.get("SpeedLimitPolicy").c_str())); + speed_limit_policy->setDescription(sourceDescription(policy_param)); +} + +void SpeedLimitPolicy::showEvent(QShowEvent *event) { + speedLimitPolicyScroller->verticalScrollBar()->setValue(0); + refresh(); + speed_limit_policy->showDescription(); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h new file mode 100644 index 0000000000..219aa53c6e --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" + +class SpeedLimitPolicy : public QWidget { + Q_OBJECT + +public: + explicit SpeedLimitPolicy(QWidget *parent = nullptr); + void refresh(); + void showEvent(QShowEvent *event) override; + +signals: + void backPress(); + +private: + Params params; + ButtonParamControlSP *speed_limit_policy; + ScrollViewSP *speedLimitPolicyScroller; + + static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) { + QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car"); + QString map_only = tr("⦿ Map Only: Use Speed Limit data only from OpenStreetMaps"); + QString car_first = tr("⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps"); + QString map_first = tr("⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car"); + QString combined = tr("⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps"); + + if (type == SpeedLimitSourcePolicy::CAR_ONLY) { + car_only = "" + car_only + ""; + } else if (type == SpeedLimitSourcePolicy::MAP_ONLY) { + map_only = "" + map_only + ""; + } else if (type == SpeedLimitSourcePolicy::CAR_FIRST) { + car_first = "" + car_first + ""; + } else if (type == SpeedLimitSourcePolicy::MAP_FIRST) { + map_first = "" + map_first + ""; + } else { + combined = "" + combined + ""; + } + + return QString("%1
%2
%3
%4
%5") + .arg(car_only) + .arg(map_only) + .arg(car_first) + .arg(map_first) + .arg(combined); + } +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc new file mode 100644 index 0000000000..ff5d6221ce --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -0,0 +1,160 @@ +/* + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" + +SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) { + subPanelFrame = new QFrame(); + QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame); + subPanelLayout->setContentsMargins(0, 0, 0, 0); + subPanelLayout->setSpacing(0); + + // Back button + PanelBackButton *back = new PanelBackButton(tr("Back")); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + subPanelLayout->addWidget(back, 0, Qt::AlignLeft); + + subPanelLayout->addSpacing(20); + + ListWidgetSP *list = new ListWidgetSP(this, false); + + speedLimitPolicyScreen = new SpeedLimitPolicy(this); + + std::vector speed_limit_mode_texts{ + SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::INFORMATION)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::WARNING)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::ASSIST)], + }; + speed_limit_mode_settings = new ButtonParamControlSP( + "SpeedLimitMode", + tr("Speed Limit"), + "", + "", + speed_limit_mode_texts, + 380); + list->addItem(speed_limit_mode_settings); + + list->addItem(horizontal_line()); + list->addItem(vertical_space()); + + speedLimitSource = new PushButtonSP(tr("Customize Source")); + connect(speedLimitSource, &QPushButton::clicked, [&]() { + speedLimitScroller->setLastScrollPosition(); + setCurrentWidget(speedLimitPolicyScreen); + speedLimitPolicyScreen->refresh(); + }); + connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() { + speedLimitScroller->restoreScrollPosition(); + setCurrentWidget(subPanelFrame); + showEvent(new QShowEvent()); + }); + + speedLimitSource->setFixedWidth(720); + list->addItem(speedLimitSource); + + list->addItem(vertical_space(0)); + list->addItem(horizontal_line()); + + QFrame *offsetFrame = new QFrame(this); + QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame); + + std::vector speed_limit_offset_texts{ + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::NONE)], + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::FIXED)], + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::PERCENT)] + }; + speed_limit_offset_settings = new ButtonParamControlSP( + "SpeedLimitOffsetType", + tr("Speed Limit Offset"), + "", + "", + speed_limit_offset_texts, + 500); + + offsetLayout->addWidget(speed_limit_offset_settings); + + speed_limit_offset = new OptionControlSP( + "SpeedLimitValueOffset", + "", + "", + "", + {-30, 30} + ); + offsetLayout->addWidget(speed_limit_offset); + + list->addItem(offsetFrame); + + connect(speed_limit_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); + connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh); + connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription); + connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); + + refresh(); + speedLimitScroller = new ScrollViewSP(list, this); + subPanelLayout->addWidget(speedLimitScroller); + addWidget(subPanelFrame); + addWidget(speedLimitPolicyScreen); + setCurrentWidget(subPanelFrame); +} + +void SpeedLimitSettings::refresh() { + bool is_metric_param = params.getBool("IsMetric"); + SpeedLimitMode speed_limit_mode_param = static_cast(std::atoi(params.get("SpeedLimitMode").c_str())); + SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); + QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); + + bool has_longitudinal_control; + bool intelligent_cruise_button_management_available; + auto cp_bytes = params.get("CarParamsPersistent"); + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { + AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); + + has_longitudinal_control = hasLongitudinalControl(CP); + intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + } else { + has_longitudinal_control = false; + intelligent_cruise_button_management_available = false; + } + + speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); + speed_limit_offset->setDescription(offsetDescription(offset_type_param)); + + if (offset_type_param == SpeedLimitOffsetType::PERCENT) { + offsetLabel += "%"; + } else if (offset_type_param == SpeedLimitOffsetType::FIXED) { + offsetLabel += QString(" %1").arg(is_metric_param ? "km/h" : "mph"); + } + + if (offset_type_param == SpeedLimitOffsetType::NONE) { + speed_limit_offset->setVisible(false); + } else { + speed_limit_offset->setVisible(true); + speed_limit_offset->setLabel(offsetLabel); + speed_limit_offset->showDescription(); + } + + if (has_longitudinal_control || intelligent_cruise_button_management_available) { + speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues())); + } else { + speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues( + {SpeedLimitMode::OFF,SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING})); + } + + speed_limit_mode_settings->showDescription(); + speed_limit_offset->showDescription(); +} + +void SpeedLimitSettings::showEvent(QShowEvent *event) { + refresh(); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h new file mode 100644 index 0000000000..f76f9bea2a --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" + +class SpeedLimitSettings : public QStackedWidget { + Q_OBJECT + +public: + SpeedLimitSettings(QWidget *parent = nullptr); + void refresh(); + void showEvent(QShowEvent *event) override; + +signals: + void backPress(); + +private: + Params params; + ScrollViewSP *speedLimitScroller; + QFrame *subPanelFrame; + ButtonParamControlSP *speed_limit_mode_settings; + PushButtonSP *speedLimitSource; + SpeedLimitPolicy *speedLimitPolicyScreen; + ButtonParamControlSP *speed_limit_offset_settings; + OptionControlSP *speed_limit_offset; + + static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) { + QString none_str = tr("⦿ None: No Offset"); + QString fixed_str = tr("⦿ Fixed: Adds a fixed offset [Speed Limit + Offset]"); + QString percent_str = tr("⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)]"); + + if (type == SpeedLimitOffsetType::FIXED) { + fixed_str = "" + fixed_str + ""; + } else if (type == SpeedLimitOffsetType::PERCENT) { + percent_str = "" + percent_str + ""; + } else { + none_str = "" + none_str + ""; + } + + return QString("%1
%2
%3") + .arg(none_str) + .arg(fixed_str) + .arg(percent_str); + } + + static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) { + QString off_str = tr("⦿ Off: Disables the Speed Limit functions."); + QString info_str = tr("⦿ Information: Displays the current road's speed limit."); + QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit."); + QString assist_str = tr("⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons."); + + if (mode == SpeedLimitMode::ASSIST) { + assist_str = "" + assist_str + ""; + } else if (mode == SpeedLimitMode::WARNING) { + warning_str = "" + warning_str + ""; + } else if (mode == SpeedLimitMode::INFORMATION) { + info_str = "" + info_str + ""; + } else { + off_str = "" + off_str + ""; + } + + return QString("%1
%2
%3
%4") + .arg(off_str) + .arg(info_str) + .arg(warning_str) + .arg(assist_str); + } + + static std::vector getSpeedLimitModeValues() { + std::vector values; + for (int i = static_cast(SpeedLimitMode::OFF); + i <= static_cast(SpeedLimitMode::ASSIST); ++i) { + values.push_back(static_cast(i)); + } + return values; + } + + static std::vector convertSpeedLimitModeValues(const std::vector &modes) { + std::vector values; + for (const auto& mode : modes) { + values.push_back(static_cast(mode)); + } + return values; + } +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 15ed10bf9a..3e3977ff64 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -8,6 +8,21 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h" LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { + setStyleSheet(R"( + #back_btn { + font-size: 50px; + margin: 0px; + padding: 15px; + border-width: 0; + border-radius: 30px; + color: #dddddd; + background-color: #393939; + } + #back_btn:pressed { + background-color: #4a4a4a; + } + )"); + main_layout = new QStackedLayout(this); ListWidget *list = new ListWidget(this, false); @@ -18,12 +33,50 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { cruisePanelScroller = new ScrollViewSP(list, this); vlayout->addWidget(cruisePanelScroller); + intelligentCruiseButtonManagement = new ParamControlSP( + "IntelligentCruiseButtonManagement", + tr("Intelligent Cruise Button Management (ICBM) (Alpha)"), + tr("When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control."), + "", + this + ); + intelligentCruiseButtonManagement->setConfirmation(true, false); + list->addItem(intelligentCruiseButtonManagement); + + SmartCruiseControlVision = new ParamControl( + "SmartCruiseControlVision", + tr("Smart Cruise Control - Vision"), + tr("Use vision path predictions to estimate the appropriate speed to drive through turns ahead."), + ""); + list->addItem(SmartCruiseControlVision); + + SmartCruiseControlMap = new ParamControl( + "SmartCruiseControlMap", + tr("Smart Cruise Control - Map"), + tr("Use map data to estimate the appropriate speed to drive through turns ahead."), + ""); + list->addItem(SmartCruiseControlMap); + customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this); list->addItem(customAccIncrement); QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh); + speedLimitSettings = new PushButtonSP(tr("Speed Limit"), 750, this); + connect(speedLimitSettings, &QPushButton::clicked, [&]() { + cruisePanelScroller->setLastScrollPosition(); + main_layout->setCurrentWidget(speedLimitScreen); + }); + list->addItem(speedLimitSettings); + + speedLimitScreen = new SpeedLimitSettings(this); + connect(speedLimitScreen, &SpeedLimitSettings::backPress, [=]() { + cruisePanelScroller->restoreScrollPosition(); + main_layout->setCurrentWidget(cruisePanelScreen); + }); + main_layout->addWidget(cruisePanelScreen); + main_layout->addWidget(speedLimitScreen); main_layout->setCurrentWidget(cruisePanelScreen); refresh(offroad); } @@ -35,16 +88,22 @@ void LongitudinalPanel::showEvent(QShowEvent *event) { void LongitudinalPanel::refresh(bool _offroad) { auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); has_longitudinal_control = hasLongitudinalControl(CP); is_pcm_cruise = CP.getPcmCruise(); + intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); } else { has_longitudinal_control = false; is_pcm_cruise = false; + intelligent_cruise_button_management_available = false; } QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); @@ -56,7 +115,7 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->setDescription(onroadOnlyDescription); customAccIncrement->showDescription(); } else { - if (has_longitudinal_control) { + if (has_longitudinal_control || intelligent_cruise_button_management_available) { if (is_pcm_cruise) { customAccIncrement->setDescription(accPcmCruiseDisabledDescription); customAccIncrement->showDescription(); @@ -68,12 +127,21 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->toggleFlipped(false); customAccIncrement->setDescription(accNoLongDescription); customAccIncrement->showDescription(); + params.remove("IntelligentCruiseButtonManagement"); + intelligentCruiseButtonManagement->toggleFlipped(false); } } + bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control; + intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad); + // enable toggle when long is available and is not PCM cruise - customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad); + bool cai_allowed = (has_longitudinal_control && !is_pcm_cruise) || icbm_allowed; + customAccIncrement->setEnabled(cai_allowed && !offroad); customAccIncrement->refresh(); + SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed); + SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed); + offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index 58e94f333b..0b7f39c645 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -8,6 +8,7 @@ #pragma once #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" @@ -23,10 +24,16 @@ private: Params params; bool has_longitudinal_control = false; bool is_pcm_cruise = false; + bool intelligent_cruise_button_management_available = false;; bool offroad = false; QStackedLayout *main_layout = nullptr; ScrollViewSP *cruisePanelScroller = nullptr; QWidget *cruisePanelScreen = nullptr; CustomAccIncrement *customAccIncrement = nullptr; + ParamControl *SmartCruiseControlVision; + ParamControl *SmartCruiseControlMap; + ParamControl *intelligentCruiseButtonManagement = nullptr; + SpeedLimitSettings *speedLimitScreen; + PushButtonSP *speedLimitSettings; }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc index 332fd71a18..e946d7870f 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc @@ -12,6 +12,7 @@ #include #include "common/swaglog.h" +#include "selfdrive/ui/sunnypilot/qt/util.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" OsmPanel::OsmPanel(QWidget *parent) : QFrame(parent) { @@ -91,24 +92,30 @@ ButtonControlSP *OsmPanel::setupOsmDownloadButton(QWidget *parent) { locationTitles.push_back(std::get<0>(loc)); } - const QString selection = MultiOptionDialog::getSelection(tr("Country"), locationTitles, currentTitle, this); - if (!selection.isEmpty()) { - params.put("OsmLocal", "1"); - params.put("OsmLocationTitle", selection.toStdString()); - for (auto &loc: locations) { - if (std::get<0>(loc) == selection) { - params.put("OsmLocationName", std::get<1>(loc).toStdString()); - break; + InputDialog d(tr("Search Country"), this, tr("Enter search keywords, or leave blank to list all countries."), false); + d.setMinLength(0); + const int ret = d.exec(); + if (ret) { + const QString selection = search(d.text(), locationTitles, tr("Select Country")); + if (!selection.isEmpty()) { + params.put("OsmLocal", "1"); + params.put("OsmLocationTitle", selection.toStdString()); + for (auto &loc: locations) { + if (std::get<0>(loc) == selection) { + params.put("OsmLocationName", std::get<1>(loc).toStdString()); + break; + } } - } - if (params.get("OsmLocationName") == "US") { - usStatesBtn->click(); - return; - } else if (selection != "== None ==") { - if (showConfirmationDialog(parent)) { - osm_download_in_progress = true; - params.putBool("OsmDbUpdatesCheck", true); - updateLabels(); + if (params.get("OsmLocationName") == "US") { + usStatesBtn->click(); + return; + } + if (selection != "== None ==") { + if (showConfirmationDialog(parent)) { + osm_download_in_progress = true; + params.putBool("OsmDbUpdatesCheck", true); + updateLabels(); + } } } } @@ -135,20 +142,25 @@ ButtonControlSP *OsmPanel::setupUsStatesButton(QWidget *parent) { locationTitles.push_back(std::get<0>(loc)); } - const QString selection = MultiOptionDialog::getSelection(tr("State"), locationTitles, currentTitle, this); - if (!selection.isEmpty()) { - params.put("OsmStateTitle", selection.toStdString()); - for (auto &loc: locations) { - if (std::get<0>(loc) == selection) { - params.put("OsmStateName", std::get<1>(loc).toStdString()); - break; + InputDialog d(tr("Search State"), this, tr("Enter search keywords, or leave blank to list all states."), false); + d.setMinLength(0); + const int ret = d.exec(); + if (ret) { + const QString selection = search(d.text(), locationTitles, tr("Select State")); + if (!selection.isEmpty()) { + params.put("OsmStateTitle", selection.toStdString()); + for (auto &loc: locations) { + if (std::get<0>(loc) == selection) { + params.put("OsmStateName", std::get<1>(loc).toStdString()); + break; + } + } + usStatesBtn->setValue(selection); + if (showConfirmationDialog(parent)) { + osm_download_in_progress = true; + params.putBool("OsmDbUpdatesCheck", true); + updateLabels(); } - } - usStatesBtn->setValue(selection); - if (showConfirmationDialog(parent)) { - osm_download_in_progress = true; - params.putBool("OsmDbUpdatesCheck", true); - updateLabels(); } } updateLabels(); @@ -281,3 +293,15 @@ void OsmPanel::updateMapSize() { mapSizeFuture = QtConcurrent::run(getDirSize, MAP_PATH); } } + +QString OsmPanel::search(const QString &query, const QStringList &list, const QString &prompt_text) { + QStringList lst_results = searchFromList(query, list); + QString selection; + + if (lst_results.isEmpty()) { + ConfirmationDialog::alert(tr("No results found for keywords: %1").arg(query), this); + return selection; + } + selection = MultiOptionDialog::getSelection(prompt_text, lst_results, "", this); + return selection; +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h index 171129a11c..e4067d415f 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h @@ -76,6 +76,7 @@ private: void updateDownloadProgress(); static int extractIntFromJson(const QJsonObject &json, const QString &key); QString processUpdateStatus(bool pending_update_check, int total_files, int downloaded_files, const QJsonObject &json, bool failed_state); + QString search(const QString &query, const QStringList &list, const QString &prompt_text); ConfirmationDialog *confirmationDialog; LabelControlSP *mapdVersion; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index 5415e75ea9..c712442df3 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc @@ -13,6 +13,7 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/developer_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h" @@ -87,6 +88,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { PanelInfo(" " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"), PanelInfo(" " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"), PanelInfo(" " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"), + PanelInfo(" " + tr("Display"), new DisplayPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_display.png"), PanelInfo(" " + tr("OSM"), new OsmPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"), PanelInfo(" " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"), PanelInfo(" " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"), diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index c3aaf12d22..0324cd70f0 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -25,14 +25,69 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "BlindSpot", tr("Show Blind Spot Warnings"), tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."), - "../assets/offroad/icon_monitoring.png", + "", false, }, { "RainbowMode", tr("Enable Tesla Rainbow Mode"), RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "
" + tr("It")+ " " + tr("does not") + " " + tr("affect driving in any way.") + "", - "../assets/offroad/icon_monitoring.png", + "", + false, + }, + { + "StandstillTimer", + tr("Enable Standstill Timer"), + tr("Show a timer on the HUD when the car is at a standstill."), + "", + false, + }, + { + "RoadNameToggle", + tr("Display Road Name"), + tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."), + "", + false, + }, + { + "GreenLightAlert", + tr("Green Traffic Light Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you.")) + .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")), + "", + false, + }, + { + "LeadDepartAlert", + tr("Lead Departure Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving.")) + .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")), + "", + false, + }, + { + "TrueVEgoUI", + tr("Speedometer: Always Display True Speed"), + tr("Always display the true vehicle current speed from wheel speed sensors."), + "", + false, + }, + { + "HideVEgoUI", + tr("Speedometer: Hide from Onroad Screen"), + tr("When enabled, the speedometer on the onroad screen is not displayed."), + "", + false, + }, + { + "ShowTurnSignals", + tr("Display Turn Signals"), + tr("When enabled, visual turn indicators are drawn on the HUD."), + "", false, }, }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/alerts.cc b/selfdrive/ui/sunnypilot/qt/onroad/alerts.cc new file mode 100644 index 0000000000..695f6d2ce8 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/onroad/alerts.cc @@ -0,0 +1,102 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/onroad/alerts.h" + +#include +#include +#include +#include + +OnroadAlerts::Alert OnroadAlertsSP::getAlert(const SubMaster &sm, uint64_t started_frame) { + OnroadAlerts::Alert alert = OnroadAlerts::getAlert(sm, started_frame); + alert.text1.replace("openpilot", "sunnypilot"); + alert.text2.replace("openpilot", "sunnypilot"); + return alert; +} + +void OnroadAlertsSP::paintEvent(QPaintEvent *event) { + if (alert.size == cereal::SelfdriveState::AlertSize::NONE) { + return; + } else if (alert.size == cereal::SelfdriveState::AlertSize::FULL) { + OnroadAlerts::paintEvent(event); + return; + } + static std::map alert_heights = { + {cereal::SelfdriveState::AlertSize::SMALL, 271}, + {cereal::SelfdriveState::AlertSize::MID, 420} + }; + int h = alert_heights[alert.size]; + + QPainter p(this); + QFont topFont; + QFont bottomFont; + QRect topTextBoundingRect; + QRect bottomTextBoundingRect; + QRect rect; + + int margin = 40; + int radius = 30; + + const int dev_ui_info = uiStateSP()->scene.dev_ui_info; + const int v_adjustment = dev_ui_info > 1 && alert.size != cereal::SelfdriveState::AlertSize::FULL ? 40 : 0; + const int h_adjustment = dev_ui_info > 0 && alert.size != cereal::SelfdriveState::AlertSize::FULL ? 230 : 0; + + if (alert.size == cereal::SelfdriveState::AlertSize::SMALL) { + topFont = InterFont(74, QFont::DemiBold); + QFontMetrics fmTop(topFont); + topTextBoundingRect = fmTop.boundingRect( + QRect(0 + margin, height() - h + margin - v_adjustment, width() - margin * 2 - h_adjustment, 0), Qt::TextWordWrap, + alert.text1); + h = topTextBoundingRect.height(); + rect = QRect(0 + margin, height() - h - margin * 2 - v_adjustment, width() - margin * 2 - h_adjustment, h + margin); + } else if (alert.size == cereal::SelfdriveState::AlertSize::MID) { + topFont = InterFont(88, QFont::Bold); + bottomFont = InterFont(66); + QFontMetrics fmTop(topFont); + QFontMetrics fmBotton(bottomFont); + topTextBoundingRect = fmTop.boundingRect( + QRect(0 + margin, height() - h + margin - v_adjustment, width() - margin * 2 - h_adjustment, 0), Qt::TextWordWrap, + alert.text1); + bottomTextBoundingRect = fmBotton.boundingRect( + QRect(0 + margin, height() - h + margin - v_adjustment + topTextBoundingRect.height(), + width() - margin * 2 - h_adjustment, 0), Qt::TextWordWrap, alert.text2); + h = topTextBoundingRect.height() + bottomTextBoundingRect.height() + margin * 2; + rect = QRect(0 + margin, height() - h - margin * 2 - v_adjustment, width() - margin * 2 - h_adjustment, h + margin); + } + + + // draw background + gradient + // draw background + gradient + p.setPen(Qt::NoPen); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + p.setBrush(QBrush(alert_colors[alert.status])); + p.drawRoundedRect(rect, radius, radius); + + QLinearGradient g(0, rect.y(), 0, rect.bottom()); + g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05)); + g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35)); + + p.setCompositionMode(QPainter::CompositionMode_DestinationOver); + p.setBrush(QBrush(g)); + p.drawRoundedRect(rect, radius, radius); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + + // text + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setRenderHint(QPainter::TextAntialiasing); + p.setFont(topFont); + if (alert.size == cereal::SelfdriveState::AlertSize::SMALL) { + p.drawText(rect, Qt::AlignCenter | Qt::TextWordWrap, alert.text1); + } else if (alert.size == cereal::SelfdriveState::AlertSize::MID) { + QRect topText = QRect(rect.x(), rect.top() + margin, rect.width(), topTextBoundingRect.height()); + p.drawText(topText, Qt::AlignHCenter | Qt::AlignTop | Qt::TextWordWrap | Qt::AlignCenter, alert.text1); + p.setFont(bottomFont); + p.drawText(QRect(rect.x(), topText.bottom() + margin, rect.width(), bottomTextBoundingRect.height()), + Qt::AlignHCenter | Qt::TextWordWrap | Qt::AlignCenter, alert.text2); + } +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/alerts.h b/selfdrive/ui/sunnypilot/qt/onroad/alerts.h new file mode 100644 index 0000000000..8b65309f46 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/onroad/alerts.h @@ -0,0 +1,22 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/qt/onroad/alerts.h" +#include "selfdrive/ui/sunnypilot/ui.h" + +class OnroadAlertsSP : public OnroadAlerts { + Q_OBJECT + +public: + OnroadAlertsSP(QWidget *parent = 0) : OnroadAlerts(parent) {} + +protected: + void paintEvent(QPaintEvent *) override; + Alert getAlert(const SubMaster &sm, uint64_t started_frame); +}; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 9ead933d04..bc7060d31c 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -4,19 +4,29 @@ * This file is part of sunnypilot and is licensed under the MIT License. * See the LICENSE.md file in the root directory for more details. */ +#include #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h" #include "selfdrive/ui/qt/util.h" -HudRendererSP::HudRendererSP() {} +HudRendererSP::HudRendererSP() { + plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {105, 105}); + minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {105, 105}); + + int small_max = e2e_alert_small * 2 - 40; + int large_max = e2e_alert_large * 2 - 40; + green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {small_max, small_max}); + green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {large_max, large_max}); + lead_depart_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {small_max, small_max}); + lead_depart_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {large_max, large_max}); +} void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); const SubMaster &sm = *(s.sm); - const bool cs_alive = sm.alive("controlsState"); const auto cs = sm["controlsState"].getControlsState(); const auto car_state = sm["carState"].getCarState(); const auto car_control = sm["carControl"].getCarControl(); @@ -25,6 +35,32 @@ void HudRendererSP::updateState(const UIState &s) { const auto gpsLocation = is_gps_location_external ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation(); const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto car_params = sm["carParams"].getCarParams(); + const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); + const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP(); + + float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH; + speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; + speedLimitLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLast() * speedConv; + speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; + speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid(); + speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); + speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv; + speedLimitMode = static_cast(s.scene.speed_limit_mode); + speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState(); + speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive(); + roadName = s.scene.road_name; + if (sm.updated("liveMapDataSP")) { + roadNameStr = QString::fromStdString(lmd.getRoadName()); + speedLimitAheadValid = lmd.getSpeedLimitAheadValid(); + speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv; + speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance(); + if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD) { + speedLimitAheadValidFrame++; + } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) { + speedLimitAheadValidFrame--; + } + } + speedLimitAheadDistancePrev = speedLimitAheadDistance; static int reverse_delay = 0; bool reverse_allowed = false; @@ -39,13 +75,6 @@ void HudRendererSP::updateState(const UIState &s) { } reversing = reverse_allowed; - is_metric = s.scene.is_metric; - - // Handle older routes where vEgoCluster is not set - v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; - float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo(); - speed = cs_alive ? std::max(0.0, v_ego) : 0.0; - speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; latActive = car_control.getLatActive(); steerOverride = car_state.getSteeringPressed(); @@ -75,11 +104,72 @@ void HudRendererSP::updateState(const UIState &s) { latAccelFactorFiltered = ltp.getLatAccelFactorFiltered(); frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered(); liveValid = ltp.getLiveValid(); + + standstillTimer = s.scene.standstill_timer; + isStandstill = car_state.getStandstill(); + longOverride = car_control.getCruiseControl().getOverride(); + smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled(); + smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive(); + smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled(); + smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); + + greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); + leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert(); + + // override stock current speed values + float v_ego = (v_ego_cluster_seen && !s.scene.trueVEgoUI) ? car_state.getVEgoCluster() : car_state.getVEgo(); + speed = std::max(0.0f, v_ego * (is_metric ? MS_TO_KPH : MS_TO_MPH)); + hideVEgoUI = s.scene.hideVEgoUI; + + leftBlinkerOn = car_state.getLeftBlinker(); + rightBlinkerOn = car_state.getRightBlinker(); + leftBlindspot = car_state.getLeftBlindspot(); + rightBlindspot = car_state.getRightBlindspot(); + showTurnSignals = s.scene.turn_signals; } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { HudRenderer::draw(p, surface_rect); + + e2eAlertDisplayTimer = std::max(0, e2eAlertDisplayTimer - 1); + + p.save(); + + if (is_cruise_available) { + drawSetSpeedSP(p, surface_rect); + } + + if (!hideVEgoUI) { + drawCurrentSpeedSP(p, surface_rect); + } + if (!reversing) { + // Smart Cruise Control + int x_offset = -260; + int y1_offset = -80; + int y2_offset = -140; + + int y_scc_v = 0, y_scc_m = 0; + const int orders[2] = {y1_offset, y2_offset}; + int i = 0; + // SCC-V takes first order + if (smartCruiseControlVisionEnabled) y_scc_v = orders[i++]; + if (smartCruiseControlMapEnabled) y_scc_m = orders[i++]; + + // Smart Cruise Control - Vision + bool scc_vision_active_pulse = pulseElement(smartCruiseControlVisionFrame); + if ((smartCruiseControlVisionEnabled && !smartCruiseControlVisionActive) || (smartCruiseControlVisionActive && scc_vision_active_pulse)) { + drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_v, "SCC-V"); + } + smartCruiseControlVisionFrame = smartCruiseControlVisionActive ? (smartCruiseControlVisionFrame + 1) : 0; + + // Smart Cruise Control - Map + bool scc_map_active_pulse = pulseElement(smartCruiseControlMapFrame); + if ((smartCruiseControlMapEnabled && !smartCruiseControlMapActive) || (smartCruiseControlMapActive && scc_map_active_pulse)) { + drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_m, "SCC-M"); + } + smartCruiseControlMapFrame = smartCruiseControlMapActive ? (smartCruiseControlMapFrame + 1) : 0; + // Bottom Dev UI if (devUiInfo == 2) { QRect rect_bottom(surface_rect.left(), surface_rect.bottom() - 60, surface_rect.width(), 61); @@ -94,7 +184,81 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { QRect rect_right(surface_rect.right() - (UI_BORDER_SIZE * 2), UI_BORDER_SIZE * 1.5, 184, 170); drawRightDevUI(p, surface_rect.right() - 184 - UI_BORDER_SIZE * 2, UI_BORDER_SIZE * 2 + rect_right.height()); } + + // Speed Limit + bool showSpeedLimit; + bool speed_limit_assist_pre_active_pulse = pulseElement(speedLimitAssistFrame); + + // Position speed limit sign next to set speed box + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + QRect sign_rect(sign_x, sign_y, sign_width, sign_height); + + if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) { + speedLimitAssistFrame++; + showSpeedLimit = speed_limit_assist_pre_active_pulse; + drawSpeedLimitPreActiveArrow(p, sign_rect); + } else { + speedLimitAssistFrame = 0; + showSpeedLimit = speedLimitMode != SpeedLimitMode::OFF; + } + + if (showSpeedLimit) { + drawSpeedLimitSigns(p, sign_rect); + + // do not show during SLA's preActive state + if (speedLimitAssistState != cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) { + drawUpcomingSpeedLimit(p); + } + } + + // Road Name + drawRoadName(p, surface_rect); + + // Green Light & Lead Depart Alerts + if (greenLightAlert or leadDepartAlert) { + e2eAlertDisplayTimer = 3 * UI_FREQ; + // reset onroad sleep timer for e2e alerts + uiStateSP()->reset_onroad_sleep_timer(); + } + + if (e2eAlertDisplayTimer > 0) { + e2eAlertFrame++; + if (greenLightAlert) { + alert_text = tr("GREEN\nLIGHT"); + alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img; + } + else if (leadDepartAlert) { + alert_text = tr("LEAD VEHICLE\nDEPARTING"); + alert_img = devUiInfo > 0 ? lead_depart_alert_small_img : lead_depart_alert_large_img; + } + drawE2eAlert(p, surface_rect); + } + // Standstill Timer + else if (standstillTimer && isStandstill) { + alert_img = QPixmap(); + + standstillElapsedTime += 1.0 / UI_FREQ; + int minute = static_cast(standstillElapsedTime / 60); + int second = static_cast(standstillElapsedTime - (minute * 60)); + alert_text = QString("%1:%2").arg(minute, 1, 10, QChar('0')).arg(second, 2, 10, QChar('0')); + drawE2eAlert(p, surface_rect, tr("STOPPED")); + e2eAlertFrame++; + } + // No Alerts displayed + else { + e2eAlertFrame = 0; + } + + // Blinker + if (showTurnSignals) { + drawBlinker(p, surface_rect); + } } + + p.restore(); } void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) { @@ -104,6 +268,48 @@ void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QCo p.drawText(real_rect.x(), real_rect.bottom(), text); } +bool HudRendererSP::pulseElement(int frame) { + if (frame % UI_FREQ < (UI_FREQ / 2.5)) { + return false; + } + + return true; +} + +void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name) { + int x = surface_rect.center().x(); + int y = surface_rect.height() / 4; + + QString text = QString::fromStdString(name); + QFont font = InterFont(36, QFont::Bold); + p.setFont(font); + + QFontMetrics fm(font); + + int padding_v = 5; + int box_width = 160; + int box_height = fm.height() + padding_v * 2; + + QRectF bg_rect(x - (box_width / 2) + x_offset, + y - (box_height / 2) + y_offset, + box_width, box_height); + + QPainterPath boxPath; + boxPath.addRoundedRect(bg_rect, 10, 10); + + int text_w = fm.horizontalAdvance(text); + qreal baseline_y = bg_rect.top() + padding_v + fm.ascent(); + qreal text_x = bg_rect.center().x() - (text_w / 2.0); + + QPainterPath textPath; + textPath.addText(QPointF(text_x, baseline_y), font, text); + boxPath = boxPath.subtracted(textPath); + + p.setPen(Qt::NoPen); + p.setBrush(longOverride ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0xff, 0, 0xff)); + p.drawPath(boxPath); +} + int HudRendererSP::drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) { p.setFont(InterFont(28, QFont::Bold)); @@ -206,3 +412,420 @@ void HudRendererSP::drawBottomDevUI(QPainter &p, int x, int y) { UiElement altitudeElement = DeveloperUi::getAltitude(gpsAccuracy, altitude); rw += drawBottomDevUIElement(p, rw, y, altitudeElement.value, altitudeElement.label, altitudeElement.units, altitudeElement.color); } + +void HudRendererSP::drawSpeedLimitSigns(QPainter &p, QRect &sign_rect) { + bool speedLimitWarningEnabled = speedLimitMode >= SpeedLimitMode::WARNING; // TODO-SP: update to include SpeedLimitMode::ASSIST + bool hasSpeedLimit = speedLimitValid || speedLimitLastValid; + bool overspeed = hasSpeedLimit && std::nearbyint(speedLimitFinalLast) < std::nearbyint(speed); + QString speedLimitStr = hasSpeedLimit ? QString::number(std::nearbyint(speedLimitLast)) : "---"; + + // Offset display text + QString speedLimitSubText = ""; + if (speedLimitOffset != 0) { + speedLimitSubText = (speedLimitOffset > 0 ? "" : "-") + QString::number(std::nearbyint(speedLimitOffset)); + } + + float speedLimitSubTextFactor = is_metric ? 0.5 : 0.6; + if (speedLimitSubText.size() >= 3) { + speedLimitSubTextFactor = 0.475; + } + + int alpha = 255; + QColor red_color = QColor(255, 0, 0, alpha); + QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color : + (!speedLimitValid && speedLimitLastValid ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0, 0, alpha)); + + if (is_metric) { + // EU Vienna Convention style circular sign + QRect vienna_rect = sign_rect; + int circle_size = std::min(vienna_rect.width(), vienna_rect.height()); + QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size); + + if (vienna_rect.width() > vienna_rect.height()) { + circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2); + } else if (vienna_rect.height() > vienna_rect.width()) { + circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2); + } + + // White background circle + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(circle_rect); + + // Red border ring with color coding + QRect red_ring = circle_rect; + + p.setBrush(red_color); + p.drawEllipse(red_ring); + + // Center white circle for text + int ring_size = circle_size * 0.12; + QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(center_circle); + + // Speed value, smaller font for 3+ digits + int font_size = (speedLimitStr.size() >= 3) ? 70 : 85; + p.setFont(InterFont(font_size, QFont::Bold)); + + p.setPen(speed_color); + p.drawText(center_circle, Qt::AlignCenter, speedLimitStr); + + // Offset value in small circular box + if (!speedLimitSubText.isEmpty() && hasSpeedLimit) { + int offset_circle_size = circle_size * 0.4; + int overlap = offset_circle_size * 0.25; + QRect offset_circle_rect( + circle_rect.right() - offset_circle_size/1.25 + overlap, + circle_rect.top() - offset_circle_size/1.75 + overlap, + offset_circle_size, + offset_circle_size + ); + + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawEllipse(offset_circle_rect); + + p.setFont(InterFont(offset_circle_size * speedLimitSubTextFactor, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText); + } + } else { + // US/Canada MUTCD style sign + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(sign_rect, 32, 32); + + // Inner border with violation color coding + QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10); + QColor border_color = QColor(0, 0, 0, alpha); + + p.setPen(QPen(border_color, 4)); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(inner_rect, 22, 22); + + // "SPEED LIMIT" text + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(0, 0, 0, alpha)); + p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + + // Speed value with color coding + p.setFont(InterFont(90, QFont::Bold)); + + p.setPen(speed_color); + p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + + // Offset value in small box + if (!speedLimitSubText.isEmpty() && hasSpeedLimit) { + int offset_box_size = sign_rect.width() * 0.4; + int overlap = offset_box_size * 0.25; + QRect offset_box_rect( + sign_rect.right() - offset_box_size/1.5 + overlap, + sign_rect.top() - offset_box_size/1.25 + overlap, + offset_box_size, + offset_box_size + ); + + int corner_radius = offset_box_size * 0.2; + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius); + + p.setFont(InterFont(offset_box_size * speedLimitSubTextFactor, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText); + } + } +} + +void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) { + bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0; + if (!speed_limit_ahead) { + return; + } + + auto roundToInterval = [&](float distance, int interval, int threshold) { + int base = static_cast(distance / interval) * interval; + return (distance - base >= threshold) ? base + interval : base; + }; + + auto outputDistance = [&] { + if (is_metric) { + if (speedLimitAheadDistance < 50) return tr("Near"); + if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km"); + + int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50); + return QString::number(rounded) + tr("m"); + } else { + float distance_ft = speedLimitAheadDistance * METER_TO_FOOT; + if (distance_ft < 100) return tr("Near"); + if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi"); + + int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50); + return QString::number(rounded) + tr("ft"); + } + }; + + QString speedStr = QString::number(std::nearbyint(speedLimitAhead)); + QString distanceStr = outputDistance(); + + // Position below current speed limit sign + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + + const int ahead_width = 170; + const int ahead_height = 160; + const int ahead_x = sign_x + (sign_width - ahead_width) / 2; + const int ahead_y = sign_y + sign_height + 10; + + QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height); + p.setPen(QPen(QColor(255, 255, 255, 100), 3)); + p.setBrush(QColor(0, 0, 0, 180)); + p.drawRoundedRect(ahead_rect, 16, 16); + + // "AHEAD" label + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(200, 200, 200, 255)); + p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD")); + + // Speed value + p.setFont(InterFont(70, QFont::Bold)); + p.setPen(QColor(255, 255, 255, 255)); + p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr); + + // Distance + p.setFont(InterFont(40, QFont::Normal)); + p.setPen(QColor(180, 180, 180, 255)); + p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr); +} + +void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) { + if (!roadName || roadNameStr.isEmpty()) return; + + // Measure text to size container + p.setFont(InterFont(46, QFont::DemiBold)); + QFontMetrics fm(p.font()); + + int text_width = fm.horizontalAdvance(roadNameStr); + int padding = 40; + int rect_width = text_width + padding; + + // Constrain to reasonable bounds + int min_width = 200; + int max_width = surface_rect.width() - 40; + rect_width = std::max(min_width, std::min(rect_width, max_width)); + + // Center at top of screen + QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -4, rect_width, 60); + + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 120)); + p.drawRoundedRect(road_rect, 12, 12); + + p.setPen(QColor(255, 255, 255, 200)); + + // Truncate if still too long + QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20); + p.drawText(road_rect, Qt::AlignCenter, truncated); +} + +void HudRendererSP::drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect) { + const int sign_margin = 12; + const int arrow_spacing = sign_margin * 3; + int arrow_x = sign_rect.right() + arrow_spacing; + + int _set_speed = std::nearbyint(set_speed); + int _speed_limit_final_last = std::nearbyint(speedLimitFinalLast); + + // Calculate the vertical offset using a sinusoidal function for smooth bouncing + double bounce_frequency = 2.0 * M_PI / UI_FREQ; // 20 frames for one full oscillation + int bounce_offset = 20 * sin(speedLimitAssistFrame * bounce_frequency); // Adjust the amplitude (20 pixels) as needed + + if (_set_speed < _speed_limit_final_last) { + QPoint iconPosition(arrow_x, sign_rect.center().y() - plus_arrow_up_img.height() / 2 + bounce_offset); + p.drawPixmap(iconPosition, plus_arrow_up_img); + } else if (_set_speed > _speed_limit_final_last) { + QPoint iconPosition(arrow_x, sign_rect.center().y() - minus_arrow_down_img.height() / 2 - bounce_offset); + p.drawPixmap(iconPosition, minus_arrow_down_img); + } +} + +void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) { + // Draw outer box + border to contain set speed + const QSize default_size = {172, 204}; + QSize set_speed_size = is_metric ? QSize(200, 204) : default_size; + QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); + + // Draw set speed box + p.setPen(QPen(QColor(255, 255, 255, 75), 6)); + p.setBrush(QColor(0, 0, 0, 166)); + p.drawRoundedRect(set_speed_rect, 32, 32); + + // Colors based on status + QColor max_color = QColor(0xa6, 0xa6, 0xa6, 0xff); + QColor set_speed_color = QColor(0x72, 0x72, 0x72, 0xff); + if (is_cruise_set) { + set_speed_color = QColor(255, 255, 255); + if (speedLimitAssistActive) { + set_speed_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 0xff, 0, 0xff); + max_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0x80, 0xd8, 0xa6, 0xff); + } else { + if (status == STATUS_DISENGAGED) { + max_color = QColor(255, 255, 255); + } else if (status == STATUS_OVERRIDE) { + max_color = QColor(0x91, 0x9b, 0x95, 0xff); + } else { + max_color = QColor(0x80, 0xd8, 0xa6, 0xff); + } + } + } + + // Draw "MAX" text + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(max_color); + p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("MAX")); + + // Draw set speed + QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(set_speed)) : "–"; + p.setFont(InterFont(90, QFont::Bold)); + p.setPen(set_speed_color); + p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); +} + +void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const QString &alert_alt_text) { + int size = devUiInfo > 0 ? e2e_alert_small : e2e_alert_large; + int x = surface_rect.center().x() + surface_rect.width() / 4; + int y = surface_rect.center().y() + 40; + x += devUiInfo > 0 ? 0 : 50; + y += devUiInfo > 0 ? 0 : 80; + QRect alertRect(x - size, y - size, size * 2, size * 2); + + // Alert Circle + QPoint center = alertRect.center(); + QColor frameColor; + if (not alert_alt_text.isEmpty()) frameColor = QColor(255, 255, 255, 75); + else frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75); + p.setPen(QPen(frameColor, 15)); + p.setBrush(QColor(0, 0, 0, 190)); + p.drawEllipse(center, size, size); + + // Alert Text + QColor txtColor; + QFont font; + int alert_bottom_adjustment; + if (not alert_alt_text.isEmpty()) { + font = InterFont(100, QFont::Bold); + alert_bottom_adjustment = 5; + txtColor = QColor(255, 255, 255, 255); + } else { + font = InterFont(48, QFont::Bold); + alert_bottom_adjustment = 7; + txtColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 255) : QColor(0, 255, 0, 190); + } + p.setPen(txtColor); + p.setFont(font); + QFontMetrics fm(p.font()); + QRect textRect = fm.boundingRect(alertRect, Qt::TextWordWrap, alert_text); + textRect.moveCenter({alertRect.center().x(), alertRect.center().y()}); + textRect.moveBottom(alertRect.bottom() - alertRect.height() / alert_bottom_adjustment); + p.drawText(textRect, Qt::AlignCenter, alert_text); + + if (not alert_alt_text.isEmpty()) { + // Alert Alternate Text + p.setFont(InterFont(80, QFont::Bold)); + p.setPen(QColor(255, 175, 3, 240)); + QFontMetrics fmt(p.font()); + QRect topTextRect = fmt.boundingRect(alertRect, Qt::TextWordWrap, alert_alt_text); + topTextRect.moveCenter({alertRect.center().x(), alertRect.center().y()}); + topTextRect.moveTop(alertRect.top() + alertRect.height() / 3.5); + p.drawText(topTextRect, Qt::AlignCenter, alert_alt_text); + } else { + // Alert Image instead of Top Text + QPointF pixmapCenterOffset = QPointF(alert_img.width() / 2.0, alert_img.height() / 2.0); + QPointF drawPoint = center - pixmapCenterOffset; + p.drawPixmap(drawPoint, alert_img); + } +} + +void HudRendererSP::drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect) { + QString speedStr = QString::number(std::nearbyint(speed)); + + p.setFont(InterFont(176, QFont::Bold)); + HudRenderer::drawText(p, surface_rect.center().x(), 210, speedStr); + + p.setFont(InterFont(66)); + HudRenderer::drawText(p, surface_rect.center().x(), 290, is_metric ? tr("km/h") : tr("mph"), 200); +} + +void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) { + if (!leftBlinkerOn && !rightBlinkerOn) { + blinkerFrameCounter = 0; + return; + } + ++blinkerFrameCounter; + + const int circleRadius = 44; + const int arrowLength = 44; + const int x_gap = 180; + const int y_offset = 272; + + const int centerX = surface_rect.center().x(); + const bool hazard = leftBlinkerOn && rightBlinkerOn; + + const QPen bgBorder(Qt::white, 5); + const QPen arrowPen(Qt::NoPen); + + p.save(); + + auto drawArrow = [&](int cx, int cy, int dir, const QBrush &arrowBrush) { + const int bodyLength = arrowLength / 2; + const int bodyWidth = arrowLength / 2; + const int headLength = arrowLength / 2; + const int headWidth = arrowLength; + + QPolygon arrow; + arrow.reserve(7); + arrow << QPoint(cx - dir * bodyLength, cy - bodyWidth / 2) + << QPoint(cx, cy - bodyWidth / 2) + << QPoint(cx, cy - headWidth / 2) + << QPoint(cx + dir * headLength, cy) + << QPoint(cx, cy + headWidth / 2) + << QPoint(cx, cy + bodyWidth / 2) + << QPoint(cx - dir * bodyLength, cy + bodyWidth / 2); + + p.setPen(arrowPen); + p.setBrush(arrowBrush); + p.drawPolygon(arrow); + }; + + auto drawCircle = [&](int cx, int cy, const QBrush &bgBrush) { + p.setPen(bgBorder); + p.setBrush(bgBrush); + p.drawEllipse(QPoint(cx, cy), circleRadius, circleRadius); + }; + + struct BlinkerSide { bool on; int dir; bool blocked; int cx; }; + const std::array sides = {{ + {leftBlinkerOn, -1, hazard ? true : (leftBlinkerOn && leftBlindspot), centerX - x_gap}, + {rightBlinkerOn, 1, hazard ? true : (rightBlinkerOn && rightBlindspot), centerX + x_gap}, + }}; + + for (const auto &s: sides) { + if (!s.on) continue; + + QColor bgColor = s.blocked ? QColor(135, 23, 23) : QColor(23, 134, 68); + QColor arrowColor = s.blocked ? QColor(66, 12, 12) : QColor(12, 67, 34); + if (pulseElement(blinkerFrameCounter)) arrowColor = Qt::white; + + const QBrush bgBrush(bgColor); + const QBrush arrowBrush(arrowColor); + + drawCircle(s.cx, y_offset, bgBrush); + drawArrow(s.cx, y_offset, s.dir, arrowBrush); + } + + p.restore(); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 968789bc15..c50e2d641d 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -8,8 +8,11 @@ #pragma once #include "selfdrive/ui/qt/onroad/hud.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h" +constexpr int SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD = 5; + class HudRendererSP : public HudRenderer { Q_OBJECT @@ -25,6 +28,17 @@ private: int drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color); int drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color); void drawBottomDevUI(QPainter &p, int x, int y); + void drawStandstillTimer(QPainter &p, int x, int y); + bool pulseElement(int frame); + void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name); + void drawSpeedLimitSigns(QPainter &p, QRect &sign_rect); + void drawUpcomingSpeedLimit(QPainter &p); + void drawRoadName(QPainter &p, const QRect &surface_rect); + void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect); + void drawSetSpeedSP(QPainter &p, const QRect &surface_rect); + void drawE2eAlert(QPainter &p, const QRect &surface_rect, const QString &alert_alt_text = ""); + void drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect); + void drawBlinker(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -53,4 +67,52 @@ private: bool reversing; cereal::CarParams::SteerControlType steerControlType; cereal::CarControl::Actuators::Reader actuators; + bool standstillTimer; + bool isStandstill; + float standstillElapsedTime; + bool longOverride; + bool smartCruiseControlVisionEnabled; + bool smartCruiseControlVisionActive; + int smartCruiseControlVisionFrame; + bool smartCruiseControlMapEnabled; + bool smartCruiseControlMapActive; + int smartCruiseControlMapFrame; + float speedLimit; + float speedLimitLast; + float speedLimitOffset; + bool speedLimitValid; + bool speedLimitLastValid; + float speedLimitFinalLast; + bool speedLimitAheadValid; + float speedLimitAhead; + float speedLimitAheadDistance; + float speedLimitAheadDistancePrev; + int speedLimitAheadValidFrame; + SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF; + bool roadName; + QString roadNameStr; + cereal::LongitudinalPlanSP::SpeedLimit::AssistState speedLimitAssistState; + bool speedLimitAssistActive; + int speedLimitAssistFrame; + QPixmap plus_arrow_up_img; + QPixmap minus_arrow_down_img; + int e2e_alert_small = 250; + int e2e_alert_large = 300; + QPixmap green_light_alert_small_img; + QPixmap green_light_alert_large_img; + bool greenLightAlert; + int e2eAlertFrame; + int e2eAlertDisplayTimer = 0; + bool leadDepartAlert; + QPixmap lead_depart_alert_small_img; + QPixmap lead_depart_alert_large_img; + QString alert_text; + QPixmap alert_img; + bool hideVEgoUI; + bool leftBlinkerOn; + bool rightBlinkerOn; + bool leftBlindspot; + bool rightBlindspot; + int blinkerFrameCounter; + bool showTurnSignals; }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc b/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc index b26d1b828a..3f853e3363 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/onroad_home.cc @@ -25,8 +25,10 @@ void OnroadWindowSP::updateState(const UIStateSP &s) { void OnroadWindowSP::mousePressEvent(QMouseEvent *e) { OnroadWindow::mousePressEvent(e); + uiStateSP()->reset_onroad_sleep_timer(); } void OnroadWindowSP::offroadTransition(bool offroad) { OnroadWindow::offroadTransition(offroad); + uiStateSP()->reset_onroad_sleep_timer(); } diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc index 2e066e88b5..be39b297d7 100644 --- a/selfdrive/ui/sunnypilot/qt/util.cc +++ b/selfdrive/ui/sunnypilot/qt/util.cc @@ -95,13 +95,12 @@ QStringList searchFromList(const QString &query, const QStringList &list) { return list; } - QStringList search_terms = query.simplified().toLower().split(" ", QString::SkipEmptyParts); + QStringList search_terms = query.simplified().toLower().replace(QRegularExpression("[^a-zA-Z0-9\\s]"), " ").split(" ", QString::SkipEmptyParts); QStringList search_results; for (const QString &element : list) { if (std::all_of(search_terms.begin(), search_terms.end(), [&](const QString &term) { QString normalized_term = term.normalized(QString::NormalizationForm_KD).toLower(); - normalized_term.remove(QRegularExpression("[^a-zA-Z0-9\\s]")); QString normalized_element = element.normalized(QString::NormalizationForm_KD).toLower(); return normalized_element.contains(normalized_term, Qt::CaseInsensitive); })) { diff --git a/selfdrive/ui/sunnypilot/qt/widgets/controls.h b/selfdrive/ui/sunnypilot/qt/widgets/controls.h index 03cb461385..e111ce2fb5 100644 --- a/selfdrive/ui/sunnypilot/qt/widgets/controls.h +++ b/selfdrive/ui/sunnypilot/qt/widgets/controls.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -500,7 +501,7 @@ private: int getParamValueScaled() { const auto param_value = QString::fromStdString(params.get(key)); - return static_cast(param_value.toFloat() * 100); + return std::nearbyint(param_value.toFloat() * 100.0f); } void setParamValueScaled(const int new_value) { @@ -544,10 +545,8 @@ public: main_layout->removeWidget(title_label); hlayout->addWidget(title_label, 1); } - if (spacingItem != nullptr && main_layout->indexOf(spacingItem) != -1) { - main_layout->removeItem(spacingItem); - spacingItem = nullptr; - } + main_layout->removeItem(spacingItem); + spacingItem = nullptr; } label.setStyleSheet(label_enabled_style); diff --git a/selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc b/selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc index b918938d11..b22684a295 100644 --- a/selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc +++ b/selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc @@ -14,10 +14,10 @@ ExpandableToggleRow::ExpandableToggleRow(const QString ¶m, const QString &ti QObject::connect(this, &ExpandableToggleRow::toggleFlipped, this, &ExpandableToggleRow::toggleClicked); collapsibleWidget = new QFrame(this); - collapsibleWidget->setContentsMargins(0, 0, 0, 0); collapsibleWidget->setVisible(false); QVBoxLayout *collapsible_layout = new QVBoxLayout(); collapsibleWidget->setLayout(collapsible_layout); + collapsible_layout->setContentsMargins(0, 0, 0, 0); list = new ListWidgetSP(this, false); diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 1277195df1..c021e0e95b 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -11,6 +11,15 @@ void UIStateSP::updateStatus() { UIState::updateStatus(); + + if (scene.started && scene.onroadScreenOffControl) { + auto selfdriveState = (*sm)["selfdriveState"].getSelfdriveState(); + if (selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE) { + reset_onroad_sleep_timer(); + } else if (scene.onroadScreenOffTimer > 0) { + scene.onroadScreenOffTimer--; + } + } } UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { @@ -20,7 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", - "carStateSP", "liveParameters" + "carStateSP", "liveParameters", "liveMapDataSP" }); // update timer @@ -34,6 +43,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { ui_update_params_sp(this); }); param_watcher->addParam("DevUIInfo"); + param_watcher->addParam("StandstillTimer"); } // This method overrides completely the update method from the parent class intentionally. @@ -51,6 +61,27 @@ void UIStateSP::update() { void ui_update_params_sp(UIStateSP *s) { auto params = Params(); s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str()); + s->scene.standstill_timer = params.getBool("StandstillTimer"); + s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str()); + s->scene.road_name = params.getBool("RoadNameToggle"); + s->scene.trueVEgoUI = params.getBool("TrueVEgoUI"); + s->scene.hideVEgoUI = params.getBool("HideVEgoUI"); + + // Onroad Screen Brightness + s->scene.onroadScreenOffBrightness = std::atoi(params.get("OnroadScreenOffBrightness").c_str()); + s->scene.onroadScreenOffControl = params.getBool("OnroadScreenOffControl"); + s->scene.onroadScreenOffTimerParam = std::atoi(params.get("OnroadScreenOffTimer").c_str()); + s->reset_onroad_sleep_timer(); + + s->scene.turn_signals = params.getBool("ShowTurnSignals"); +} + +void UIStateSP::reset_onroad_sleep_timer() { + if (scene.onroadScreenOffTimerParam >= 0 and scene.onroadScreenOffControl) { + scene.onroadScreenOffTimer = scene.onroadScreenOffTimerParam * UI_FREQ; + } else { + scene.onroadScreenOffTimer = -1; + } } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { diff --git a/selfdrive/ui/sunnypilot/ui.h b/selfdrive/ui/sunnypilot/ui.h index 393f997cbd..55dd43ad52 100644 --- a/selfdrive/ui/sunnypilot/ui.h +++ b/selfdrive/ui/sunnypilot/ui.h @@ -61,6 +61,7 @@ public: return user.user_id.toLower() != "unregisteredsponsor" && user.user_id.toLower() != "temporarysponsor"; }); } + void reset_onroad_sleep_timer(); signals: void sunnylinkRoleChanged(bool subscriber); diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index 93e0cd6c91..233f1e15de 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -9,4 +9,13 @@ typedef struct UISceneSP : UIScene { int dev_ui_info = 0; + bool standstill_timer = false; + int speed_limit_mode = 0; + bool road_name = false; + int onroadScreenOffBrightness, onroadScreenOffTimer = 0; + bool onroadScreenOffControl; + int onroadScreenOffTimerParam; + bool trueVEgoUI; + bool hideVEgoUI; + bool turn_signals = false; } UISceneSP; diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 653395ba1d..68d1f0c185 100755 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -29,7 +29,7 @@ UI_DELAY = 0.5 # may be slower on CI? TEST_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19" STREAMS: list[tuple[VisionStreamType, CameraConfig, bytes]] = [] -OFFROAD_ALERTS = ['Offroad_StorageMissing', 'Offroad_IsTakingSnapshot'] +OFFROAD_ALERTS = ['Offroad_IsTakingSnapshot', ] DATA: dict[str, capnp.lib.capnp._DynamicStructBuilder] = dict.fromkeys( ["carParams", "deviceState", "pandaStates", "controlsState", "selfdriveState", "liveCalibration", "modelV2", "radarState", "driverMonitoringState", "carState", diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index ef7110e72e..4048daf74a 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -464,15 +472,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot إعادة التشغيل @@ -517,10 +516,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -533,6 +528,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -594,6 +617,61 @@ This is the time after which settings UI closes automatically if user is not int تشغيل وضع الراحة + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + التحقق + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -649,6 +727,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp MAX + + HudRendererSP + + km/h + كم/س + + + mph + ميل/س + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + MAX + + HyundaiSettings @@ -767,6 +902,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -790,6 +937,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1048,6 +1223,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1169,10 +1356,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. يتم تنزيل تحديث لنظام تشغيل جهازك في الخلفية. سيطلَب منك التحديث عندما يصبح جاهزاً للتثبيت. - - NVMe drive not mounted. - محرك NVMe غير مثبَّت. - Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support. @@ -1211,6 +1394,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1597,6 +1784,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1691,6 +1931,10 @@ Warning: You are on a metered connection! Developer المطور + + Display + + SetupWidget @@ -1927,6 +2171,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + السابق + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + السابق + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2309,6 +2631,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2319,6 +2695,10 @@ Warning: You are on a metered connection! Cancel إلغاء + + Favorites + + VisualsPanel @@ -2362,6 +2742,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index dde9241460..3d90dca182 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -464,15 +472,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot Neustart @@ -517,10 +516,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -533,6 +528,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -594,6 +617,61 @@ This is the time after which settings UI closes automatically if user is not int ENTSPANNTER MODUS AN + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + ÜBERPRÜFEN + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -645,6 +723,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp MAX + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + MAX + + HyundaiSettings @@ -759,6 +894,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -782,6 +929,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1040,6 +1215,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1157,10 +1344,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. Ein Update für das Betriebssystem deines Geräts wird im Hintergrund heruntergeladen. Du wirst aufgefordert, das Update zu installieren, sobald es bereit ist. - - NVMe drive not mounted. - NVMe-Laufwerk nicht gemounted. - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 Gerätetemperatur zu hoch. Das System kühlt ab, bevor es startet. Aktuelle interne Komponententemperatur: %1 @@ -1203,6 +1386,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1577,6 +1764,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1671,6 +1911,10 @@ Warning: You are on a metered connection! Developer Entwickler + + Display + + SetupWidget @@ -1909,6 +2153,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + Zurück + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + Zurück + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2291,6 +2613,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2301,6 +2677,10 @@ Warning: You are on a metered connection! Cancel Abbrechen + + Favorites + + VisualsPanel @@ -2344,6 +2724,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 3b53e7f944..284f63ebf3 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -468,15 +476,6 @@ La calibración del retraso de la dirección está completa. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot Reiniciar @@ -521,10 +520,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -537,6 +532,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -598,6 +621,61 @@ This is the time after which settings UI closes automatically if user is not int MODO CHILL + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + VERIFICAR + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -649,6 +727,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp MAX + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + MAX + + HyundaiSettings @@ -763,6 +898,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -786,6 +933,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1044,6 +1219,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1165,10 +1352,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. Se está descargando una actualización del sistema operativo de su dispositivo en segundo plano. Se le pedirá que actualice cuando esté listo para instalarse. - - NVMe drive not mounted. - Unidad NVMe no montada. - Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support. @@ -1207,6 +1390,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1581,6 +1768,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1675,6 +1915,10 @@ Warning: You are on a metered connection! Developer Desarrollador + + Display + + SetupWidget @@ -1911,6 +2155,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2293,6 +2615,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2303,6 +2679,10 @@ Warning: You are on a metered connection! Cancel Cancelar + + Favorites + + VisualsPanel @@ -2346,6 +2726,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index 45705defd9..c6c998b723 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -464,15 +472,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot Redémarrer @@ -517,10 +516,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -533,6 +528,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -594,6 +617,61 @@ This is the time after which settings UI closes automatically if user is not int MODE DÉTENTE ACTIVÉ + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + VÉRIFIER + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -645,6 +723,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp MAX + + HudRendererSP + + km/h + km/h + + + mph + mi/h + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + MAX + + HyundaiSettings @@ -759,6 +894,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -782,6 +929,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1040,6 +1215,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1161,10 +1348,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. Une mise à jour du système d'exploitation de votre appareil est en cours de téléchargement en arrière-plan. Vous serez invité à effectuer la mise à jour lorsqu'elle sera prête à être installée. - - NVMe drive not mounted. - Le disque NVMe n'est pas monté. - Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support. @@ -1203,6 +1386,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1577,6 +1764,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1671,6 +1911,10 @@ Warning: You are on a metered connection! Developer Dév. + + Display + + SetupWidget @@ -1907,6 +2151,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + Retour + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + Retour + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2289,6 +2611,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2299,6 +2675,10 @@ Warning: You are on a metered connection! Cancel Annuler + + Favorites + + VisualsPanel @@ -2342,6 +2722,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 3faa183954..b6a29a867c 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -468,15 +476,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot 再起動 @@ -521,10 +520,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -537,6 +532,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -598,6 +621,61 @@ This is the time after which settings UI closes automatically if user is not int CHILLモード + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + 確認 + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -648,6 +726,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp 最大速度 + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + 最大速度 + + HyundaiSettings @@ -761,6 +896,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -784,6 +931,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1042,6 +1217,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1159,10 +1346,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. オペレーティングシステムがバックグラウンドでダウンロードされています。インストールの準備が整うと更新を促されます。 - - NVMe drive not mounted. - SSDドライブ(NVMe)がマウントされていません。 - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 デバイスの温度が高すぎるためシステム起動前の冷却中です。現在のデバイス内部温度: %1 @@ -1205,6 +1388,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1576,6 +1763,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1670,6 +1910,10 @@ Warning: You are on a metered connection! Developer 開発 + + Display + + SetupWidget @@ -1906,6 +2150,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + 戻る + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + 戻る + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2288,6 +2610,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2298,6 +2674,10 @@ Warning: You are on a metered connection! Cancel キャンセル + + Favorites + + VisualsPanel @@ -2341,6 +2721,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 756cad6045..f3ec6b69e8 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -134,14 +134,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - 밝기 - - - Overrides the brightness of the device. - 기기 밝기를 직접 설정합니다. - Auto (Dark) 자동 (어둡게) @@ -150,6 +142,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto 자동 + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -254,6 +254,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. 빠른 부팅 모드는 업데이트가 비활성화되어 있어야 합니다.<br>먼저 소프트웨어 패널에서 '업데이트 비활성화'를 활성화하세요. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -472,16 +480,6 @@ Steering lag calibration is complete. Wake-Up Behavior 깨우기 동작 - - Interactivity Timeout - 상호작용 타임아웃 - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - 설정 화면에 사용자 지정 타임아웃을 적용하세요. -사용자가 화면과 상호작용하지 않으면 설정 화면이 자동으로 닫히는 시간입니다. - Reboot 재부팅 @@ -526,10 +524,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad 항상 오프로드 종료 - - Always Offroad - 항상 오프로드 - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. ⁍ 기본값: 기기가 정상적으로 부팅/절전 모드에서 해제되며, 작동 준비가 완료됩니다. @@ -542,6 +536,35 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. 부팅/절전 모드 후 기기의 상태를 제어합니다. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + 상호작용 타임아웃 + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + 설정 화면에 사용자 지정 타임아웃을 적용하세요. +사용자가 화면과 상호작용하지 않으면 설정 화면이 자동으로 닫히는 시간입니다. + DriveStats @@ -603,6 +626,61 @@ This is the time after which settings UI closes automatically if user is not int 안정 모드 켜짐 + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + 확인 + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -655,6 +733,63 @@ Firehose 모드를 사용하면 훈련 데이터 업로드를 극대화하여 op 최대 + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + 분 + + + mi + + + + ft + + + + AHEAD + + + + MAX + 최대 + + HyundaiSettings @@ -768,6 +903,18 @@ Firehose 모드를 사용하면 훈련 데이터 업로드를 극대화하여 op This platform supports limited MADS settings. 이 플랫폼은 제한된 MADS 설정을 지원합니다. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -791,6 +938,34 @@ Firehose 모드를 사용하면 훈련 데이터 업로드를 극대화하여 op Start the vehicle to check vehicle compatibility. 차량 호환성을 확인하려면 차량 시동을 걸어주세요. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1050,6 +1225,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1167,10 +1354,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. 기기 운영체제 업데이트가 백그라운드에서 다운로드 중입니다. 설치가 준비되면 업데이트를 진행하라는 메시지가 표시됩니다. - - NVMe drive not mounted. - NVMe 드라이브가 마운트되지 않았습니다. - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 기기 온도가 너무 높습니다. 시스템이 시작하기 전에 온도를 낮추고 있습니다. 현재 내부 부품 온도는 %1입니다. @@ -1215,6 +1398,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1590,6 +1777,59 @@ Warning: You are on a metered connection! sunnypilot sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1684,6 +1924,10 @@ Warning: You are on a metered connection! Developer 개발자 + + Display + + SetupWidget @@ -1920,6 +2164,84 @@ Warning: You are on a metered connection! 이 설정들을 변경하려면 항상 오프로드 모드를 활성화하거나 차량 시동을 꺼야 합니다. + + SpeedLimitPolicy + + Back + 뒤로 + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + 뒤로 + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2302,6 +2624,60 @@ Warning: You are on a metered connection! 실험 모드를 활성화하려면 sunnypilot 가감속 제어(알파) 토글을 켜세요. + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2312,6 +2688,10 @@ Warning: You are on a metered connection! Cancel 취소 + + Favorites + + VisualsPanel @@ -2355,6 +2735,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). 앞차를 추적하는 역삼각형 아래에 유용한 측정 지표를 표시합니다. (openpilot 가감속 제어 기능이 있는 차량에만 해당됩니다) + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 0a886d435c..e820f472af 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -468,15 +476,6 @@ A calibração do atraso da direção foi concluída. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot Reiniciar @@ -521,10 +520,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -537,6 +532,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -598,6 +621,61 @@ This is the time after which settings UI closes automatically if user is not int MODO CHILL ON + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + VERIFICAR + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -649,6 +727,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp LIMITE + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + LIMITE + + HyundaiSettings @@ -763,6 +898,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -786,6 +933,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1044,6 +1219,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1161,10 +1348,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. Uma atualização para o sistema operacional do seu dispositivo está sendo baixada em segundo plano. Você será solicitado a atualizar quando estiver pronto para instalar. - - NVMe drive not mounted. - Unidade NVMe não montada. - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 Temperatura do dispositivo muito alta. O sistema está sendo resfriado antes de iniciar. A temperatura atual do componente interno é: %1 @@ -1207,6 +1390,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1581,6 +1768,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1675,6 +1915,10 @@ Warning: You are on a metered connection! Developer Desenvdor + + Display + + SetupWidget @@ -1911,6 +2155,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + Voltar + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + Voltar + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2293,6 +2615,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2303,6 +2679,10 @@ Warning: You are on a metered connection! Cancel Cancelar + + Favorites + + VisualsPanel @@ -2346,6 +2726,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 7b89f2a694..67a5a5eaeb 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -464,15 +472,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot รีบูต @@ -517,10 +516,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -533,6 +528,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -594,6 +617,61 @@ This is the time after which settings UI closes automatically if user is not int คุณกำลังใช้โหมดชิล + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + ตรวจสอบ + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -644,6 +722,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp สูงสุด + + HudRendererSP + + km/h + กม./ชม. + + + mph + ไมล์/ชม. + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + สูงสุด + + HyundaiSettings @@ -757,6 +892,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -780,6 +927,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1038,6 +1213,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1159,10 +1346,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. กำลังดาวน์โหลดอัปเดทสำหรับระบบปฏิบัติการอยู่เบื้องหลัง คุณจะได้รับการแจ้งเตือนเมื่อระบบพร้อมสำหรับการติดตั้ง - - NVMe drive not mounted. - ไม่ได้ติดตั้งไดร์ฟ NVMe - Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support. @@ -1201,6 +1384,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1572,6 +1759,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1666,6 +1906,10 @@ Warning: You are on a metered connection! Developer นักพัฒนา + + Display + + SetupWidget @@ -1902,6 +2146,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + ย้อนกลับ + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + ย้อนกลับ + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2284,6 +2606,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2294,6 +2670,10 @@ Warning: You are on a metered connection! Cancel ยกเลิก + + Favorites + + VisualsPanel @@ -2337,6 +2717,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 0598d08364..923ab69139 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -464,15 +472,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot Yeniden başlat @@ -517,10 +516,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -533,6 +528,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -594,6 +617,61 @@ This is the time after which settings UI closes automatically if user is not int + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + KONTROL ET + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -644,6 +722,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp MAX + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + MAX + + HyundaiSettings @@ -757,6 +892,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -780,6 +927,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1038,6 +1213,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1158,10 +1345,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. - - NVMe drive not mounted. - - Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support. @@ -1200,6 +1383,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1571,6 +1758,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1665,6 +1905,10 @@ Warning: You are on a metered connection! Developer + + Display + + SetupWidget @@ -1901,6 +2145,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2283,6 +2605,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2293,6 +2669,10 @@ Warning: You are on a metered connection! Cancel + + Favorites + + VisualsPanel @@ -2336,6 +2716,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 1f4db2d6d0..3be9377503 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -468,15 +476,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot 重启 @@ -521,10 +520,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -537,6 +532,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -598,6 +621,61 @@ This is the time after which settings UI closes automatically if user is not int 轻松模式运行 + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + 查看 + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -648,6 +726,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp 最高定速 + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + 最高定速 + + HyundaiSettings @@ -761,6 +896,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -784,6 +931,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1042,6 +1217,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1159,10 +1346,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. 一个针对您设备的操作系统更新正在后台下载中。当更新准备好安装时，您将收到提示进行更新。 - - NVMe drive not mounted. - NVMe固态硬盘未被挂载。 - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 设备温度过高。系统正在冷却中，等冷却完毕后才会启动。目前内部组件温度：%1 @@ -1205,6 +1388,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1576,6 +1763,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1670,6 +1910,10 @@ Warning: You are on a metered connection! Developer 开发人员 + + Display + + SetupWidget @@ -1906,6 +2150,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + 返回 + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + 返回 + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2288,6 +2610,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2298,6 +2674,10 @@ Warning: You are on a metered connection! Cancel 取消 + + Favorites + + VisualsPanel @@ -2341,6 +2721,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 1a8784846c..fac29bed62 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -133,14 +133,6 @@ Please use caution when using this feature. Only use the blinker when traffic an Brightness - - Brightness - - - - Overrides the brightness of the device. - - Auto (Dark) @@ -149,6 +141,14 @@ Please use caution when using this feature. Only use the blinker when traffic an Auto + + Global Brightness + + + + Overrides the brightness of the device. This applies to both onroad and offroad screens. + + ConfirmationDialog @@ -250,6 +250,14 @@ This only toggles the visibility of the controls; it does not toggle the actual Quickboot mode requires updates to be disabled.<br>Enable 'Disable Updates' in the Software panel first. + + Enable Copyparty service + + + + Copyparty is a very capable file server, you can use it to download your routes, view your logs and even make some edits on some files from your browser. Requires you to connect to your comma locally via it's IP. + + DevicePanel @@ -468,15 +476,6 @@ Steering lag calibration is complete. Wake-Up Behavior - - Interactivity Timeout - - - - Apply a custom timeout for settings UI. -This is the time after which settings UI closes automatically if user is not interacting with the screen. - - Reboot 重新啟動 @@ -521,10 +520,6 @@ This is the time after which settings UI closes automatically if user is not int Exit Always Offroad - - Always Offroad - - ⁍ Default: Device will boot/wake-up normally & will be ready to engage. @@ -537,6 +532,34 @@ This is the time after which settings UI closes automatically if user is not int Controls state of the device after boot/sleep. + + Onroad Uploads + + + + Enable Always Offroad + + + + + DisplayPanel + + Onroad Screen: Reduced Brightness + + + + Turn off device screen or reduce brightness after driving starts. It automatically brightens again when screen is touched or a visible alert is displayed. + + + + Interactivity Timeout + + + + Apply a custom timeout for settings UI. +This is the time after which settings UI closes automatically if user is not interacting with the screen. + + DriveStats @@ -598,6 +621,61 @@ This is the time after which settings UI closes automatically if user is not int 輕鬆模式 ON + + ExternalStorageControl + + External Storage + + + + Extend your comma device's storage by inserting a USB drive into the aux port. + + + + CHECK + 檢查 + + + MOUNT + + + + UNMOUNT + + + + FORMAT + + + + Are you sure you want to format this drive? This will erase all data. + + + + Format + + + + formatting + + + + insert drive + + + + needs format + + + + mounting + + + + unmounting + + + FirehosePanel @@ -648,6 +726,63 @@ Firehose Mode allows you to maximize your training data uploads to improve openp 最高 + + HudRendererSP + + km/h + km/h + + + mph + mph + + + GREEN +LIGHT + + + + LEAD VEHICLE +DEPARTING + + + + SPEED + + + + LIMIT + + + + Near + + + + km + + + + m + + + + mi + + + + ft + + + + AHEAD + + + + MAX + 最高 + + HyundaiSettings @@ -761,6 +896,18 @@ Firehose Mode allows you to maximize your training data uploads to improve openp This platform supports limited MADS settings. + + Enforce Torque Lateral Control + + + + Enable this to enforce sunnypilot to steer with Torque lateral control. + + + + Customize Params + + LongitudinalPanel @@ -784,6 +931,34 @@ Firehose Mode allows you to maximize your training data uploads to improve openp Start the vehicle to check vehicle compatibility. + + Intelligent Cruise Button Management (ICBM) (Alpha) + + + + When enabled, sunnypilot will attempt to manage the built-in cruise control buttons by emulating button presses for limited longitudinal control. + + + + Smart Cruise Control - Vision + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Smart Cruise Control - Map + + + + Use map data to estimate the appropriate speed to drive through turns ahead. + + + + Speed Limit + + MadsSettings @@ -1042,6 +1217,18 @@ The default software delay value is 0.2 Total Delay: + + Use Lane Turn Desires + + + + Adjust Lane Turn Speed + + + + Set the maximum speed for lane turn desires. Default is 19 %1. + + MultiOptionDialog @@ -1159,10 +1346,6 @@ The default software delay value is 0.2 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. 一個有關操作系統的更新正在後台下載中。當更新準備好安裝時，您將收到提示進行更新。 - - NVMe drive not mounted. - NVMe 固態硬碟未被掛載。 - Device temperature too high. System cooling down before starting. Current internal component temperature: %1 裝置溫度過高。系統正在冷卻中，等冷卻完畢後才會啟動。目前內部組件溫度：%1 @@ -1205,6 +1388,10 @@ The default software delay value is 0.2 %1 + + <b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three. + + OffroadHome @@ -1576,6 +1763,59 @@ Warning: You are on a metered connection! sunnypilot + + None + + + + Fixed + + + + Percent + + + + Car +Only + + + + Map +Only + + + + Car +First + + + + Map +First + + + + Combined +Data + + + + Off + + + + Information + + + + Warning + + + + Assist + + SettingsWindow @@ -1670,6 +1910,10 @@ Warning: You are on a metered connection! Developer 開發人員 + + Display + + SetupWidget @@ -1906,6 +2150,84 @@ Warning: You are on a metered connection! + + SpeedLimitPolicy + + Back + 回上頁 + + + Speed Limit Source + + + + ⦿ Car Only: Use Speed Limit data only from Car + + + + ⦿ Map Only: Use Speed Limit data only from OpenStreetMaps + + + + ⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps + + + + ⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car + + + + ⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps + + + + + SpeedLimitSettings + + Back + 回上頁 + + + Speed Limit + + + + Customize Source + + + + Speed Limit Offset + + + + ⦿ None: No Offset + + + + ⦿ Fixed: Adds a fixed offset [Speed Limit + Offset] + + + + ⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)] + + + + ⦿ Off: Disables the Speed Limit functions. + + + + ⦿ Information: Displays the current road's speed limit. + + + + ⦿ Warning: Provides a warning when exceeding the current road's speed limit. + + + + ⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons. + + + SshControl @@ -2288,6 +2610,60 @@ Warning: You are on a metered connection! + + TorqueLateralControlCustomParams + + Manual Real-Time Tuning + + + + Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values. + + + + Lateral Acceleration Factor + + + + Friction + + + + Real-time and Offline + + + + Offline Only + + + + + TorqueLateralControlSettings + + Self-Tune + + + + Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default. + + + + Less Restrict Settings for Self-Tune (Beta) + + + + Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values. + + + + Enable Custom Tuning + + + + Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within "opendbc/car/torque_data". The values will also be used live when "Manual Real-Time Tuning" toggle is enabled. + + + TreeOptionDialog @@ -2298,6 +2674,10 @@ Warning: You are on a metered connection! Cancel 取消 + + Favorites + + VisualsPanel @@ -2341,6 +2721,91 @@ Warning: You are on a metered connection! Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control). + + Enable Tesla Rainbow Mode + + + + A beautiful rainbow effect on the path the model wants to take. + + + + It + + + + does not + + + + affect driving in any way. + + + + Enable Standstill Timer + + + + Show a timer on the HUD when the car is at a standstill. + + + + Display Road Name + + + + Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name. + + + + Green Traffic Light Alert (Beta) + + + + A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you. + + + + Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly. + + + + Lead Departure Alert (Beta) + + + + A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving. + + + + Speedometer: Always Display True Speed + + + + Always display the true vehicle current speed from wheel speed sensors. + + + + Speedometer: Hide from Onroad Screen + + + + Right + + + + Right && +Bottom + + + + Developer UI + + + + Display real-time parameters and metrics from various sources. + + WiFiPromptWidget diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4b8f79785b..6bca2b7f7f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -55,8 +55,7 @@ void update_state(UIState *s) { } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); - float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; - scene.light_sensor = std::max(100.0f - scale * cam_state.getExposureValPercent(), 0.0f); + scene.light_sensor = std::max(100.0f - cam_state.getExposureValPercent(), 0.0f); } else if (!sm.allAliveAndValid({"wideRoadCameraState"})) { scene.light_sensor = -1; } @@ -205,6 +204,13 @@ void Device::updateBrightness(const UIState &s) { brightness = 0; } + // Onroad Brightness Control +#ifdef SUNNYPILOT + if (awake && s.scene.started && s.scene.onroadScreenOffControl && s.scene.onroadScreenOffTimer == 0) { + brightness = s.scene.onroadScreenOffBrightness * 0.01 * brightness; + } +#endif + if (brightness != last_brightness) { if (!brightness_future.isRunning()) { brightness_future = QtConcurrent::run(Hardware::set_brightness, brightness); diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index c2bb5f77a4..c4a2c0ca11 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -105,10 +105,7 @@ class UIState: # Handle wide road camera state updates if self.sm.updated["wideRoadCameraState"]: cam_state = self.sm["wideRoadCameraState"] - - # Scale factor based on sensor type - scale = 6.0 if cam_state.sensor == 'ar0231' else 1.0 - self.light_sensor = max(100.0 - scale * cam_state.exposureValPercent, 0.0) + self.light_sensor = max(100.0 - cam_state.exposureValPercent, 0.0) elif not self.sm.alive["wideRoadCameraState"] or not self.sm.valid["wideRoadCameraState"]: self.light_sensor = -1 diff --git a/sunnypilot/SConscript b/sunnypilot/SConscript index 3aa302b92b..6b3856cb34 100644 --- a/sunnypilot/SConscript +++ b/sunnypilot/SConscript @@ -1,2 +1,3 @@ SConscript(['modeld/SConscript']) -SConscript(['modeld_v2/SConscript']) \ No newline at end of file +SConscript(['modeld_v2/SConscript']) +SConscript(['selfdrive/locationd/SConscript']) \ No newline at end of file diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py index e69de29bb2..dd9870597d 100644 --- a/sunnypilot/__init__.py +++ b/sunnypilot/__init__.py @@ -0,0 +1,18 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import hashlib + +PARAMS_UPDATE_PERIOD = 3 # seconds + + +def get_file_hash(path: str) -> str: + sha256_hash = hashlib.sha256() + with open(path, "rb") as f: + for byte_block in iter(lambda: f.read(4096), b""): + sha256_hash.update(byte_block) + return sha256_hash.hexdigest() diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 70b590a8b2..f9c4057bab 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -9,6 +9,10 @@ from openpilot.common.params import Params from opendbc.car import structs from opendbc.safety import ALTERNATIVE_EXPERIENCE from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP +from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP + + +MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla") class MadsSteeringModeOnBrake: @@ -17,20 +21,25 @@ class MadsSteeringModeOnBrake: DISENGAGE = 2 -def get_mads_limited_brands(CP: structs.CarParams) -> bool: - return CP.brand in ("rivian", "tesla") +def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool: + if CP.brand == 'rivian': + return True + if CP.brand == 'tesla': + return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS + + return False -def read_steering_mode_param(CP: structs.CarParams, params: Params): - if get_mads_limited_brands(CP): +def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): + if get_mads_limited_brands(CP, CP_SP): return MadsSteeringModeOnBrake.DISENGAGE return params.get("MadsSteeringMode", return_default=True) -def set_alternative_experience(CP: structs.CarParams, params: Params): +def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): enabled = params.get_bool("Mads") - steering_mode = read_steering_mode_param(CP, params) + steering_mode = read_steering_mode_param(CP, CP_SP, params) if enabled: CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS @@ -53,9 +62,12 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p # MADS Partial Support # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms - # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling - mads_partial_support = get_mads_limited_brands(CP) + # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling + mads_partial_support = get_mads_limited_brands(CP, CP_SP) if mads_partial_support: params.put("MadsSteeringMode", 2) params.put_bool("MadsUnifiedEngagementMode", True) + + # no ACC MAIN button for these brands + if CP.brand in MADS_NO_ACC_MAIN_BUTTON: params.remove("MadsMainCruiseAllowed") diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index f16b688358..7eab55e6ea 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -10,7 +10,7 @@ from cereal import log, custom from opendbc.car import structs from opendbc.car.hyundai.values import HyundaiFlags from openpilot.common.params import Params -from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands +from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -27,6 +27,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): self.CP = selfdrive.CP + self.CP_SP = selfdrive.CP_SP self.params = selfdrive.params self.enabled = False @@ -43,14 +44,16 @@ class ModularAssistiveDrivingSystem: if self.CP.brand == "hyundai": if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True + if self.CP.brand == "tesla": + self.allow_always = True - if get_mads_limited_brands(self.CP): + if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON: self.no_main_cruise = True # read params on init self.enabled_toggle = self.params.get_bool("Mads") self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") - self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params) + self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params) self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def read_params(self): @@ -58,6 +61,7 @@ class ModularAssistiveDrivingSystem: self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool: + # ignore `pedalPressed` events caused by gas presses if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator): return True @@ -102,12 +106,13 @@ class ModularAssistiveDrivingSystem: def update_events(self, CS: structs.CarState): if not self.selfdrive.enabled and self.enabled: - if self.events.has(EventName.doorOpen): - self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) - self.transition_paused_state() - if self.events.has(EventName.seatbeltNotLatched): - self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) - self.transition_paused_state() + if CS.standstill: + if self.events.has(EventName.doorOpen): + self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen) + self.transition_paused_state() + if self.events.has(EventName.seatbeltNotLatched): + self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched) + self.transition_paused_state() if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse): self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear) self.transition_paused_state() diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index 536d7720b7..25925937f5 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -4,25 +4,23 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -import time from abc import abstractmethod, ABC -from cereal import messaging -from openpilot.common.gps import get_gps_location_service +import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param +from openpilot.sunnypilot.navd.helpers import coordinate_from_param class BaseMapData(ABC): def __init__(self): self.params = Params() - self.gps_location_service = get_gps_location_service(self.params) - self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service]) + self.sm = messaging.SubMaster(['liveLocationKalman']) self.pm = messaging.PubMaster(['liveMapDataSP']) - self.last_position = coordinate_from_param("LastGPSPosition", self.params) - self.last_altitude = None + self.localizer_valid = False + self.last_bearing = None + self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params) @abstractmethod def update_location(self) -> None: @@ -40,24 +38,12 @@ class BaseMapData(ABC): def get_current_road_name(self) -> str: pass - def get_current_location(self) -> None: - gps = self.sm[self.gps_location_service] - - # ignore the message if the fix is invalid - gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0 - if not gps_ok and self.sm['livePose'].inputsOK: - return None - - # livePose has these data, but aren't on cereal - self.last_position = Coordinate(gps.latitude, gps.longitude) - self.last_altitude = gps.altitude - def publish(self) -> None: speed_limit = self.get_current_speed_limit() next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose']) + mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK live_map_data = mapd_sp_send.liveMapDataSP live_map_data.speedLimitValid = bool(speed_limit > 0) @@ -70,7 +56,6 @@ class BaseMapData(ABC): self.pm.send('liveMapDataSP', mapd_sp_send) def tick(self) -> None: - self.sm.update() - self.get_current_location() + self.sm.update(0) self.update_location() self.publish() diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index da9f4d7771..794f2ef8ff 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -37,15 +37,14 @@ def live_map_data_sp_thread(): def live_map_data_sp_thread_debug(gps_location_service): - _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', gps_location_service]) + _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service]) _sub_master.update() v_ego = _sub_master['carState'].vEgo - long_spl = _sub_master['longitudinalPlanSP'].speedLimit - _policy = Policy.car_state_priority - _resolver = SpeedLimitResolver(_policy) - _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master) - print(_speed_limit, _distance, _source, " <-> ", long_spl) + _resolver = SpeedLimitResolver() + _resolver.policy = Policy.car_state_priority + _resolver.update(v_ego, _sub_master) + print(_resolver.speed_limit, _resolver.distance, _resolver.source) def main(): diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py index 1eaf76dc8d..0662e2d589 100644 --- a/sunnypilot/mapd/live_map_data/osm_map_data.py +++ b/sunnypilot/mapd/live_map_data/osm_map_data.py @@ -5,8 +5,10 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ import json +import math import platform +from cereal import log from openpilot.common.params import Params from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData from openpilot.sunnypilot.navd.helpers import Coordinate @@ -15,19 +17,27 @@ from openpilot.sunnypilot.navd.helpers import Coordinate class OsmMapData(BaseMapData): def __init__(self): super().__init__() - self.params = Params() self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params def update_location(self) -> None: - if self.last_position is None or self.last_altitude is None: + location = self.sm['liveLocationKalman'] + self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + + if self.localizer_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + + if self.last_position is None: return params = { "latitude": self.last_position.latitude, "longitude": self.last_position.longitude, - "altitude": self.last_altitude, } + if self.last_bearing is not None: + params['bearing'] = self.last_bearing + self.mem_params.put("LastGPSPosition", json.dumps(params)) def get_current_speed_limit(self) -> float: diff --git a/sunnypilot/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py index 9f7a8c2a5c..08d17376d6 100755 --- a/sunnypilot/mapd/mapd_installer.py +++ b/sunnypilot/mapd/mapd_installer.py @@ -22,7 +22,7 @@ from openpilot.system.version import is_prebuilt from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR import openpilot.system.sentry as sentry -VERSION = 'v1.10.0' +VERSION = "v1.12.0" URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd" diff --git a/sunnypilot/mapd/tests/__init__.py b/sunnypilot/mapd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/mapd/tests/mapd_hash b/sunnypilot/mapd/tests/mapd_hash new file mode 100644 index 0000000000..0322ea97ca --- /dev/null +++ b/sunnypilot/mapd/tests/mapd_hash @@ -0,0 +1 @@ +fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b \ No newline at end of file diff --git a/sunnypilot/mapd/tests/test_mapd_version.py b/sunnypilot/mapd/tests/test_mapd_version.py new file mode 100644 index 0000000000..5619d2ec29 --- /dev/null +++ b/sunnypilot/mapd/tests/test_mapd_version.py @@ -0,0 +1,19 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.sunnypilot import get_file_hash +from openpilot.sunnypilot.mapd import MAPD_PATH +from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH + + +class TestMapdVersion: + def test_compare_versions(self): + mapd_hash = get_file_hash(MAPD_PATH) + + with open(MAPD_HASH_PATH) as f: + current_hash = f.read().strip() + + assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash" diff --git a/sunnypilot/mapd/update_version.py b/sunnypilot/mapd/update_version.py new file mode 100755 index 0000000000..c5e08b3f8f --- /dev/null +++ b/sunnypilot/mapd/update_version.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import argparse +import os +import re + +from openpilot.sunnypilot import get_file_hash +from openpilot.common.basedir import BASEDIR +from openpilot.sunnypilot.mapd import MAPD_PATH + +MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash") +MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py") + + +def update_mapd_hash(): + mapd_hash = get_file_hash(MAPD_PATH) + + with open(MAPD_HASH_PATH, "w") as f: + f.write(mapd_hash) + + print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}") + + +def get_current_mapd_version(path: str) -> str: + print("[GET CURRENT MAPD VERSION]") + with open(path) as f: + for line in f: + if line.strip().startswith("VERSION"): + # Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces) + match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line) + if match: + ver = match.group(1) + print(f'Current mapd version: "{ver}"') + return ver + else: + print("[ERROR] VERSION line found but no quoted value detected.") + return "" + print("[ERROR] VERSION not found in file!") + return "" + + +def update_mapd_version(ver: str, path: str): + print("[CHANGE CURRENT MAPD VERSION]") + + with open(path) as f: + lines = f.readlines() + + found = False + new_lines = [] + for line in lines: + if not found and line.startswith("VERSION ="): + new_lines.append(f'VERSION = "{ver}"\n') + found = True + new_lines.extend(lines[lines.index(line) + 1:]) + break + else: + new_lines.append(line) + + if not found: + print("[ERROR] VERSION line not found! Aborting without writing.") + return + + with open(path, "w") as f: + f.writelines(new_lines) + + print(f'New mapd version: "{ver}"') + print("[DONE]") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Update mapd version and hash") + parser.add_argument("--new_ver", type=str, help="New mapd version") + args = parser.parse_args() + + if not args.new_ver: + print("Warning: No new mapd version provided. Use --new_ver to specify") + print("Example:") + print(" python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"") + print("Current mapd version and hash will not be updated! (aborted)") + exit(0) + + current_ver = get_current_mapd_version(MAPD_VERSION_PATH) + new_ver = f"{args.new_ver}" + if current_ver == new_ver: + print(f'Proposed mapd version: "{new_ver}"') + confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip() + if confirm != "Y": + print("Current mapd version and hash will not be updated! (aborted)") + exit(0) + + update_mapd_version(new_ver, MAPD_VERSION_PATH) + update_mapd_hash() diff --git a/sunnypilot/mapd/version.py b/sunnypilot/mapd/version.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index dadffc8433..a62c451efd 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -120,23 +121,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2_action.desiredCurvature = desired_curvature # times at X_IDXS according to model plan - PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N - PLAN_T_IDXS[0] = 0.0 - plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() - for xidx in range(1, ModelConstants.IDX_N): - tidx = 0 - # increment tidx until we find an element that's further away than the current xidx - while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: - tidx += 1 - if tidx == ModelConstants.IDX_N - 1: - # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break - PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] - break - # interpolate to find `t` for the current xidx - current_x_val = plan_x[tidx] - next_x_val = plan_x[tidx+1] - p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan') - PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] + PLAN_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py index c7de698f61..ee0eb48684 100644 --- a/sunnypilot/modeld_v2/fill_model_msg.py +++ b/sunnypilot/modeld_v2/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan +from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -118,8 +119,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action (includes lateral planning now) modelV2.action = action - # times at X_IDXS of edges and lines aren't used - LINE_T_IDXS: list[float] = [] + # times at X_IDXS of edges and lines + LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data) # lane lines modelV2.init('laneLines', 4) diff --git a/sunnypilot/models/default_model.py b/sunnypilot/models/default_model.py index 718c910c5c..0260a3c3bc 100755 --- a/sunnypilot/models/default_model.py +++ b/sunnypilot/models/default_model.py @@ -3,6 +3,7 @@ import os import hashlib from openpilot.common.basedir import BASEDIR +from openpilot.sunnypilot import get_file_hash DEFAULT_MODEL_NAME_PATH = os.path.join(BASEDIR, "common", "model.h") MODEL_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "models", "tests", "model_hash") @@ -10,14 +11,6 @@ VISION_ONNX_PATH = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "drivi POLICY_ONNX_PATH = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "driving_policy.onnx") -def get_file_hash(path: str) -> str: - sha256_hash = hashlib.sha256() - with open(path, "rb") as f: - for byte_block in iter(lambda: f.read(4096), b""): - sha256_hash.update(byte_block) - return sha256_hash.hexdigest() - - def update_model_hash(): vision_hash = get_file_hash(VISION_ONNX_PATH) policy_hash = get_file_hash(POLICY_ONNX_PATH) diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index ecf0a39b72..20b94fb611 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -185,3 +185,27 @@ def load_meta_constants(model_metadata): meta = MetaTombRaider return meta + + +# The following method(s) are modeld helper methods +def plan_x_idxs_helper(constants, plan, model_output) -> list[float]: + # times at X_IDXS according to plan. + LINE_T_IDXS = [np.nan] * constants.IDX_N + LINE_T_IDXS[0] = 0.0 + plan_x = model_output['plan'][0, :, plan.POSITION][:, 0].tolist() + for xidx in range(1, constants.IDX_N): + tidx = 0 + # increment tidx until we find an element that's further away than the current xidx + while tidx < constants.IDX_N - 1 and plan_x[tidx + 1] < constants.X_IDXS[xidx]: + tidx += 1 + if tidx == constants.IDX_N - 1: + # if the plan doesn't extend far enough, set plan_t to the max value (10s), then break + LINE_T_IDXS[xidx] = constants.T_IDXS[constants.IDX_N - 1] + break + # interpolate to find `t` for the current xidx + current_x_val = plan_x[tidx] + next_x_val = plan_x[tidx + 1] + p = (constants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs( + next_x_val - current_x_val) > 1e-9 else float('nan') + LINE_T_IDXS[xidx] = p * constants.T_IDXS[tidx + 1] + (1 - p) * constants.T_IDXS[tidx] + return LINE_T_IDXS diff --git a/sunnypilot/models/tests/model_hash b/sunnypilot/models/tests/model_hash index f66baa7e71..33d7d86e28 100644 --- a/sunnypilot/models/tests/model_hash +++ b/sunnypilot/models/tests/model_hash @@ -1 +1 @@ -2ff2f49176a13bc7f856645d785b3b838a5c7ecf7f6cb37699fa0459ebf12453 \ No newline at end of file +70406ab4dd66d0e384734a8a56632ae4a62bc9670c2e630a0f71588c4e212cd8 \ No newline at end of file diff --git a/sunnypilot/models/tests/test_default_model.py b/sunnypilot/models/tests/test_default_model.py index 5067dbaf46..7c2fde70a8 100644 --- a/sunnypilot/models/tests/test_default_model.py +++ b/sunnypilot/models/tests/test_default_model.py @@ -1,4 +1,12 @@ -from openpilot.sunnypilot.models.default_model import get_file_hash, MODEL_HASH_PATH, VISION_ONNX_PATH, POLICY_ONNX_PATH +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from openpilot.sunnypilot import get_file_hash +from openpilot.sunnypilot.models.default_model import MODEL_HASH_PATH, VISION_ONNX_PATH, POLICY_ONNX_PATH import hashlib diff --git a/sunnypilot/selfdrive/assets/images/green_light.png b/sunnypilot/selfdrive/assets/images/green_light.png new file mode 100644 index 0000000000..2da2c13a82 --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/green_light.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3aa5ec9ac1daee6a549e62647d90bcaa66d2485f7df7f386ff902fcfb04c1716 +size 6583 diff --git a/sunnypilot/selfdrive/assets/images/lead_depart.png b/sunnypilot/selfdrive/assets/images/lead_depart.png new file mode 100644 index 0000000000..6030ee67cf --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/lead_depart.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:087db35bd469e85aefe3b45636f11ab3e8b55ceb7bc94ea059cfd9a69c2f338f +size 8914 diff --git a/sunnypilot/selfdrive/assets/img_minus_arrow_down.png b/sunnypilot/selfdrive/assets/img_minus_arrow_down.png new file mode 100644 index 0000000000..250f640585 --- /dev/null +++ b/sunnypilot/selfdrive/assets/img_minus_arrow_down.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0f4a0bf26cfd2c64939759d43bee839aadfd017a6f74065095a27b03615e55a4 +size 26627 diff --git a/sunnypilot/selfdrive/assets/img_plus_arrow_up.png b/sunnypilot/selfdrive/assets/img_plus_arrow_up.png new file mode 100644 index 0000000000..14c1529da3 --- /dev/null +++ b/sunnypilot/selfdrive/assets/img_plus_arrow_up.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e451ff31e9b3f144822ab282b8e47cd5a603ca59ff94bd1849271181b86c6b1 +size 29220 diff --git a/sunnypilot/selfdrive/assets/offroad/icon_display.png b/sunnypilot/selfdrive/assets/offroad/icon_display.png new file mode 100644 index 0000000000..547977f522 --- /dev/null +++ b/sunnypilot/selfdrive/assets/offroad/icon_display.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:614767308d435d165a9ab78e3eac22ee15697d94066df8200ef32afb33ef8d60 +size 5698 diff --git a/sunnypilot/selfdrive/car/car_specific.py b/sunnypilot/selfdrive/car/car_specific.py index 7e6f300d73..97ce26429c 100644 --- a/sunnypilot/selfdrive/car/car_specific.py +++ b/sunnypilot/selfdrive/car/car_specific.py @@ -18,9 +18,9 @@ GearShifter = structs.CarState.GearShifter class CarSpecificEventsSP: - def __init__(self, CP: structs.CarParams, params): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP): self.CP = CP - self.params = params + self.CP_SP = CP_SP self.low_speed_alert = False @@ -42,4 +42,10 @@ class CarSpecificEventsSP: if self.low_speed_alert: events.add(EventName.belowSteerSpeed) + elif self.CP.brand == 'toyota': + if self.CP.openpilotLongitudinalControl: + if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor: + if events.has(EventName.resumeRequired): + events.remove(EventName.resumeRequired) + return events_sp diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py index f443aeee0e..3691c35972 100644 --- a/sunnypilot/selfdrive/car/cruise_ext.py +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -6,20 +6,64 @@ See the LICENSE.md file in the root directory for more details. """ import numpy as np -from cereal import car +from cereal import car, custom +from opendbc.car import structs +from openpilot.common.constants import CV from openpilot.common.params import Params +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target ButtonType = car.CarState.ButtonEvent.Type +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState + +CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, + ButtonType.setCruise: 0, ButtonType.resumeCruise: 0, + ButtonType.cancel: 0, ButtonType.mainCruise: 0} + +V_CRUISE_MIN = 8 +V_CRUISE_MAX = 145 +V_CRUISE_UNSET = 255 + + +def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None: + # increment timer for buttons still pressed + for k in button_timers: + if button_timers[k] > 0: + button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in button_timers: + # Start/end timer and store current state on change of button pressed + button_timers[b.type.raw] = 1 if b.pressed else 0 class VCruiseHelperSP: - def __init__(self) -> None: + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None: + self.CP = CP + self.CP_SP = CP_SP + self.v_cruise_kph = V_CRUISE_UNSET + self.v_cruise_cluster_kph = V_CRUISE_UNSET self.params = Params() + self.v_cruise_min = 0 + self.enabled_prev = False self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True) + self.enable_button_timers = CRUISE_BUTTON_TIMER + + # Speed Limit Assist + self.sla_state = SpeedLimitAssistState.disabled + self.prev_sla_state = SpeedLimitAssistState.disabled + self.has_speed_limit = False + self.speed_limit_final_last = 0. + self.speed_limit_final_last_kph = 0. + self.prev_speed_limit_final_last_kph = 0. + self.req_plus = False + self.req_minus = False + def read_custom_set_speed_params(self) -> None: self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True) @@ -39,3 +83,56 @@ class VCruiseHelperSP: v_cruise_delta = v_cruise_delta * actual_increment return round_to_nearest, v_cruise_delta + + def get_minimum_set_speed(self, is_metric: bool) -> None: + if self.CP_SP.pcmCruiseSpeed: + self.v_cruise_min = V_CRUISE_MIN + return + + self.v_cruise_min = get_minimum_set_speed(is_metric) + + def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool: + # special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise + if not self.CP_SP.pcmCruiseSpeed: + update_manual_button_timers(CS, self.enable_button_timers) + button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers) + + if enabled and not self.enabled_prev: + self.enabled_prev = not button_pressed + enabled = False + elif not enabled: + self.enabled_prev = enabled + + return enabled and self.enabled_prev + + return enabled + + def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None: + resolver = LP_SP.speedLimit.resolver + self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid + self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast + self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH + self.sla_state = LP_SP.speedLimit.assist.state + self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS, + self.speed_limit_final_last, is_metric) + + @property + def update_speed_limit_final_last_changed(self) -> bool: + return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph) + + def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool: + if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive: + if button_type == ButtonType.decelCruise and self.req_minus: + return True + if button_type == ButtonType.accelCruise and self.req_plus: + return True + + return False + + def update_speed_limit_assist_v_cruise_non_pcm(self) -> None: + if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or + self.update_speed_limit_final_last_changed): + self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX) + + self.prev_sla_state = self.sla_state + self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py new file mode 100644 index 0000000000..1f491e0f58 --- /dev/null +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py @@ -0,0 +1,127 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import car, custom +from opendbc.car import structs, apply_hysteresis +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL +from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed +from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers + +LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource +State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState +SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState + +ALLOWED_SPEED_THRESHOLD = 1.8 # m/s, ~4 MPH +HYST_GAP = 0.0 # currently disabled; TODO-SP: might need to be brand-specific +INACTIVE_TIMER = 0.4 + + +SEND_BUTTONS = { + State.increasing: SendButtonState.increase, + State.decreasing: SendButtonState.decrease, +} + + +class IntelligentCruiseButtonManagement: + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP): + self.CP = CP + self.CP_SP = CP_SP + + self.v_target = 0 + self.v_cruise_cluster = 0 + self.v_cruise_min = 0 + self.cruise_button = SendButtonState.none + self.state = State.inactive + self.pre_active_timer = 0 + + self.is_ready = False + self.is_ready_prev = False + self.v_target_ms_last = 0.0 + self.is_metric = False + + self.cruise_button_timers = CRUISE_BUTTON_TIMER + + @property + def v_cruise_equal(self) -> bool: + return self.v_target == self.v_cruise_cluster + + def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None: + speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + + self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv) + + self.v_target = round(self.v_target_ms_last * speed_conv) + self.v_cruise_min = get_minimum_set_speed(self.is_metric) + self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv) + + def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState: + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + # HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE + if self.state != State.inactive: + if not self.is_ready: + self.state = State.inactive + + else: + # PRE_ACTIVE + if self.state == State.preActive: + if self.pre_active_timer <= 0: + if self.v_cruise_equal: + self.state = State.holding + + elif self.v_target > self.v_cruise_cluster: + self.state = State.increasing + + elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min: + self.state = State.decreasing + + # HOLDING + elif self.state == State.holding: + if not self.v_cruise_equal: + self.state = State.preActive + + # ACCELERATING + elif self.state == State.increasing: + if self.v_target <= self.v_cruise_cluster: + self.state = State.holding + + # DECELERATING + elif self.state == State.decreasing: + if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min: + self.state = State.holding + + # INACTIVE + elif self.state == State.inactive: + if self.is_ready and not self.is_ready_prev: + self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL) + self.state = State.preActive + + send_button = SEND_BUTTONS.get(self.state, SendButtonState.none) + + return send_button + + def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None: + update_manual_button_timers(CS, self.cruise_button_timers) + + ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume + button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers) + + self.is_ready = ready and not button_pressed + + def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None: + if self.CP_SP.pcmCruiseSpeed: + return + + self.is_metric = is_metric + + self.update_calculations(CS, LP_SP) + self.update_readiness(CS, CC) + + self.cruise_button = self.update_state_machine() + + self.is_ready_prev = self.is_ready diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py new file mode 100644 index 0000000000..eb4bbdecb5 --- /dev/null +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py @@ -0,0 +1,8 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +def get_minimum_set_speed(is_metric: bool) -> int: + return 30 if is_metric else 20 diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index cd7a24b63b..ed0c11fb19 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -22,8 +22,18 @@ def log_fingerprint(CP: structs.CarParams) -> None: sentry.capture_fingerprint(CP.carFingerprint, CP.brand) -def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, CP_SP: structs.CarParamsSP, - params: Params = None, enabled: bool = False) -> None: +def _enforce_torque_lateral_control(CP: structs.CarParams, params: Params = None, enabled: bool = False) -> bool: + if params is None: + params = Params() + + if CP.steerControlType != structs.CarParams.SteerControlType.angle: + enabled = params.get_bool("EnforceTorqueControl") + + return enabled + + +def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP, + params: Params = None, enabled: bool = False) -> bool: if params is None: params = Params() @@ -35,19 +45,35 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs if nnlc_model_name != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle: enabled = params.get_bool("NeuralNetworkLateralControl") - if enabled: - CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning) - CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match + return enabled + + +def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None: + if params is None: + params = Params() + + icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement") + if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl: + CP_SP.pcmCruiseSpeed = False + + +def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, enforce_torque: bool, nnlc_enabled: bool) -> None: + if nnlc_enabled or enforce_torque: + CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning) + def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: CP = CI.CP CP_SP = CI.CP_SP - _initialize_neural_network_lateral_control(CI, CP, CP_SP, params) + enforce_torque = _enforce_torque_lateral_control(CP, params) + nnlc_enabled = _initialize_neural_network_lateral_control(CP, CP_SP, params) + _initialize_intelligent_cruise_button_management(CP, CP_SP, params) + _initialize_torque_lateral_control(CI, CP, enforce_torque, nnlc_enabled) def initialize_params(params) -> list[dict[str, Any]]: diff --git a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py index 3bed8534ef..a1f7f8f027 100644 --- a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py +++ b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -11,7 +11,8 @@ ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestCustomAccIncrements(TestVCruiseHelper): def setup_method(self): TestVCruiseHelper.setup_method(self) diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index a096b7dc8c..8caeeaeabc 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -10,7 +10,6 @@ from cereal import log, custom from opendbc.car import structs from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog -from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral @@ -19,7 +18,6 @@ class ControlsExt: self.CP = CP self.params = params self.blinker_pause_lateral = BlinkerPauseLateral() - self.param_store = ParamStore(self.CP) self.get_params_sp() cloudlog.info("controlsd_ext is waiting for CarParamsSP") @@ -30,7 +28,6 @@ class ControlsExt: self.pm_services_ext = ['carControlSP'] def get_params_sp(self) -> None: - self.param_store.update(self.params) self.blinker_pause_lateral.get_params() def get_lat_active(self, sm: messaging.SubMaster) -> bool: @@ -73,7 +70,7 @@ class ControlsExt: # MADS state CC_SP.mads = sm['selfdriveStateSP'].mads - CC_SP.params = self.param_store.param_list + CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement return CC_SP diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py new file mode 100644 index 0000000000..0135b2806c --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -0,0 +1,58 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from cereal import messaging, custom + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP + +TRIGGER_THRESHOLD = 30 + + +class E2EAlertsHelper: + def __init__(self): + self._params = Params() + self._frame = -1 + + self.green_light_alert = False + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + self.lead_depart_alert = False + self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") + + def _read_params(self) -> None: + if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") + + self._frame += 1 + + def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: + self._read_params() + + if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled): + return + + CS = sm['carState'] + CC = sm['carControl'] + + model_x = sm['modelV2'].position.x + max_idx = len(model_x) - 1 + has_lead = sm['radarState'].leadOne.status + lead_vRel: float = sm['radarState'].leadOne.vRel + + # Green light alert + self.green_light_alert = (self.green_light_alert_enabled and model_x[max_idx] > TRIGGER_THRESHOLD + and not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled) + + # Lead Departure Alert + self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30 + and has_lead and lead_vRel > 1 and not CS.gasPressed) + + if self.green_light_alert or self.lead_depart_alert: + events_sp.add(custom.OnroadEventSP.EventName.e2eChime) diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index 6e134276e5..39525b3b8e 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -6,11 +6,13 @@ See the LICENSE.md file in the root directory for more details. """ from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import LatControlTorqueExtOverride -class LatControlTorqueExt(NeuralNetworkLateralControl): +class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride): def __init__(self, lac_torque, CP, CP_SP, CI): - super().__init__(lac_torque, CP, CP_SP, CI) + NeuralNetworkLateralControl.__init__(self, lac_torque, CP, CP_SP, CI) + LatControlTorqueExtOverride.__init__(self, CP) def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 1965d50b51..609d2f78b6 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -58,7 +58,8 @@ class LatControlTorqueExtBase: self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space() self._ff = 0.0 - self._pid = PIDController(0.0, 0.0, k_f=0.0) + self._pid = PIDController(self.torque_params.kp, self.torque_params.ki, + k_f=self.torque_params.kf) self._pid_log = None self._setpoint = 0.0 self._measurement = 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py new file mode 100644 index 0000000000..f07a7292c1 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py @@ -0,0 +1,34 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from openpilot.common.params import Params + + +class LatControlTorqueExtOverride: + def __init__(self, CP): + self.CP = CP + self.params = Params() + self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl") # only during init + self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled") + self.frame = -1 + + def update_override_torque_params(self, torque_params) -> bool: + if not self.enforce_torque_control_toggle: + return False + + self.frame += 1 + if self.frame % 300 == 0: + self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled") + + if not self.torque_override_enabled: + return False + + torque_params.latAccelFactor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True)) + torque_params.friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True)) + return True + + return False diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 6952a97f1f..4c2443f253 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,16 +7,34 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs +from openpilot.common.constants import CV +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState +LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): + self.events_sp = EventsSP() + self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) + self.scc = SmartCruiseControl() + self.resolver = SpeedLimitResolver() + self.sla = SpeedLimitAssist(CP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None + self.source = LongitudinalPlanSource.cruise + self.e2e_alerts_helper = E2EAlertsHelper() + + self.output_v_target = 0. + self.output_a_target = 0. @property def mlsim(self) -> bool: @@ -29,8 +47,40 @@ class LongitudinalPlannerSP: return self.dec.mode() + def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: + CS = sm['carState'] + v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) + v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS + + long_enabled = sm['carControl'].enabled + long_override = sm['carControl'].cruiseControl.override + + # Smart Cruise Control + self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) + + # Speed Limit Resolver + self.resolver.update(v_ego, sm) + + # Speed Limit Assist + has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid + self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit, + self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp) + + targets = { + LongitudinalPlanSource.cruise: (v_cruise, a_ego), + LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), + LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target), + LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target), + } + + self.source = min(targets, key=lambda k: targets[k][0]) + self.output_v_target, self.output_a_target = targets[self.source] + return self.output_v_target, self.output_a_target + def update(self, sm: messaging.SubMaster) -> None: + self.events_sp.clear() self.dec.update(sm) + self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') @@ -38,6 +88,10 @@ class LongitudinalPlannerSP: plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlanSP = plan_sp_send.longitudinalPlanSP + longitudinalPlanSP.longitudinalPlanSource = self.source + longitudinalPlanSP.vTarget = float(self.output_v_target) + longitudinalPlanSP.aTarget = float(self.output_a_target) + longitudinalPlanSP.events = self.events_sp.to_msg() # Dynamic Experimental Control dec = longitudinalPlanSP.dec @@ -45,4 +99,47 @@ class LongitudinalPlannerSP: dec.enabled = self.dec.enabled() dec.active = self.dec.active() + # Smart Cruise Control + smartCruiseControl = longitudinalPlanSP.smartCruiseControl + # Vision Control + sccVision = smartCruiseControl.vision + sccVision.state = self.scc.vision.state + sccVision.vTarget = float(self.scc.vision.output_v_target) + sccVision.aTarget = float(self.scc.vision.output_a_target) + sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc) + sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc) + sccVision.enabled = self.scc.vision.is_enabled + sccVision.active = self.scc.vision.is_active + # Map Control + sccMap = smartCruiseControl.map + sccMap.state = self.scc.map.state + sccMap.vTarget = float(self.scc.map.output_v_target) + sccMap.aTarget = float(self.scc.map.output_a_target) + sccMap.enabled = self.scc.map.is_enabled + sccMap.active = self.scc.map.is_active + + # Speed Limit + speedLimit = longitudinalPlanSP.speedLimit + resolver = speedLimit.resolver + resolver.speedLimit = float(self.resolver.speed_limit) + resolver.speedLimitLast = float(self.resolver.speed_limit_last) + resolver.speedLimitFinal = float(self.resolver.speed_limit_final) + resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last) + resolver.speedLimitValid = self.resolver.speed_limit_valid + resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid + resolver.speedLimitOffset = float(self.resolver.speed_limit_offset) + resolver.distToSpeedLimit = float(self.resolver.distance) + resolver.source = self.resolver.source + assist = speedLimit.assist + assist.state = self.sla.state + assist.enabled = self.sla.is_enabled + assist.active = self.sla.is_active + assist.vTarget = float(self.sla.output_v_target) + assist.aTarget = float(self.sla.output_a_target) + + # E2E Alerts + e2eAlerts = longitudinalPlanSP.e2eAlerts + e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert + e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/param_store.py b/sunnypilot/selfdrive/controls/lib/param_store.py deleted file mode 100644 index 65a0175340..0000000000 --- a/sunnypilot/selfdrive/controls/lib/param_store.py +++ /dev/null @@ -1,44 +0,0 @@ -""" -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -This file is part of sunnypilot and is licensed under the MIT License. -See the LICENSE.md file in the root directory for more details. -""" -from cereal import custom -from opendbc.car import structs -from openpilot.common.params import Params - -from sunnypilot.sunnylink.utils import get_param_as_byte - - -class ParamStore: - keys: list[str] - _params: dict[str, custom.CarControlSP.Param] - - def __init__(self, CP: structs.CarParams): - universal_params: list[str] = [] - brand_params: list[str] = [] - - self.keys = universal_params + brand_params - self._params = {} - - self.frame = 0 - - def update(self, params: Params) -> None: - self.frame += 1 - if self.frame % 300 != 0: - return - - for key in self.keys: - param_type = params.get_type(key).name.lower() # Using string instead of number because its "loose" dependency, and could change by OP at anytime. - - # Over engineering opportunity: It's possible this conversion is slow, we may check the value as params returns it for cache purposes. Not today. - param_value = get_param_as_byte(key, params) - if (existing_param := self._params.get(key)) is not None and existing_param.value == param_value: - continue - - self._params[key] = custom.CarControlSP.Param(key=key, value=param_value, type=param_type) - - @property - def param_list(self) -> list[custom.CarControlSP.Param]: - return [v for k,v in self._params.items()] diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py new file mode 100644 index 0000000000..271f49dcc2 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py @@ -0,0 +1,9 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.common.constants import CV + +MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py new file mode 100644 index 0000000000..c7f11a1bb2 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py @@ -0,0 +1,261 @@ +import json +import math +import platform + +from cereal import custom +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V + +MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState + +ACTIVE_STATES = (MapState.turning, ) +ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES) + +R = 6373000.0 # approximate radius of earth in meters +TO_RADIANS = math.pi / 180 +TO_DEGREES = 180 / math.pi +TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some +TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit +TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps + # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot. + # The value is multiplied against the target velocity to determine the additional distance. This is + # done to keep the distance calculations consistent but results in the offset actually being less + # time than specified depending on how much of a speed differential there is between v_ego and the + # target velocity. + + +def velocities_from_param(param: str, params: Params): + if params is None: + params = Params() + + json_str = params.get(param) + if json_str is None: + return None + + velocities = json.loads(json_str) + + return velocities + + +def calculate_accel(t, target_jerk, a_ego): + return a_ego + target_jerk * t + + +def calculate_velocity(t, target_jerk, a_ego, v_ego): + return v_ego + a_ego * t + target_jerk/2 * (t ** 2) + + +def calculate_distance(t, target_jerk, a_ego, v_ego): + return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) + + +# points should be in radians +# output is meters +def distance_to_point(ax, ay, bx, by): + a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + + return R * c # in meters + + +class SmartCruiseControlMap: + v_target: float = 0 + a_target: float = 0. + v_ego: float = 0. + a_ego: float = 0. + output_v_target: float = V_CRUISE_UNSET + output_a_target: float = 0. + + def __init__(self): + self.params = Params() + self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params + self.enabled = self.params.get_bool("SmartCruiseControlMap") + self.long_enabled = False + self.long_override = False + self.is_enabled = False + self.is_active = False + self.state = MapState.disabled + self.v_cruise = 0 + self.target_lat = 0.0 + self.target_lon = 0.0 + self.frame = -1 + + self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) + self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or [] + + def get_v_target_from_control(self) -> float: + if self.is_active: + return max(self.v_target, MIN_V) + + return V_CRUISE_UNSET + + def get_a_target_from_control(self) -> float: + return self.a_ego + + def update_params(self): + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.enabled = self.params.get_bool("SmartCruiseControlMap") + + def update_calculations(self) -> None: + self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) + lat = self.last_position.latitude + lon = self.last_position.longitude + + self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or [] + + if self.last_position is None or self.target_velocities is None: + return + + min_dist = 1000 + min_idx = 0 + distances = [] + + # find our location in the path + for i in range(len(self.target_velocities)): + target_velocity = self.target_velocities[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) + distances.append(d) + if d < min_dist: + min_dist = d + min_idx = i + + # only look at values from our current position forward + forward_points = self.target_velocities[min_idx:] + forward_distances = distances[min_idx:] + + # find velocities that we are within the distance we need to adjust for + valid_velocities = [] + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > self.v_ego: + continue + + d = forward_distances[i] + + a_diff = (self.a_ego - TARGET_ACCEL) + accel_t = abs(a_diff / TARGET_JERK) + min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego) + + max_d = 0 + if tv > min_accel_v: + # calculate time needed based on target jerk + a = 0.5 * TARGET_JERK + b = self.a_ego + c = self.v_ego - tv + t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a + t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a + if not isinstance(t_a, complex) and t_a > 0: + t = t_a + else: + t = t_b + if isinstance(t, complex): + continue + + max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego) + else: + t = accel_t + max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego) + + # calculate additional time needed based on target accel + t = abs((min_accel_v - tv) / TARGET_ACCEL) + max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v) + + if d < max_d + tv * TARGET_OFFSET: + valid_velocities.append((float(tv), tlat, tlon)) + + # Find the smallest velocity we need to adjust for + min_v = 100.0 + target_lat = 0.0 + target_lon = 0.0 + for tv, lat, lon in valid_velocities: + if tv < min_v: + min_v = tv + target_lat = lat + target_lon = lon + + if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0): + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > self.v_ego: + continue + + if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target: + return + + # not found so let's reset + self.v_target = 0.0 + self.target_lat = 0.0 + self.target_lon = 0.0 + + self.v_target = min_v + self.target_lat = target_lat + self.target_lon = target_lon + + def _update_state_machine(self) -> tuple[bool, bool]: + # ENABLED, TURNING + if self.state != MapState.disabled: + if not self.long_enabled or not self.enabled: + self.state = MapState.disabled + elif self.long_override: + self.state = MapState.overriding + + else: + # ENABLED + if self.state == MapState.enabled: + if self.v_cruise > self.v_target != 0: + self.state = MapState.turning + + # TURNING + elif self.state == MapState.turning: + if self.v_cruise <= self.v_target or self.v_target == 0: + self.state = MapState.enabled + + # OVERRIDING + elif self.state == MapState.overriding: + if not self.long_override: + if self.v_cruise > self.v_target != 0: + self.state = MapState.turning + else: + self.state = MapState.enabled + + # DISABLED + elif self.state == MapState.disabled: + if self.long_enabled and self.enabled: + if self.long_override: + self.state = MapState.overriding + else: + self.state = MapState.enabled + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None: + self.long_enabled = long_enabled + self.long_override = long_override + self.v_ego = v_ego + self.a_ego = a_ego + self.v_cruise = v_cruise + + self.update_params() + self.update_calculations() + + self.is_enabled, self.is_active = self._update_state_machine() + + self.output_v_target = self.get_v_target_from_control() + self.output_a_target = self.get_a_target_from_control() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py new file mode 100644 index 0000000000..4ca45202fc --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py @@ -0,0 +1,19 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import cereal.messaging as messaging +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap + + +class SmartCruiseControl: + def __init__(self): + self.vision = SmartCruiseControlVision() + self.map = SmartCruiseControlMap() + + def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None: + self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise) + self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py new file mode 100644 index 0000000000..537f0033f0 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py @@ -0,0 +1,58 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import platform + +from cereal import custom +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap + +MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState + + +class TestSmartCruiseControlMap: + + def setup_method(self): + self.params = Params() + self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params + self.reset_params() + self.scc_m = SmartCruiseControlMap() + + def reset_params(self): + self.params.put_bool("SmartCruiseControlMap", True) + + # TODO-SP: mock data from gpsLocation + self.params.put("LastGPSPosition", "{}") + self.params.put("MapTargetVelocities", "{}") + + def test_initial_state(self): + assert self.scc_m.state == VisionState.disabled + assert not self.scc_m.is_active + assert self.scc_m.output_v_target == V_CRUISE_UNSET + assert self.scc_m.output_a_target == 0. + + def test_system_disabled(self): + self.params.put_bool("SmartCruiseControlMap", False) + self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap") + + for _ in range(int(10. / DT_MDL)): + self.scc_m.update(True, False, 0., 0., 0.) + assert self.scc_m.state == VisionState.disabled + assert not self.scc_m.is_active + + def test_disabled(self): + for _ in range(int(10. / DT_MDL)): + self.scc_m.update(False, False, 0., 0., 0.) + assert self.scc_m.state == VisionState.disabled + + def test_transition_disabled_to_enabled(self): + for _ in range(int(10. / DT_MDL)): + self.scc_m.update(True, False, 0., 0., 0.) + assert self.scc_m.state == VisionState.enabled + + # TODO-SP: mock data from modelV2 to test other states diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py new file mode 100644 index 0000000000..9f6efffb55 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py @@ -0,0 +1,104 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +import cereal.messaging as messaging +from cereal import custom, log +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision + +VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState + + +def generate_modelV2(): + model = messaging.new_message('modelV2') + position = log.XYZTData.new_message() + speed = 30 + position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + orientation = log.XYZTData.new_message() + curvature = 0.05 + orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS] + orientation.y = [0.0 for _ in ModelConstants.T_IDXS] + model.modelV2.orientation = orientation + orientationRate = log.XYZTData.new_message() + orientationRate.z = [float(z) for z in ModelConstants.T_IDXS] + model.modelV2.orientationRate = orientationRate + velocity = log.XYZTData.new_message() + velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + velocity.x[0] = float(speed) # always start at current speed + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)] + model.modelV2.acceleration = acceleration + + return model + + +def generate_carState(): + car_state = messaging.new_message('carState') + speed = 30 + v_cruise = 50 + car_state.carState.vEgo = float(speed) + car_state.carState.standstill = False + car_state.carState.vCruise = float(v_cruise * 3.6) + + return car_state + + +def generate_controlsState(): + controls_state = messaging.new_message('controlsState') + controls_state.controlsState.curvature = 0.05 + + return controls_state + + +class TestSmartCruiseControlVision: + + def setup_method(self): + self.params = Params() + self.reset_params() + self.scc_v = SmartCruiseControlVision() + + mdl = generate_modelV2() + cs = generate_carState() + controls_state = generate_controlsState() + self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState} + + def reset_params(self): + self.params.put_bool("SmartCruiseControlVision", True) + + def test_initial_state(self): + assert self.scc_v.state == VisionState.disabled + assert not self.scc_v.is_active + assert self.scc_v.output_v_target == V_CRUISE_UNSET + assert self.scc_v.output_a_target == 0. + + def test_system_disabled(self): + self.params.put_bool("SmartCruiseControlVision", False) + self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision") + + for _ in range(int(10. / DT_MDL)): + self.scc_v.update(self.sm, True, False, 0., 0., 0.) + assert self.scc_v.state == VisionState.disabled + assert not self.scc_v.is_active + + def test_disabled(self): + for _ in range(int(10. / DT_MDL)): + self.scc_v.update(self.sm, False, False, 0., 0., 0.) + assert self.scc_v.state == VisionState.disabled + + def test_transition_disabled_to_enabled(self): + for _ in range(int(10. / DT_MDL)): + self.scc_v.update(self.sm, True, False, 0., 0., 0.) + assert self.scc_v.state == VisionState.enabled + + # TODO-SP: mock modelV2 data to test other states diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py new file mode 100644 index 0000000000..d144178e93 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py @@ -0,0 +1,203 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +import cereal.messaging as messaging +from cereal import custom +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V + +VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState + +ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving) +ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES) + +_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state. +_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops. + +_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state. + +_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state. +_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle. + +_A_LAT_REG_MAX = 2. # Maximum lateral acceleration + +_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting. + +# Lookup table for the minimum smooth deceleration during the ENTERING state +# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. +_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state +_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead + +# Lookup table for the acceleration for the TURNING state +# depending on the current lateral acceleration of the vehicle. +_TURNING_ACC_V = [0.5, 0., -0.4] # acc value +_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc + +_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn. + + +class SmartCruiseControlVision: + v_target: float = 0 + a_target: float = 0. + v_ego: float = 0. + a_ego: float = 0. + output_v_target: float = V_CRUISE_UNSET + output_a_target: float = 0. + + def __init__(self): + self.params = Params() + self.frame = -1 + self.long_enabled = False + self.long_override = False + self.is_enabled = False + self.is_active = False + self.enabled = self.params.get_bool("SmartCruiseControlVision") + self.v_cruise_setpoint = 0. + + self.state = VisionState.disabled + self.current_lat_acc = 0. + self.max_pred_lat_acc = 0. + + def get_a_target_from_control(self) -> float: + return self.a_target + + def get_v_target_from_control(self) -> float: + if self.is_active: + return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON + + return V_CRUISE_UNSET + + def _update_params(self) -> None: + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.enabled = self.params.get_bool("SmartCruiseControlVision") + + def _update_calculations(self, sm: messaging.SubMaster) -> None: + if not self.long_enabled: + return + else: + rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z)) + vel_plan = np.array(sm['modelV2'].velocity.x) + + self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature) + + # get the maximum lat accel from the model + predicted_lat_accels = rate_plan * vel_plan + self.max_pred_lat_acc = np.amax(predicted_lat_accels) + + # get the maximum curve based on the current velocity + v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations + max_curve = self.max_pred_lat_acc / (v_ego**2) + + # Get the target velocity for the maximum curve + self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5 + + def _update_state_machine(self) -> tuple[bool, bool]: + # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING + if self.state != VisionState.disabled: + # longitudinal and feature disable always have priority in a non-disabled state + if not self.long_enabled or not self.enabled: + self.state = VisionState.disabled + elif self.long_override: + self.state = VisionState.overriding + + else: + # ENABLED + if self.state == VisionState.enabled: + # Do not enter a turn control cycle if the speed is low. + if self.v_ego <= MIN_V: + pass + # If significant lateral acceleration is predicted ahead, then move to Entering turn state. + elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: + self.state = VisionState.entering + + # OVERRIDING + elif self.state == VisionState.overriding: + if not self.long_override: + self.state = VisionState.enabled + + # ENTERING + elif self.state == VisionState.entering: + # Transition to Turning if current lateral acceleration is over the threshold. + if self.current_lat_acc >= _TURNING_LAT_ACC_TH: + self.state = VisionState.turning + # Abort if the predicted lateral acceleration drops + elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH: + self.state = VisionState.enabled + + # TURNING + elif self.state == VisionState.turning: + # Transition to Leaving if current lateral acceleration drops below a threshold. + if self.current_lat_acc <= _LEAVING_LAT_ACC_TH: + self.state = VisionState.leaving + + # LEAVING + elif self.state == VisionState.leaving: + # Transition back to Turning if current lateral acceleration goes back over the threshold. + if self.current_lat_acc >= _TURNING_LAT_ACC_TH: + self.state = VisionState.turning + # Finish if current lateral acceleration goes below a threshold. + elif self.current_lat_acc < _FINISH_LAT_ACC_TH: + self.state = VisionState.enabled + + # DISABLED + elif self.state == VisionState.disabled: + if self.long_enabled and self.enabled: + if self.long_override: + self.state = VisionState.overriding + else: + self.state = VisionState.enabled + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def _update_solution(self) -> float: + # DISABLED, ENABLED, OVERRIDING + if self.state not in ACTIVE_STATES: + # when not overshooting, calculate v_turn as the speed at the prediction horizon when following + # the smooth deceleration. + a_target = self.a_ego + # ENTERING + elif self.state == VisionState.entering: + # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come. + a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) + # TURNING + elif self.state == VisionState.turning: + # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt. + a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) + # LEAVING + elif self.state == VisionState.leaving: + # When leaving, we provide a comfortable acceleration to regain speed. + a_target = _LEAVING_ACC + else: + raise NotImplementedError(f"SCC-V state not supported: {self.state}") + + return a_target + + def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, + v_cruise_setpoint: float) -> None: + self.long_enabled = long_enabled + self.long_override = long_override + self.v_ego = v_ego + self.a_ego = a_ego + self.v_cruise_setpoint = v_cruise_setpoint + + self._update_params() + self._update_calculations(sm) + + self.is_enabled, self.is_active = self._update_state_machine() + self.a_target = self._update_solution() + + self.output_v_target = self.get_v_target_from_control() + self.output_a_target = self.get_a_target_from_control() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py new file mode 100644 index 0000000000..22ea75fe19 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -0,0 +1,19 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. +LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. + +# Speed Limit Assist constants +PCM_LONG_REQUIRED_MAX_SET_SPEED = { + True: (33.3333, 36.1111), # km/h, (120, 130) + False: (31.2928, 35.7632), # mph, (70, 80) +} + +CONFIRM_SPEED_THRESHOLD = { + True: 80, # km/h + False: 50, # mph +} diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py new file mode 100644 index 0000000000..baf9328032 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py @@ -0,0 +1,28 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from enum import IntEnum + + +class Policy(IntEnum): + car_state_only = 0 + map_data_only = 1 + car_state_priority = 2 + map_data_priority = 3 + combined = 4 + + +class OffsetType(IntEnum): + off = 0 + fixed = 1 + percentage = 2 + + +class Mode(IntEnum): + off = 0 + information = 1 + warning = 2 + assist = 3 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py new file mode 100644 index 0000000000..40e8f653ff --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py @@ -0,0 +1,19 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from openpilot.common.constants import CV + + +def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]: + speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH + v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv) + target_set_speed_conv = round(target_set_speed * speed_conv) + + req_plus = v_cruise_cluster_conv < target_set_speed_conv + req_minus = v_cruise_cluster_conv > target_set_speed_conv + + return req_plus, req_minus diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py new file mode 100644 index 0000000000..302d8cd14d --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -0,0 +1,397 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import time + +from cereal import custom, car +from openpilot.common.params import Params +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target +from openpilot.selfdrive.modeld.constants import ModelConstants + +ButtonType = car.CarState.ButtonEvent.Type +EventNameSP = custom.OnroadEventSP.EventName +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + +ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) +ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES) + +DISABLED_GUARD_PERIOD = 0.5 # secs. +PRE_ACTIVE_GUARD_PERIOD = 15 # secs. Time to wait after activation before considering temp deactivation signal. +SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive. + +LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. +LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. +LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. +LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. +V_CRUISE_UNSET = 255 + +CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise) +CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise) +CRUISE_BUTTON_CONFIRM_HOLD = 0.5 # secs. + + +class SpeedLimitAssist: + _speed_limit_final_last: float + _distance: float + v_ego: float + a_ego: float + v_offset: float + + def __init__(self, CP): + self.params = Params() + self.CP = CP + self.frame = -1 + self.long_engaged_timer = 0 + self.pre_active_timer = 0 + self.is_metric = self.params.get_bool("IsMetric") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist + self.long_enabled = False + self.long_enabled_prev = False + self.is_enabled = False + self.is_active = False + self.output_v_target = V_CRUISE_UNSET + self.output_a_target = 0. + self.v_ego = 0. + self.a_ego = 0. + self.v_offset = 0. + self.target_set_speed_conv = 0 + self.prev_target_set_speed_conv = 0 + self.v_cruise_cluster = 0. + self.v_cruise_cluster_prev = 0. + self.v_cruise_cluster_conv = 0 + self.prev_v_cruise_cluster_conv = 0 + self._has_speed_limit = False + self._speed_limit = 0. + self._speed_limit_final_last = 0. + self.speed_limit_prev = 0. + self.speed_limit_final_last_conv = 0 + self.prev_speed_limit_final_last_conv = 0 + self._distance = 0. + self.state = SpeedLimitAssistState.disabled + self._state_prev = SpeedLimitAssistState.disabled + self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise + + self._plus_hold = 0. + self._minus_hold = 0. + self._last_carstate_ts = 0. + + # TODO-SP: SLA's own output_a_target for planner + # Solution functions mapped to respective states + self.acceleration_solutions = { + SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target, + SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target, + SpeedLimitAssistState.pending: self.get_current_acceleration_as_target, + SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration, + SpeedLimitAssistState.active: self.get_active_state_target_acceleration, + } + + @property + def speed_limit_changed(self) -> bool: + return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev) + + @property + def v_cruise_cluster_changed(self) -> bool: + return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv) + + @property + def target_set_speed_confirmed(self) -> bool: + return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv) + + def get_v_target_from_control(self) -> float: + if self._has_speed_limit: + if self.pcm_op_long and self.is_enabled: + return self._speed_limit_final_last + if not self.pcm_op_long and self.is_active: + return self._speed_limit_final_last + + # Fallback + return V_CRUISE_UNSET + + # TODO-SP: SLA's own output_a_target for planner + def get_a_target_from_control(self) -> float: + return self.a_ego + + def update_params(self) -> None: + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.is_metric = self.params.get_bool("IsMetric") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist + + def update_car_state(self, CS: car.CarState) -> None: + now = time.monotonic() + self._last_carstate_ts = now + + for b in CS.buttonEvents: + if not b.pressed: + if b.type in CRUISE_BUTTONS_PLUS: + self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) + elif b.type in CRUISE_BUTTONS_MINUS: + self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD) + + def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool: + now = time.monotonic() + if req_plus and now <= self._plus_hold: + self._plus_hold = 0. + return True + elif req_minus and now <= self._minus_hold: + self._minus_hold = 0. + return True + + # expired + if now > self._plus_hold: + self._plus_hold = 0. + if now > self._minus_hold: + self._minus_hold = 0. + return False + + def update_calculations(self, v_cruise_cluster: float) -> None: + speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + self.v_cruise_cluster = v_cruise_cluster + + # Update current velocity offset (error) + self.v_offset = self._speed_limit_final_last - self.v_ego + + self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv) + self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv) + + cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] + pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \ + cst_high + pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) + + self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv + + @property + def apply_confirm_speed_threshold(self) -> bool: + # below CST: always require user confirmation + if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]: + return True + + # at/above CST: + # - new speed limit >= CST: auto change + # - new speed limit < CST: user confirmation required + return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]) + + def get_current_acceleration_as_target(self) -> float: + return self.a_ego + + def get_adapting_state_target_acceleration(self) -> float: + if self._distance > 0: + return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance) + + return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) + + def get_active_state_target_acceleration(self) -> float: + return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N]) + + def _update_confirmed_state(self): + if self._has_speed_limit: + if self.v_offset < LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.adapting + else: + self.state = SpeedLimitAssistState.active + else: + self.state = SpeedLimitAssistState.pending + + def _update_non_pcm_long_confirmed_state(self) -> bool: + if self.target_set_speed_confirmed: + return True + + if self.state != SpeedLimitAssistState.preActive: + return False + + req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric) + + return self._get_button_release(req_plus, req_minus) + + def update_state_machine_pcm_op_long(self): + self.long_engaged_timer = max(0, self.long_engaged_timer - 1) + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE + if self.state != SpeedLimitAssistState.disabled: + if not self.long_enabled or not self.enabled: + self.state = SpeedLimitAssistState.disabled + + else: + # ACTIVE + if self.state == SpeedLimitAssistState.active: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.adapting + + # ADAPTING + elif self.state == SpeedLimitAssistState.adapting: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self.v_offset >= LIMIT_SPEED_OFFSET_TH: + self.state = SpeedLimitAssistState.active + + # PENDING + elif self.state == SpeedLimitAssistState.pending: + if self.target_set_speed_confirmed: + self._update_confirmed_state() + elif self.speed_limit_changed: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + + # PRE_ACTIVE + elif self.state == SpeedLimitAssistState.preActive: + if self.target_set_speed_confirmed: + self._update_confirmed_state() + elif self.pre_active_timer <= 0: + # Timeout - session ended + self.state = SpeedLimitAssistState.inactive + + # INACTIVE + elif self.state == SpeedLimitAssistState.inactive: + pass + + # DISABLED + elif self.state == SpeedLimitAssistState.disabled: + if self.long_enabled and self.enabled: + # start or reset preActive timer if initially enabled or manual set speed change detected + if not self.long_enabled_prev or self.v_cruise_cluster_changed: + self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) + + elif self.long_engaged_timer <= 0: + if self.target_set_speed_confirmed: + self._update_confirmed_state() + elif self._has_speed_limit: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + else: + self.state = SpeedLimitAssistState.pending + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def update_state_machine_non_pcm_long(self): + self.long_engaged_timer = max(0, self.long_engaged_timer - 1) + self.pre_active_timer = max(0, self.pre_active_timer - 1) + + # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE + if self.state != SpeedLimitAssistState.disabled: + if not self.long_enabled or not self.enabled: + self.state = SpeedLimitAssistState.disabled + + else: + # ACTIVE + if self.state == SpeedLimitAssistState.active: + if self.v_cruise_cluster_changed: + self.state = SpeedLimitAssistState.inactive + + elif self.speed_limit_changed and self.apply_confirm_speed_threshold: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + + # PRE_ACTIVE + elif self.state == SpeedLimitAssistState.preActive: + if self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + elif self.pre_active_timer <= 0: + # Timeout - session ended + self.state = SpeedLimitAssistState.inactive + + # INACTIVE + elif self.state == SpeedLimitAssistState.inactive: + if self.speed_limit_changed: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + elif self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + + # DISABLED + elif self.state == SpeedLimitAssistState.disabled: + if self.long_enabled and self.enabled: + # start or reset preActive timer if initially enabled or manual set speed change detected + if not self.long_enabled_prev or self.v_cruise_cluster_changed: + self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL) + + elif self.long_engaged_timer <= 0: + if self._update_non_pcm_long_confirmed_state(): + self.state = SpeedLimitAssistState.active + elif self._has_speed_limit: + self.state = SpeedLimitAssistState.preActive + self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + else: + self.state = SpeedLimitAssistState.inactive + + enabled = self.state in ENABLED_STATES + active = self.state in ACTIVE_STATES + + return enabled, active + + def update_events(self, events_sp: EventsSP) -> None: + if self.state == SpeedLimitAssistState.preActive: + events_sp.add(EventNameSP.speedLimitPreActive) + + if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending: + events_sp.add(EventNameSP.speedLimitPending) + + if self.is_active: + if self._state_prev not in ACTIVE_STATES: + events_sp.add(EventNameSP.speedLimitActive) + + # only notify if we acquire a valid speed limit + # do not check has_speed_limit here + elif self._speed_limit != self.speed_limit_prev: + if self.speed_limit_prev <= 0: + events_sp.add(EventNameSP.speedLimitActive) + elif self.speed_limit_prev > 0 and self._speed_limit > 0: + events_sp.add(EventNameSP.speedLimitChanged) + + def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float, + speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None: + self.long_enabled = long_enabled + self.v_ego = v_ego + self.a_ego = a_ego + + self._has_speed_limit = has_speed_limit + self._speed_limit = speed_limit + self._speed_limit_final_last = speed_limit_final_last + self._distance = distance + + self.update_params() + self.update_calculations(v_cruise_cluster) + + self._state_prev = self.state + if self.pcm_op_long: + self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long() + else: + self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long() + + self.update_events(events_sp) + + # Update change tracking variables + self.speed_limit_prev = self._speed_limit + self.v_cruise_cluster_prev = self.v_cruise_cluster + self.long_enabled_prev = self.long_enabled + self.prev_target_set_speed_conv = self.target_set_speed_conv + self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv + self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv + + self.output_v_target = self.get_v_target_from_control() + self.output_a_target = self.get_a_target_from_control() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py new file mode 100644 index 0000000000..51fb8f6d64 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py @@ -0,0 +1,178 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import time + +import cereal.messaging as messaging +from cereal import custom +from openpilot.common.constants import CV +from openpilot.common.gps import get_gps_location_service +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + +ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values()) + + +class SpeedLimitResolver: + limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] + distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] + v_ego: float + speed_limit: float + speed_limit_last: float + speed_limit_final: float + speed_limit_final_last: float + distance: float + source: custom.LongitudinalPlanSP.SpeedLimit.Source + speed_limit_offset: float + + def __init__(self): + self.params = Params() + self.frame = -1 + + self._gps_location_service = get_gps_location_service(self.params) + self.limit_solutions = {} # Store for speed limit solutions from different sources + self.distance_solutions = {} # Store for distance to current speed limit start for different sources + + self.policy = self.params.get("SpeedLimitPolicy", return_default=True) + self._policy_to_sources_map = { + Policy.car_state_only: [SpeedLimitSource.car], + Policy.map_data_only: [SpeedLimitSource.map], + Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], + Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car], + Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map], + } + self.source = SpeedLimitSource.none + for source in ALL_SOURCES: + self._reset_limit_sources(source) + + self.is_metric = self.params.get_bool("IsMetric") + self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) + self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + + self.speed_limit = 0. + self.speed_limit_last = 0. + self.speed_limit_final = 0. + self.speed_limit_final_last = 0. + self.speed_limit_offset = 0. + + def update_speed_limit_states(self) -> None: + self.speed_limit_final = self.speed_limit + self.speed_limit_offset + + if self.speed_limit > 0.: + self.speed_limit_last = self.speed_limit + self.speed_limit_final_last = self.speed_limit_final + + @property + def speed_limit_valid(self) -> bool: + return self.speed_limit > 0. + + @property + def speed_limit_last_valid(self) -> bool: + return self.speed_limit_last > 0. + + def update_params(self): + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.policy = self.params.get("SpeedLimitPolicy", return_default=True) + self.is_metric = self.params.get_bool("IsMetric") + self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) + self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + + def _get_speed_limit_offset(self) -> float: + if self.offset_type == OffsetType.off: + return 0 + elif self.offset_type == OffsetType.fixed: + return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)) + elif self.offset_type == OffsetType.percentage: + return float(self.offset_value * 0.01 * self.speed_limit) + else: + raise NotImplementedError("Offset not supported") + + def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None: + self.limit_solutions[source] = 0. + self.distance_solutions[source] = 0. + + def _get_from_car_state(self, sm: messaging.SubMaster) -> None: + self._reset_limit_sources(SpeedLimitSource.car) + self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit + self.distance_solutions[SpeedLimitSource.car] = 0. + + def _get_from_map_data(self, sm: messaging.SubMaster) -> None: + self._reset_limit_sources(SpeedLimitSource.map) + self._process_map_data(sm) + + def _process_map_data(self, sm: messaging.SubMaster) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3 + if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: + return + + speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. + next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. + + self._calculate_map_data_limits(sm, speed_limit, next_speed_limit) + + def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3) + distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) + + self.limit_solutions[SpeedLimitSource.map] = speed_limit + self.distance_solutions[SpeedLimitSource.map] = 0. + + if 0. < next_speed_limit < self.v_ego: + adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC + adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2 + + if distance_to_speed_limit_ahead <= adapt_distance: + self.limit_solutions[SpeedLimitSource.map] = next_speed_limit + self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead + + def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source: + sources_for_policy = self._policy_to_sources_map[self.policy] + + if self.policy != Policy.combined: + # They are ordered in the order of preference, so we pick the first that's non-zero + for source in sources_for_policy: + if self.limit_solutions[source] > 0.: + return source + return SpeedLimitSource.none + + sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.] + if sources_with_limits: + return min(sources_with_limits, key=lambda x: x[1])[0] + + return SpeedLimitSource.none + + def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]: + """Get limit solutions from each data source""" + self._get_from_car_state(sm) + self._get_from_map_data(sm) + + source = self._get_source_solution_according_to_policy() + speed_limit = self.limit_solutions[source] if source else 0. + distance = self.distance_solutions[source] if source else 0. + + return speed_limit, distance, source + + def update(self, v_ego: float, sm: messaging.SubMaster) -> None: + self.v_ego = v_ego + self.update_params() + + self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm) + self.speed_limit_offset = self._get_speed_limit_offset() + + self.update_speed_limit_states() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py new file mode 100644 index 0000000000..d2c7a4716d --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -0,0 +1,241 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import custom + +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.common.constants import CV +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET +from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ + PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP + +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState + +ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values()) + +SPEED_LIMITS = { + 'residential': 25 * CV.MPH_TO_MS, # 25 mph + 'city': 35 * CV.MPH_TO_MS, # 35 mph + 'highway': 65 * CV.MPH_TO_MS, # 65 mph + 'freeway': 80 * CV.MPH_TO_MS, # 80 mph +} + + +class TestSpeedLimitAssist: + + def setup_method(self): + self.params = Params() + self.reset_custom_params() + self.events_sp = EventsSP() + CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2) + self.sla = SpeedLimitAssist(CI.CP) + self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL) + self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now + self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH + + def teardown_method(self, method): + self.reset_state() + + def _setup_platform(self, car_name): + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) + CI = CarInterface(CP, CP_SP) + sunnypilot_interfaces.setup_interfaces(CI, self.params) + return CI + + def reset_custom_params(self): + self.params.put("SpeedLimitMode", int(Mode.assist)) + self.params.put_bool("IsMetric", False) + self.params.put("SpeedLimitOffsetType", 0) + self.params.put("SpeedLimitValueOffset", 0) + + def reset_state(self): + self.sla.state = SpeedLimitAssistState.disabled + self.sla.frame = -1 + self.sla.last_op_engaged_frame = 0 + self.sla.op_engaged = False + self.sla.op_engaged_prev = False + self.sla._speed_limit = 0. + self.sla.speed_limit_prev = 0. + self.sla.last_valid_speed_limit_offsetted = 0. + self.sla._distance = 0. + self.events_sp.clear() + + def initialize_active_state(self, initialize_v_cruise): + self.sla.state = SpeedLimitAssistState.active + self.sla.v_cruise_cluster = initialize_v_cruise + self.sla.v_cruise_cluster_prev = initialize_v_cruise + self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv) + + def test_initial_state(self): + assert self.sla.state == SpeedLimitAssistState.disabled + assert not self.sla.is_enabled + assert not self.sla.is_active + assert V_CRUISE_UNSET == self.sla.get_v_target_from_control() + + def test_disabled(self): + self.params.put("SpeedLimitMode", int(Mode.off)) + for _ in range(int(10. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.disabled + + def test_transition_disabled_to_preactive(self): + for _ in range(int(3. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.preActive + assert self.sla.is_enabled and not self.sla.is_active + + def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self): + for _ in range(int(3. / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.pending + assert self.sla.is_enabled and not self.sla.is_active + + def test_preactive_to_active_with_max_speed_confirmation(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + assert self.sla.is_enabled and self.sla.is_active + assert self.sla.output_v_target == SPEED_LIMITS['highway'] + + def test_preactive_timeout_to_inactive(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + + for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive + + def test_preactive_to_pending_no_speed_limit(self): + self.sla.state = SpeedLimitAssistState.preActive + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.pending + assert self.sla.is_enabled and not self.sla.is_active + + def test_pending_to_active_when_speed_limit_available(self): + self.sla.state = SpeedLimitAssistState.pending + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, + SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + + def test_pending_to_adapting_when_below_speed_limit(self): + self.sla.state = SpeedLimitAssistState.pending + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed, + SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert self.sla.is_enabled and self.sla.is_active + + def test_active_to_adapting_transition(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + + def test_adapting_to_active_transition(self): + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'], + SPEED_LIMITS['highway'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.active + + def test_manual_cruise_change_detection(self): + self.sla.state = SpeedLimitAssistState.active + expected_cruise = SPEED_LIMITS['highway'] + self.sla.v_cruise_cluster_prev = expected_cruise + + different_cruise = SPEED_LIMITS['highway'] + 5 + self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.inactive + + # TODO-SP: test lower CST cases + def test_rapid_speed_limit_changes(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']] + + for _, speed_limit in enumerate(speed_limits): + self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp) + assert self.sla.state in ACTIVE_STATES + + def test_invalid_speed_limits_handling(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] + + for invalid_limit in invalid_limits: + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp) + assert isinstance(self.sla.output_v_target, (int, float)) + assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 + + def test_stale_data_handling(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + old_speed_limit = SPEED_LIMITS['city'] + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp) + assert self.sla.state in ACTIVE_STATES + assert self.sla.output_v_target == old_speed_limit + + def test_distance_based_adapting(self): + self.sla.state = SpeedLimitAssistState.adapting + self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed + self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv) + + distance = 100.0 + current_speed = SPEED_LIMITS['freeway'] + target_speed = SPEED_LIMITS['highway'] + + self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.adapting + assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions + + def test_long_disengaged_to_disabled(self): + self.initialize_active_state(self.pcm_long_max_set_speed) + + self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], + SPEED_LIMITS['city'], True, 0, self.events_sp) + assert self.sla.state == SpeedLimitAssistState.disabled + assert self.sla.output_v_target == V_CRUISE_UNSET + + def test_maintain_states_with_no_changes(self): + """Test that states are maintained when no significant changes occur""" + test_states = [ + SpeedLimitAssistState.preActive, + SpeedLimitAssistState.pending, + SpeedLimitAssistState.active, + SpeedLimitAssistState.adapting + ] + + for state in test_states: + self.sla.state = state + self.sla.op_engaged = True + + initial_state = state + + self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp) + + assert self.sla.state in ALL_STATES # Sanity check + + if initial_state == SpeedLimitAssistState.preActive: + assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active] + elif initial_state in ACTIVE_STATES: + assert self.sla.state in ACTIVE_STATES diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py new file mode 100644 index 0000000000..c02831d71a --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py @@ -0,0 +1,144 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import random +import time + +import pytest +from pytest_mock import MockerFixture + +from cereal import custom +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE + +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + + +def create_mock(properties, mocker: MockerFixture): + mock = mocker.MagicMock() + for _property, value in properties.items(): + setattr(mock, _property, value) + return mock + + +def setup_sm_mock(mocker: MockerFixture): + cruise_speed_limit = random.uniform(0, 120) + live_map_data_limit = random.uniform(0, 120) + + car_state = create_mock({ + 'gasPressed': False, + 'brakePressed': False, + 'standstill': False, + }, mocker) + car_state_sp = create_mock({ + 'speedLimit': cruise_speed_limit, + }, mocker) + live_map_data = create_mock({ + 'speedLimit': live_map_data_limit, + 'speedLimitValid': True, + 'speedLimitAhead': 0., + 'speedLimitAheadValid': 0., + 'speedLimitAheadDistance': 0., + }, mocker) + gps_data = create_mock({ + 'unixTimestampMillis': time.monotonic() * 1e3, + }, mocker) + sm_mock = mocker.MagicMock() + sm_mock.__getitem__.side_effect = lambda key: { + 'carState': car_state, + 'liveMapDataSP': live_map_data, + 'carStateSP': car_state_sp, + 'gpsLocation': gps_data, + }[key] + return sm_mock + + +parametrized_policies = pytest.mark.parametrize( + "policy, sm_key, function_key", [ + (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car), + (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car), + (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map), + (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map), + ], + ids=lambda val: val.name if hasattr(val, 'name') else str(val) +) + + +@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver]) +class TestSpeedLimitResolverValidation: + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_initial_state(self, resolver_class, policy): + resolver = resolver_class() + resolver.policy = policy + for source in ALL_SOURCES: + if source in resolver.limit_solutions: + assert resolver.limit_solutions[source] == 0. + assert resolver.distance_solutions[source] == 0. + + @parametrized_policies + def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = setup_sm_mock(mocker) + source_speed_limit = sm_mock[sm_key].speedLimit + + # Assert the resolver + resolver.update(source_speed_limit, sm_mock) + assert resolver.speed_limit == source_speed_limit + assert resolver.source == ALL_SOURCES[function_key] + + def test_resolver_combined(self, resolver_class, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = Policy.combined + sm_mock = setup_sm_mock(mocker) + socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map} + minimum_key, minimum_speed_limit = min( + ((key, sm_mock[key].speedLimit) for key in + socket_to_source.keys()), key=lambda x: x[1]) + + # Assert the resolver + resolver.update(minimum_speed_limit, sm_mock) + assert resolver.speed_limit == minimum_speed_limit + assert resolver.source == socket_to_source[minimum_key] + + @parametrized_policies + def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = setup_sm_mock(mocker) + source_speed_limit = sm_mock[sm_key].speedLimit + + # Assert the parsing + resolver.update(source_speed_limit, sm_mock) + assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit + assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0. + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture): + v_ego = 50 + resolver = resolver_class() + resolver.policy = policy + + sm_mock = setup_sm_mock(mocker) + resolver.update(v_ego, sm_mock) + + # After resolution + assert resolver.speed_limit is not None + assert resolver.distance is not None + assert resolver.source is not None + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = mocker.MagicMock() + sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 + resolver._get_from_map_data(sm_mock) + assert resolver.limit_solutions[SpeedLimitSource.map] == 0. + assert resolver.distance_solutions[SpeedLimitSource.map] == 0. diff --git a/sunnypilot/selfdrive/locationd/.gitignore b/sunnypilot/selfdrive/locationd/.gitignore new file mode 100644 index 0000000000..11b9f127b2 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/.gitignore @@ -0,0 +1,3 @@ +params_learner +paramsd +locationd diff --git a/sunnypilot/selfdrive/locationd/SConscript b/sunnypilot/selfdrive/locationd/SConscript new file mode 100644 index 0000000000..27cd4d5b40 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/SConscript @@ -0,0 +1,37 @@ +Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations') + +loc_libs = [messaging, common, 'pthread', 'dl'] + +# build ekf models +rednose_gen_dir = 'models/generated' +rednose_gen_deps = [ + "models/constants.py", +] +live_ekf = env.RednoseCompileFilter( + target='live', + filter_gen_script='models/live_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=['live_kf_constants.h'], + gen_script_deps=rednose_gen_deps, +) +car_ekf = env.RednoseCompileFilter( + target='car', + filter_gen_script='models/car_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=[], + gen_script_deps=rednose_gen_deps, +) + +# locationd build +locationd_sources = ["locationd.cc", "models/live_kf.cc"] + +lenv = env.Clone() +# ekf filter libraries need to be linked, even if no symbols are used +if arch != "Darwin": + lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"] + +lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath) +lenv["RPATH"].append(Dir(rednose_gen_dir).abspath) +locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations) +lenv.Depends(locationd, rednose) +lenv.Depends(locationd, live_ekf) diff --git a/sunnypilot/selfdrive/locationd/__init__.py b/sunnypilot/selfdrive/locationd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/locationd.cc b/sunnypilot/selfdrive/locationd/locationd.cc new file mode 100644 index 0000000000..c42ab13f25 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/locationd.cc @@ -0,0 +1,750 @@ +#include "sunnypilot/selfdrive/locationd/locationd.h" + +#include +#include + +#include +#include +#include + +using namespace EKFS; +using namespace Eigen; + +ExitHandler do_exit; +const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 +const double ROTATION_SANITY_CHECK = 10.0; // rad/s +const double TRANS_SANITY_CHECK = 200.0; // m/s +const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m +const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad +const double VALID_TIME_SINCE_RESET = 1.0; // s +const double VALID_POS_STD = 50.0; // m +const double MAX_RESET_TRACKER = 5.0; +const double SANE_GPS_UNCERTAINTY = 1500.0; // m +const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker +const double RESET_TRACKER_DECAY = 0.99995; +const double DECAY = 0.9993; // ~10 secs to resume after a bad input +const double MAX_FILTER_REWIND_TIME = 0.8; // s +const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; + +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s +const float GPS_POS_STD_THRESHOLD = 50.0; +const float GPS_VEL_STD_THRESHOLD = 5.0; +const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; +const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; + +const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; + +static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { + VectorXd res(floatlist.size()); + for (int i = 0; i < floatlist.size(); i++) { + res[i] = floatlist[i]; + } + return res; +} + +static Vector4d quat2vector(const Quaterniond& quat) { + return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); +} + +static Quaterniond vector2quat(const VectorXd& vec) { + return Quaterniond(vec(0), vec(1), vec(2), vec(3)); +} + +static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { + meas.setValue(kj::arrayPtr(val.data(), val.size())); + meas.setStd(kj::arrayPtr(std.data(), std.size())); + meas.setValid(valid); +} + + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + +Localizer::Localizer(LocalizerGnssSource gnss_source) { + this->kf = std::make_unique(); + this->reset_kalman(); + + this->calib = Vector3d(0.0, 0.0, 0.0); + this->device_from_calib = MatrixXdr::Identity(3, 3); + this->calib_from_device = MatrixXdr::Identity(3, 3); + + for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { + this->posenet_stds.push_back(10.0); + } + + VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); + this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + this->configure_gnss_source(gnss_source); +} + +void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { + VectorXd predicted_state = this->kf->get_x(); + MatrixXdr predicted_cov = this->kf->get_P(); + VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); + + VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); + VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); + VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); + VectorXd fix_pos_geo_vec = this->get_position_geodetic(); + VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); + VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose()); + + VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); + MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); + VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); + VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + + MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); + VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); + + VectorXd vel_device = device_from_ecef * vel_ecef; + VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); + MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + condensed_cov.topLeftCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + condensed_cov.topRightCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomRightCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomLeftCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); + H_input << device_from_ecef_eul, vel_ecef; + MatrixXdr HH = this->kf->H(H_input); + MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); + VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); + + VectorXd vel_calib = this->calib_from_device * vel_device; + VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); + + VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); + VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); + VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef); + VectorXd nextfix_ecef = fix_ecef + vel_ecef; + VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); + + VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); + VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); + + VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); + + Vector3d nans = Vector3d(NAN, NAN, NAN); + + // TODO fill in NED and Calibrated stds + // write measurements to msg + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode); + init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); + init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); + init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); + init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + if (DEBUG) { + init_measurement(fix.initFilterState(), predicted_state, predicted_std, true); + } + + double old_mean = 0.0, new_mean = 0.0; + int i = 0; + for (double x : this->posenet_stds) { + if (i < POSENET_STD_HIST_HALF) { + old_mean += x; + } else { + new_mean += x; + } + i++; + } + old_mean /= POSENET_STD_HIST_HALF; + new_mean /= POSENET_STD_HIST_HALF; + // experimentally found these values, no false positives in 20k minutes of driving + bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); + + fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); + fix.setDeviceStable(!this->device_fell); + fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); + fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); + this->device_fell = false; + + //fix.setGpsWeek(this->time.week); + //fix.setGpsTimeOfWeek(this->time.tow); + fix.setUnixTimestampMillis(this->unix_timestamp_millis); + + double time_since_reset = this->kf->get_filter_time() - this->last_reset_time; + fix.setTimeSinceReset(time_since_reset); + if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::VALID); + } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); + } else { + fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); + } +} + +VectorXd Localizer::get_position_geodetic() { + VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_START); + ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; + Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); + return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); +} + +VectorXd Localizer::get_state() { + return this->kf->get_x(); +} + +VectorXd Localizer::get_stdev() { + return this->kf->get_P().diagonal().array().sqrt(); +} + +bool Localizer::are_inputs_ok() { + return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; +} + +void Localizer::observation_timings_invalid_reset(){ + this->observation_timings_invalid = false; +} + +void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { + // TODO does not yet account for double sensor readings in the log + + // Ignore empty readings (e.g. in case the magnetometer had no data ready) + if (log.getTimestamp() == 0) { + return; + } + + double sensor_time = 1e-9 * log.getTimestamp(); + + // sensor time and log time should be close + if (std::abs(current_time - sensor_time) > 0.1) { + LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); + this->observation_timings_invalid = true; + return; + } else if (!this->is_timestamp_valid(sensor_time)) { + this->observation_timings_invalid = true; + return; + } + + // TODO: handle messages from two IMUs at the same time + if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { + return; + } + + // Gyro Uncalibrated + if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { + auto v = log.getGyroUncalibrated().getV(); + auto meas = Vector3d(-v[2], -v[1], -v[0]); + + VectorXd gyro_bias = this->kf->get_x().segment(STATE_GYRO_BIAS_START); + float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); + float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; + bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; + + if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + this->observation_values_invalid["gyroscope"] *= DECAY; + } else { + this->observation_values_invalid["gyroscope"] += 1.0; + } + } + + // Accelerometer + if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { + auto v = log.getAcceleration().getV(); + + // TODO: reduce false positives and re-enable this check + // check if device fell, estimate 10 for g + // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving + // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; + + auto meas = Vector3d(-v[2], -v[1], -v[0]); + if (meas.norm() < ACCEL_SANITY_CHECK) { + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + this->observation_values_invalid["accelerometer"] *= DECAY; + } else { + this->observation_values_invalid["accelerometer"] += 1.0; + } + } +} + +void Localizer::input_fake_gps_observations(double current_time) { + // This is done to make sure that the error estimate of the position does not blow up + // when the filter is in no-gps mode + // Steps : first predict -> observe current obs with reasonable STD + this->kf->predict(current_time); + + VectorXd current_x = this->kf->get_x(); + VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START); + VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); + const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); + const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { + bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); + bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); + bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); + + if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + //this->gps_valid = false; + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = current_time - sensor_time_offset; + + // Process message + //this->gps_valid = true; + this->gps_mode = true; + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(geodetic); + + VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); + VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; + float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); + MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); + MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); + + this->unix_timestamp_millis = log.getUnixTimestampMillis(); + double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { + LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + } else if (gps_est_error > 100.0) { + LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } + + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { + + if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = log.getMeasTime() * 1e-9; + sensor_time -= this->gps_time_offset; + + auto ecef_pos_v = log.getPositionECEF().getValue(); + VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); + + // indexed at 0 cause all std values are the same MAE + auto ecef_pos_std = log.getPositionECEF().getStd()[0]; + MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); + + auto ecef_vel_v = log.getVelocityECEF().getValue(); + VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); + + // indexed at 0 cause all std values are the same MAE + auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; + MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); + + double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef); + + LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; + VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); + double bearing_rad = atan2(ned_vel[1], ned_vel[0]); + + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + this->determine_gps_mode(current_time); + return; + } + + // prevent jumping gnss measurements (covered lots, standstill...) + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; + orientation_reset &= !this->standstill; + if (orientation_reset) { + this->orientation_reset_count++; + } else { + this->orientation_reset_count = 0; + } + + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + // always reset on first gps message and if the location is off but the accuracy is high + LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { + LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + this->orientation_reset_count = 0; + } + + this->gps_mode = true; + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { + this->car_speed = std::abs(log.getVEgo()); + this->standstill = log.getStandstill(); + if (this->standstill) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); + } +} + +void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + + if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); + + // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise + trans_calib_std *= 10.0; + rot_calib_std *= 10.0; + MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal(); + MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal(); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, + { rot_device }, { rot_device_cov }); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, + { trans_device }, { trans_device_cov }); + this->observation_values_invalid["cameraOdometry"] *= DECAY; + this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); +} + +void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if (log.getRpyCalib().size() > 0) { + auto live_calib = floatlist2vector(log.getRpyCalib()); + if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { + this->observation_values_invalid["liveCalibration"] += 1.0; + return; + } + + this->calib = live_calib; + this->device_from_calib = euler2rot(this->calib); + this->calib_from_device = this->device_from_calib.transpose(); + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; + this->observation_values_invalid["liveCalibration"] *= DECAY; + } +} + +void Localizer::reset_kalman(double current_time) { + const VectorXd &init_x = this->kf->get_initial_x(); + const MatrixXdr &init_P = this->kf->get_initial_P(); + this->reset_kalman(current_time, init_x, init_P); +} + +void Localizer::finite_check(double current_time) { + bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); + if (!all_finite) { + LOGE("Non-finite values detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::time_check(double current_time) { + if (std::isnan(this->last_reset_time)) { + this->last_reset_time = current_time; + } + if (std::isnan(this->first_valid_log_time)) { + this->first_valid_log_time = current_time; + } + double filter_time = this->kf->get_filter_time(); + bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); + if (big_time_gap) { + LOGE("Time gap of over 10s detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::update_reset_tracker() { + // reset tracker is tuned to trigger when over 1reset/10s over 2min period + if (this->is_gps_ok()) { + this->reset_tracker *= RESET_TRACKER_DECAY; + } else { + this->reset_tracker = 0.0; + } +} + +void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { + // too nonlinear to init on completely wrong + VectorXd current_x = this->kf->get_x(); + MatrixXdr current_P = this->kf->get_P(); + MatrixXdr init_P = this->kf->get_initial_P(); + const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); + int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + + current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; + current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; + current_x.segment(STATE_ECEF_POS_START) = init_pos; + + init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); + init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); + init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); + init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, + STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); + + this->reset_kalman(current_time, current_x, init_P); +} + +void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) { + this->kf->init_state(init_x, init_P, current_time); + this->last_reset_time = current_time; + this->reset_tracker += 1.0; +} + +void Localizer::handle_msg_bytes(const char *data, const size_t size) { + AlignedBuffer aligned_buf; + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); + cereal::Event::Reader event = cmsg.getRoot(); + + this->handle_msg(event); +} + +void Localizer::handle_msg(const cereal::Event::Reader& log) { + double t = log.getLogMonoTime() * 1e-9; + this->time_check(t); + if (log.isAccelerometer()) { + this->handle_sensor(t, log.getAccelerometer()); + } else if (log.isGyroscope()) { + this->handle_sensor(t, log.getGyroscope()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isCarState()) { + this->handle_car_state(t, log.getCarState()); + } else if (log.isCameraOdometry()) { + this->handle_cam_odo(t, log.getCameraOdometry()); + } else if (log.isLiveCalibration()) { + this->handle_live_calib(t, log.getLiveCalibration()); + } + this->finite_check(); + this->update_reset_tracker(); +} + +kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, + bool sensorsOK, bool gpsOK, bool msgValid) { + cereal::Event::Builder evt = msg_builder.initEvent(); + evt.setValid(msgValid); + cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); + this->build_live_location(liveLoc); + liveLoc.setSensorsOK(sensorsOK); + liveLoc.setGpsOK(gpsOK); + liveLoc.setInputsOK(inputsOK); + return msg_builder.toBytes(); +} + +bool Localizer::is_gps_ok() { + return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; +} + +bool Localizer::critical_services_valid(const std::map &critical_services) { + for (auto &kv : critical_services){ + if (kv.second >= INPUT_INVALID_THRESHOLD){ + return false; + } + } + return true; +} + +bool Localizer::is_timestamp_valid(double current_time) { + double filter_time = this->kf->get_filter_time(); + if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { + LOGE("Observation timestamp is older than the max rewind threshold of the filter"); + return false; + } + return true; +} + +void Localizer::determine_gps_mode(double current_time) { + // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode + // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs + // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is + VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); + if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ + if (this->gps_mode){ + this->gps_mode = false; + this->reset_kalman(current_time); + } else { + this->input_fake_gps_observations(current_time); + } + } +} + +void Localizer::configure_gnss_source(const LocalizerGnssSource &source) { + this->gnss_source = source; + if (source == LocalizerGnssSource::UBLOX) { + this->gps_std_factor = 10.0; + this->gps_variance_factor = 1.0; + this->gps_vertical_variance_factor = 1.0; + this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; + } else { + this->gps_std_factor = 2.0; + this->gps_variance_factor = 0.0; + this->gps_vertical_variance_factor = 3.0; + this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; + } +} + +int Localizer::locationd_thread() { + Params params; + LocalizerGnssSource source; + const char* gps_location_socket; + if (params.getBool("UbloxAvailable")) { + source = LocalizerGnssSource::UBLOX; + gps_location_socket = "gpsLocationExternal"; + } else { + source = LocalizerGnssSource::QCOM; + gps_location_socket = "gpsLocation"; + } + + this->configure_gnss_source(source); + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + "carState", "accelerometer", "gyroscope"}; + + SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); + PubMaster pm({"liveLocationKalman"}); + + uint64_t cnt = 0; + bool filterInitialized = false; + const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; + for (std::string service : critical_input_services) { + this->observation_values_invalid.insert({service, 0.0}); + } + + while (!do_exit) { + sm.update(); + if (filterInitialized){ + this->observation_timings_invalid_reset(); + for (const char* service : service_list) { + if (sm.updated(service) && sm.valid(service)){ + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } + } + } else { + filterInitialized = sm.allAliveAndValid(); + } + + const char* trigger_msg = "cameraOdometry"; + if (sm.updated(trigger_msg)) { + bool inputsOK = sm.allValid() && this->are_inputs_ok(); + bool gpsOK = this->is_gps_ok(); + bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); + + // Log time to first fix + if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { + this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); + } + + MessageBuilder msg_builder; + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); + pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + + if (cnt % 1200 == 0 && gpsOK) { // once a minute + VectorXd posGeo = this->get_position_geodetic(); + std::string lastGPSPosJSON = util::string_format( + "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON); + } + cnt++; + } + } + return 0; +} + +int main() { + util::set_realtime_priority(5); + + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/sunnypilot/selfdrive/locationd/locationd.h b/sunnypilot/selfdrive/locationd/locationd.h new file mode 100644 index 0000000000..4d04492102 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/locationd.h @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +#include "sunnypilot/system/sensord/sensors/constants.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 +#include "sunnypilot/selfdrive/locationd/models/live_kf.h" + +#define POSENET_STD_HIST_HALF 20 + +enum LocalizerGnssSource { + UBLOX, QCOM +}; + +class Localizer { +public: + Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); + void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); + void finite_check(double current_time = NAN); + void time_check(double current_time = NAN); + void update_reset_tracker(); + bool is_gps_ok(); + bool critical_services_valid(const std::map &critical_services); + bool is_timestamp_valid(double current_time); + void determine_gps_mode(double current_time); + bool are_inputs_ok(); + void observation_timings_invalid_reset(); + + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); + void build_live_location(cereal::LiveLocationKalman::Builder& fix); + + Eigen::VectorXd get_position_geodetic(); + Eigen::VectorXd get_state(); + Eigen::VectorXd get_stdev(); + + void handle_msg_bytes(const char *data, const size_t size); + void handle_msg(const cereal::Event::Reader& log); + void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); + void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); + void handle_car_state(double current_time, const cereal::CarState::Reader& log); + void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); + void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + + void input_fake_gps_observations(double current_time); + +private: + std::unique_ptr kf; + + Eigen::VectorXd calib; + MatrixXdr device_from_calib; + MatrixXdr calib_from_device; + bool calibrated = false; + + double car_speed = 0.0; + double last_reset_time = NAN; + std::deque posenet_stds; + + std::unique_ptr converter; + + int64_t unix_timestamp_millis = 0; + double reset_tracker = 0.0; + bool device_fell = false; + bool gps_mode = false; + double first_valid_log_time = NAN; + double ttff = NAN; + double last_gps_msg = 0; + LocalizerGnssSource gnss_source; + bool observation_timings_invalid = false; + std::map observation_values_invalid; + bool standstill = true; + int32_t orientation_reset_count = 0; + float gps_std_factor; + float gps_variance_factor; + float gps_vertical_variance_factor; + double gps_time_offset; + Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std + + void configure_gnss_source(const LocalizerGnssSource &source); +}; diff --git a/sunnypilot/selfdrive/locationd/models/.gitignore b/sunnypilot/selfdrive/locationd/models/.gitignore new file mode 100644 index 0000000000..9ab870da89 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/.gitignore @@ -0,0 +1 @@ +generated/ diff --git a/sunnypilot/selfdrive/locationd/models/__init__.py b/sunnypilot/selfdrive/locationd/models/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/models/car_kf.py b/sunnypilot/selfdrive/locationd/models/car_kf.py new file mode 100755 index 0000000000..9964cab973 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/car_kf.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 +import math +import sys +from typing import Any + +import numpy as np + +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind +from openpilot.common.swaglog import cloudlog + +from rednose.helpers.kalmanfilter import KalmanFilter + +if __name__ == '__main__': # Generating sympy + import sympy as sp + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx + + +i = 0 + +def _slice(n): + global i + s = slice(i, i + n) + i += n + + return s + + +class States: + # Vehicle model params + STIFFNESS = _slice(1) # [-] + STEER_RATIO = _slice(1) # [-] + ANGLE_OFFSET = _slice(1) # [rad] + ANGLE_OFFSET_FAST = _slice(1) # [rad] + + VELOCITY = _slice(2) # (x, y) [m/s] + YAW_RATE = _slice(1) # [rad/s] + STEER_ANGLE = _slice(1) # [rad] + ROAD_ROLL = _slice(1) # [rad] + + +class CarKalman(KalmanFilter): + name = 'car' + + initial_x = np.array([ + 1.0, + 15.0, + 0.0, + 0.0, + + 10.0, 0.0, + 0.0, + 0.0, + 0.0 + ]) + + # process noise + Q = np.diag([ + (.05 / 100)**2, + .01**2, + math.radians(0.02)**2, + math.radians(0.25)**2, + + .1**2, .01**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + math.radians(1)**2, + ]) + P_initial = Q.copy() + + obs_noise: dict[int, Any] = { + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), + ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), + ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(0.5**2), + ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), + } + + global_vars = [ + 'mass', + 'rotational_inertia', + 'center_to_front', + 'center_to_rear', + 'stiffness_front', + 'stiffness_rear', + ] + + @staticmethod + def generate_code(generated_dir): + dim_state = CarKalman.initial_x.shape[0] + name = CarKalman.name + + # Linearized single-track lateral dynamics, equations 7.211-7.213 + # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars + # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6 + + # globals + global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] + m, j, aF, aR, cF_orig, cR_orig = global_vars + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + # Vehicle model constants + sf = state[States.STIFFNESS, :][0, 0] + + cF, cR = sf * cF_orig, sf * cR_orig + angle_offset = state[States.ANGLE_OFFSET, :][0, 0] + angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] + theta = state[States.ROAD_ROLL, :][0, 0] + sa = state[States.STEER_ANGLE, :][0, 0] + + sR = state[States.STEER_RATIO, :][0, 0] + u, v = state[States.VELOCITY, :] + r = state[States.YAW_RATE, :][0, 0] + + A = sp.Matrix(np.zeros((2, 2))) + A[0, 0] = -(cF + cR) / (m * u) + A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u + A[1, 0] = -(cF * aF - cR * aR) / (j * u) + A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u) + + B = sp.Matrix(np.zeros((2, 1))) + B[0, 0] = cF / m / sR + B[1, 0] = (cF * aF) / j / sR + + C = sp.Matrix(np.zeros((2, 1))) + C[0, 0] = ACCELERATION_DUE_TO_GRAVITY + C[1, 0] = 0 + + x = sp.Matrix([v, r]) # lateral velocity, yaw rate + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.VELOCITY.start + 1, 0] = x_dot[0] + state_dot[States.YAW_RATE.start, 0] = x_dot[1] + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + obs_eqs = [ + [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], + [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None], + [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], + [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], + [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], + [sp.Matrix([sf]), ObservationKind.STIFFNESS, None], + [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None], + ] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) + + def __init__(self, generated_dir): + dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0] + self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, + dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) + + def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): + self.filter.set_global("mass", mass) + self.filter.set_global("rotational_inertia", rotational_inertia) + self.filter.set_global("center_to_front", center_to_front) + self.filter.set_global("center_to_rear", center_to_rear) + self.filter.set_global("stiffness_front", stiffness_front) + self.filter.set_global("stiffness_rear", stiffness_rear) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + CarKalman.generate_code(generated_dir) diff --git a/sunnypilot/selfdrive/locationd/models/constants.py b/sunnypilot/selfdrive/locationd/models/constants.py new file mode 100644 index 0000000000..6d328ce6f5 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/constants.py @@ -0,0 +1,92 @@ +import os + +GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) + +class ObservationKind: + UNKNOWN = 0 + NO_OBSERVATION = 1 + GPS_NED = 2 + ODOMETRIC_SPEED = 3 + PHONE_GYRO = 4 + GPS_VEL = 5 + PSEUDORANGE_GPS = 6 + PSEUDORANGE_RATE_GPS = 7 + SPEED = 8 + NO_ROT = 9 + PHONE_ACCEL = 10 + ORB_POINT = 11 + ECEF_POS = 12 + CAMERA_ODO_TRANSLATION = 13 + CAMERA_ODO_ROTATION = 14 + ORB_FEATURES = 15 + MSCKF_TEST = 16 + FEATURE_TRACK_TEST = 17 + LANE_PT = 18 + IMU_FRAME = 19 + PSEUDORANGE_GLONASS = 20 + PSEUDORANGE_RATE_GLONASS = 21 + PSEUDORANGE = 22 + PSEUDORANGE_RATE = 23 + ECEF_VEL = 35 + ECEF_ORIENTATION_FROM_GPS = 32 + NO_ACCEL = 33 + ORB_FEATURES_WIDE = 34 + + ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] + ROAD_FRAME_YAW_RATE = 25 # [rad/s] + STEER_ANGLE = 26 # [rad] + ANGLE_OFFSET_FAST = 27 # [rad] + STIFFNESS = 28 # [-] + STEER_RATIO = 29 # [-] + ROAD_FRAME_X_SPEED = 30 # (x) [m/s] + ROAD_ROLL = 31 # [rad] + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + 'pseudorange', + 'pseudorange rate', + + 'Road Frame x,y speed', + 'Road Frame yaw rate', + 'Steer Angle', + 'Fast Angle Offset', + 'Stiffness', + 'Steer Ratio', + 'Road Frame x speed', + 'Road Roll', + 'ECEF orientation from GPS', + 'NO accel', + 'ORB features wide camera', + 'ECEF_VEL', + ] + + @classmethod + def to_string(cls, kind): + return cls.names[kind] + + +SAT_OBS = [ObservationKind.PSEUDORANGE_GPS, + ObservationKind.PSEUDORANGE_RATE_GPS, + ObservationKind.PSEUDORANGE_GLONASS, + ObservationKind.PSEUDORANGE_RATE_GLONASS] diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.cc b/sunnypilot/selfdrive/locationd/models/live_kf.cc new file mode 100644 index 0000000000..7ef6be638e --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,122 @@ +#include "sunnypilot/selfdrive/locationd/models/live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { + return Eigen::Map((double*)vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(const MatrixXdr &mat) { + return Eigen::Map((double*)mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(const std::vector &vec_vec) { + std::vector> res; + for (const Eigen::VectorXd &vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(const std::vector &mat_vec) { + std::vector> res; + for (const MatrixXdr &mat : mat_vec) { + res.push_back(get_mapmat(mat)); + } + return res; +} + +LiveKalman::LiveKalman() { + this->dim_state = live_initial_x.rows(); + this->dim_state_err = live_initial_P_diag.rows(); + + this->initial_x = live_initial_x; + this->initial_P = live_initial_P_diag.asDiagonal(); + this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal(); + this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal(); + this->reset_orientation_P = live_reset_orientation_diag.asDiagonal(); + this->Q = live_Q_diag.asDiagonal(); + for (auto& pair : live_obs_noise_diag) { + this->obs_noise[pair.first] = pair.second.asDiagonal(); + } + + // init filter + this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), + get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), + std::vector{3}, std::vector(), 0.8); +} + +void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) { + MatrixXdr covs = covs_diag.asDiagonal(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) { + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +void LiveKalman::init_state(const VectorXd &state, double filter_time) { + MatrixXdr covs = this->filter->covs(); + this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); +} + +VectorXd LiveKalman::get_x() { + return this->filter->state(); +} + +MatrixXdr LiveKalman::get_P() { + return this->filter->covs(); +} + +double LiveKalman::get_filter_time() { + return this->filter->get_filter_time(); +} + +std::vector LiveKalman::get_R(int kind, int n) { + std::vector R; + for (int i = 0; i < n; i++) { + R.push_back(this->obs_noise[kind]); + } + return R; +} + +std::optional LiveKalman::predict_and_observe(double t, int kind, const std::vector &meas, std::vector R) { + std::optional r; + if (R.size() == 0) { + R = this->get_R(kind, meas.size()); + } + r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); + return r; +} + +void LiveKalman::predict(double t) { + this->filter->predict(t); +} + +const Eigen::VectorXd &LiveKalman::get_initial_x() { + return this->initial_x; +} + +const MatrixXdr &LiveKalman::get_initial_P() { + return this->initial_P; +} + +const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() { + return this->fake_gps_pos_cov; +} + +const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() { + return this->fake_gps_vel_cov; +} + +const MatrixXdr &LiveKalman::get_reset_orientation_P() { + return this->reset_orientation_P; +} + +MatrixXdr LiveKalman::H(const VectorXd &in) { + assert(in.size() == 6); + Matrix res; + this->filter->get_extra_routine("H")((double*)in.data(), res.data()); + return res; +} diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.h b/sunnypilot/selfdrive/locationd/models/live_kf.h new file mode 100644 index 0000000000..e4b3e326b3 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "generated/live_kf_constants.h" +#include "rednose/helpers/ekf_sym.h" + +#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth) + +using namespace EKFS; + +Eigen::Map get_mapvec(const Eigen::VectorXd &vec); +Eigen::Map get_mapmat(const MatrixXdr &mat); +std::vector> get_vec_mapvec(const std::vector &vec_vec); +std::vector> get_vec_mapmat(const std::vector &mat_vec); + +class LiveKalman { +public: + LiveKalman(); + + void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time); + void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time); + void init_state(const Eigen::VectorXd &state, double filter_time); + + Eigen::VectorXd get_x(); + MatrixXdr get_P(); + double get_filter_time(); + std::vector get_R(int kind, int n); + + std::optional predict_and_observe(double t, int kind, const std::vector &meas, std::vector R = {}); + std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); + std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); + std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind); + void predict(double t); + + const Eigen::VectorXd &get_initial_x(); + const MatrixXdr &get_initial_P(); + const MatrixXdr &get_fake_gps_pos_cov(); + const MatrixXdr &get_fake_gps_vel_cov(); + const MatrixXdr &get_reset_orientation_P(); + + MatrixXdr H(const Eigen::VectorXd &in); + +private: + std::string name = "live"; + + std::shared_ptr filter; + + int dim_state; + int dim_state_err; + + Eigen::VectorXd initial_x; + MatrixXdr initial_P; + MatrixXdr fake_gps_pos_cov; + MatrixXdr fake_gps_vel_cov; + MatrixXdr reset_orientation_P; + MatrixXdr Q; // process noise + std::unordered_map obs_noise; +}; diff --git a/sunnypilot/selfdrive/locationd/models/live_kf.py b/sunnypilot/selfdrive/locationd/models/live_kf.py new file mode 100755 index 0000000000..e5626b8f28 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/models/live_kf.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +import sys +import os +import numpy as np + +from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind + +import sympy as sp +import inspect +from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate +from rednose.helpers.ekf_sym import gen_code + +EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) + + +def numpy2eigenstring(arr): + assert(len(arr.shape) == 1) + arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') + return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" + + +class States: + ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters + ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef + ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s + ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s + GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases + ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 + ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 + + # Error-state has different slices because it is an ESKF + ECEF_POS_ERR = slice(0, 3) + ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error + ECEF_VELOCITY_ERR = slice(6, 9) + ANGULAR_VELOCITY_ERR = slice(9, 12) + GYRO_BIAS_ERR = slice(12, 15) + ACCELERATION_ERR = slice(15, 18) + ACC_BIAS_ERR = slice(18, 21) + + +class LiveKalman: + name = 'live' + + initial_x = np.array([3.88e6, -3.37e6, 3.76e6, + 0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0]) + + # state covariance + initial_P_diag = np.array([10**2, 10**2, 10**2, + 0.01**2, 0.01**2, 0.01**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + 1**2, 1**2, 1**2, + 100**2, 100**2, 100**2, + 0.01**2, 0.01**2, 0.01**2]) + + # state covariance when resetting midway in a segment + reset_orientation_diag = np.array([1**2, 1**2, 1**2]) + + # fake observation covariance, to ensure the uncertainty estimate of the filter is under control + fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2]) + fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2]) + + # process noise + Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, + 0.001**2, 0.001**2, 0.001**2, + 0.01**2, 0.01**2, 0.01**2, + 0.1**2, 0.1**2, 0.1**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + 3**2, 3**2, 3**2, + 0.005**2, 0.005**2, 0.005**2]) + + obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), + ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), + ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), + ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} + + @staticmethod + def generate_code(generated_dir): + name = LiveKalman.name + dim_state = LiveKalman.initial_x.shape[0] + dim_state_err = LiveKalman.initial_P_diag.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x, y, z = state[States.ECEF_POS, :] + q = state[States.ECEF_ORIENTATION, :] + v = state[States.ECEF_VELOCITY, :] + vx, vy, vz = v + omega = state[States.ANGULAR_VELOCITY, :] + vroll, vpitch, vyaw = omega + roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] + acceleration = state[States.ACCELERATION, :] + acc_bias = state[States.ACC_BIAS, :] + + dt = sp.Symbol('dt') + + # calibration and attitude rotation matrices + quat_rot = quat_rotate(*q) + + # Got the quat predict equations from here + # A New Quaternion-Based Kalman Filter for + # Real-Time Attitude Estimation Using the Two-Step + # Geometrically-Intuitive Correction Algorithm + A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], + [vroll, 0, vyaw, -vpitch], + [vpitch, -vyaw, 0, vroll], + [vyaw, vpitch, -vroll, 0]]) + q_dot = A * q + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.ECEF_POS, :] = v + state_dot[States.ECEF_ORIENTATION, :] = q_dot + state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) + state_err = sp.Matrix(state_err_sym) + quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] + v_err = state_err[States.ECEF_VELOCITY_ERR, :] + omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] + acceleration_err = state_err[States.ACCELERATION_ERR, :] + + # Time derivative of the state error as a function of state error and state + quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) + q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) + state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) + state_err_dot[States.ECEF_POS_ERR, :] = v_err + state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot + state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) + f_err_sym = state_err + dt * state_err_dot + + # Observation matrix modifier + H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) + H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) + H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] + H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) + + # these error functions are defined so that say there + # is a nominal x and true x: + # true x = err_function(nominal x, delta x) + # delta x = inv_err_function(nominal x, true x) + nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) + true_x = sp.MatrixSymbol('true_x', dim_state, 1) + delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) + + err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) + delta_quat = sp.Matrix(np.ones(4)) + delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) + err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) + err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat + err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) + + inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) + inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) + delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] + inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) + inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) + + eskf_params = [[err_function_sym, nom_x, delta_x], + [inv_err_function_sym, nom_x, true_x], + H_mod_sym, f_err_sym, state_err_sym] + # + # Observation functions + # + h_gyro_sym = sp.Matrix([ + vroll + roll_bias, + vpitch + pitch_bias, + vyaw + yaw_bias]) + + pos = sp.Matrix([x, y, z]) + gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) + h_acc_sym = (gravity + acceleration + acc_bias) + h_acc_stationary_sym = acceleration + h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) + h_pos_sym = sp.Matrix([x, y, z]) + h_vel_sym = sp.Matrix([vx, vy, vz]) + h_orientation_sym = q + h_relative_motion = sp.Matrix(quat_rot.T * v) + + obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], + [h_phone_rot_sym, ObservationKind.NO_ROT, None], + [h_acc_sym, ObservationKind.PHONE_ACCEL, None], + [h_pos_sym, ObservationKind.ECEF_POS, None], + [h_vel_sym, ObservationKind.ECEF_VEL, None], + [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None], + [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], + [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], + [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] + + # this returns a sympy routine for the jacobian of the observation function of the local vel + in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz + h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) + extra_routines = [('H', h.jacobian(in_vec), [in_vec])] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) + + # write constants to extra header file for use in cpp + live_kf_header = "#pragma once\n\n" + live_kf_header += "#include \n" + live_kf_header += "#include \n\n" + for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)): + assert(slc.step is None) # unsupported + live_kf_header += f'#define STATE_{state}_START {slc.start}\n' + live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' + live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' + live_kf_header += "\n" + + for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)): + live_kf_header += f'#define OBSERVATION_{kind} {val}\n' + live_kf_header += "\n" + + live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" + live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n" + live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" + live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n" + for kind, noise in LiveKalman.obs_noise_diag.items(): + live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" + live_kf_header += "};\n\n" + + open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + LiveKalman.generate_code(generated_dir) diff --git a/sunnypilot/selfdrive/locationd/tests/.gitignore b/sunnypilot/selfdrive/locationd/tests/.gitignore new file mode 100644 index 0000000000..89f9ac04aa --- /dev/null +++ b/sunnypilot/selfdrive/locationd/tests/.gitignore @@ -0,0 +1 @@ +out/ diff --git a/sunnypilot/selfdrive/locationd/tests/__init__.py b/sunnypilot/selfdrive/locationd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/locationd/tests/test_locationd.py b/sunnypilot/selfdrive/locationd/tests/test_locationd.py new file mode 100644 index 0000000000..c409f5b5a5 --- /dev/null +++ b/sunnypilot/selfdrive/locationd/tests/test_locationd.py @@ -0,0 +1,89 @@ +import pytest +import json +import random +import time +import capnp + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.params import Params +from openpilot.common.transformations.coordinates import ecef2geodetic + +from openpilot.system.manager.process_config import managed_processes + + +class TestLocationdProc: + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + 'accelerometer', 'gyroscope', 'magnetometer'] + + def setup_method(self): + self.pm = messaging.PubMaster(self.LLD_MSGS) + + self.params = Params() + self.params.put_bool("UbloxAvailable", True) + managed_processes['locationd_llk'].prepare() + managed_processes['locationd_llk'].start() + + def teardown_method(self): + managed_processes['locationd_llk'].stop() + + def get_msg(self, name, t): + try: + msg = messaging.new_message(name) + except capnp.lib.capnp.KjException: + msg = messaging.new_message(name, 0) + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.hasFix = True + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True + elif name == 'cameraOdometry': + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] + msg.logMonoTime = t + msg.valid = True + return msg + + def test_params_gps(self): + random.seed(123489234) + self.params.remove('LastGPSPositionLLK') + + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + + # get fake messages at the correct frequency, listed in services.py + msgs = [] + for sec in range(65): + for name in self.LLD_MSGS: + for j in range(int(SERVICE_LIST[name].frequency)): + msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9))) + + for msg in sorted(msgs, key=lambda x: x.logMonoTime): + self.pm.send(msg.which(), msg) + if msg.which() == "cameraOdometry": + self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005) + time.sleep(1) # wait for async params write + + lastGPS = json.loads(self.params.get('LastGPSPositionLLK')) + assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001) + assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001) + assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001) diff --git a/sunnypilot/selfdrive/locationd/torqued_ext.py b/sunnypilot/selfdrive/locationd/torqued_ext.py new file mode 100644 index 0000000000..58a23da00e --- /dev/null +++ b/sunnypilot/selfdrive/locationd/torqued_ext.py @@ -0,0 +1,65 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +import numpy as np + +from cereal import car + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD + +RELAXED_MIN_BUCKET_POINTS = np.array([1, 200, 300, 500, 500, 300, 200, 1]) + +ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda'] + + + +class TorqueEstimatorExt: + def __init__(self, CP: car.CarParams): + self.CP = CP + self._params = Params() + self.frame = -1 + + self.enforce_torque_control_toggle = self._params.get_bool("EnforceTorqueControl") # only during init + self.use_params = self.CP.brand in ALLOWED_CARS and self.CP.lateralTuning.which() == 'torque' + self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle") + self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled") + self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS + self.factor_sanity = 0.0 + self.friction_sanity = 0.0 + self.offline_latAccelFactor = 0.0 + self.offline_friction = 0.0 + + def initialize_custom_params(self, decimated=False): + self.update_use_params() + + if self.enforce_torque_control_toggle: + if self._params.get_bool("LiveTorqueParamsRelaxedToggle"): + self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS / (10 if decimated else 1) + self.factor_sanity = 0.5 if decimated else 1.0 + self.friction_sanity = 0.8 if decimated else 1.0 + + if self._params.get_bool("CustomTorqueParams"): + self.offline_latAccelFactor = float(self._params.get("TorqueParamsOverrideLatAccelFactor", return_default=True)) + self.offline_friction = float(self._params.get("TorqueParamsOverrideFriction", return_default=True)) + + def _update_params(self): + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle") + self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled") + + def update_use_params(self): + self._update_params() + + if self.enforce_torque_control_toggle: + if self.torque_override_enabled: + self.use_params = False + else: + self.use_params = self.use_live_torque_params + + self.frame += 1 diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 7c723397fb..e9f4b29bae 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -1,6 +1,10 @@ +import cereal.messaging as messaging from cereal import log, car, custom +from openpilot.common.constants import CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target AlertSize = log.SelfdriveState.AlertSize @@ -14,6 +18,49 @@ EventNameSP = custom.OnroadEventSP.EventName EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()} +def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit + speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) + message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' + return Alert( + message, + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.) + + +def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH + speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast + speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv) + + if CP.openpilotLongitudinalControl and CP.pcmCruise: + # PCM long + cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric] + pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high + pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv) + speed_unit = "km/h" if metric else "mph" + alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate" + else: + # Non PCM long + v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS + + req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric) + arrow_str = "" + if req_plus: + arrow_str = "RES/+" + elif req_minus: + arrow_str = "SET/-" + + alert_2_str = f"Operate the {arrow_str} cruise control button to activate" + + return Alert( + "Speed Limit Assist: Activation Required", + alert_2_str, + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + + class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -148,5 +195,41 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), - } + }, + + EventNameSP.speedLimitActive: { + ET.WARNING: Alert( + "Automatically adjusting to the posted speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, + + EventNameSP.speedLimitChanged: { + ET.WARNING: Alert( + "Set speed changed", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, + + EventNameSP.speedLimitPreActive: { + ET.WARNING: speed_limit_pre_active_alert, + }, + + EventNameSP.speedLimitPending: { + ET.WARNING: Alert( + "Automatically adjusting to the last speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), + }, + + EventNameSP.e2eChime: { + ET.PERMANENT: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1), + }, } diff --git a/sunnypilot/sunnylink/api.py b/sunnypilot/sunnylink/api.py index b7838a40b0..cd2a358c60 100644 --- a/sunnypilot/sunnylink/api.py +++ b/sunnypilot/sunnylink/api.py @@ -147,7 +147,7 @@ class SunnylinkApi(BaseApi): self.params.put("SunnylinkDongleId", sunnylink_dongle_id or UNREGISTERED_SUNNYLINK_DONGLE_ID) # Set the last ping time to the current time since we were just talking to the API - last_ping = int(time.monotonic() * 1e9) if successful_registration else start_time + last_ping = int((time.monotonic() if successful_registration else start_time) * 1e9) Params().put("LastSunnylinkPingTime", last_ping) # Disable sunnylink if registration was not successful diff --git a/sunnypilot/sunnylink/athena/sunnylinkd.py b/sunnypilot/sunnylink/athena/sunnylinkd.py index 25a77c367b..d1b38d656f 100755 --- a/sunnypilot/sunnylink/athena/sunnylinkd.py +++ b/sunnypilot/sunnylink/athena/sunnylinkd.py @@ -178,12 +178,29 @@ def getParamsAllKeys() -> list[str]: return keys +@dispatcher.add_method +def getParamsAllKeysV1() -> dict[str, str]: + available_keys: list[str] = [k.decode('utf-8') for k in Params().all_keys()] + + params_dict: dict[str, list[dict[str, str | bool | int | None]]] = {"params": []} + for key in available_keys: + value = get_param_as_byte(key, get_default=True) + params_dict["params"].append({ + "key": key, + "type": int(params.get_type(key).value), + "default_value": base64.b64encode(value).decode('utf-8') if value else None, + }) + + return {"keys": json.dumps(params_dict.get("params", []))} + + @dispatcher.add_method def getParams(params_keys: list[str], compression: bool = False) -> str | dict[str, str]: params = Params() + available_keys: list[str] = [k.decode('utf-8') for k in Params().all_keys()] try: - param_keys_validated = [key for key in params_keys if key in getParamsAllKeys()] + param_keys_validated = [key for key in params_keys if key in available_keys] params_dict: dict[str, list[dict[str, str | bool | int]]] = {"params": []} for key in param_keys_validated: value = get_param_as_byte(key) diff --git a/sunnypilot/sunnylink/registration_manager.py b/sunnypilot/sunnylink/registration_manager.py index 34323c2789..805d2482db 100755 --- a/sunnypilot/sunnylink/registration_manager.py +++ b/sunnypilot/sunnylink/registration_manager.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import time +from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.common.swaglog import cloudlog @@ -12,17 +13,22 @@ NetworkType = log.DeviceState.NetworkType def main(): """The main method is expected to be called by the manager when the device boots up.""" - rk = Ratekeeper(.5) - sm = messaging.SubMaster(['deviceState'], poll='deviceState') - while True: - sm.update(1000) - if sm['deviceState'].networkType != NetworkType.none: - break + try: + rk = Ratekeeper(.5) + sm = messaging.SubMaster(['deviceState'], poll='deviceState') + while True: + sm.update(1000) + if sm['deviceState'].networkType != NetworkType.none: + break - cloudlog.info(f"Waiting to become online... {time.monotonic()}") - rk.keep_time() + cloudlog.info(f"Waiting to become online... {time.monotonic()}") + rk.keep_time() - register_sunnylink() + register_sunnylink() + except Exception: + cloudlog.exception("Sunnylink registration failed") + Params().put_bool("SunnylinkTempFault", True) + raise if __name__ == "__main__": diff --git a/sunnypilot/sunnylink/utils.py b/sunnypilot/sunnylink/utils.py index 91c0788795..e86a795170 100644 --- a/sunnypilot/sunnylink/utils.py +++ b/sunnypilot/sunnylink/utils.py @@ -6,31 +6,32 @@ from openpilot.common.params import Params, ParamKeyType from openpilot.system.version import is_prebuilt -def get_sunnylink_status(params=None) -> tuple[bool, bool]: +def get_sunnylink_status(params=None) -> tuple[bool, bool, bool]: """Get the status of Sunnylink on the device. Returns a tuple of (is_sunnylink_enabled, is_registered).""" params = params or Params() is_sunnylink_enabled = params.get_bool("SunnylinkEnabled") is_registered = params.get("SunnylinkDongleId") not in (None, UNREGISTERED_SUNNYLINK_DONGLE_ID) - return is_sunnylink_enabled, is_registered + is_on_temporary_fault = params.get_bool("SunnylinkTempFault") + return is_sunnylink_enabled, is_registered, is_on_temporary_fault def sunnylink_ready(params=None) -> bool: """Check if the device is ready to communicate with Sunnylink. That means it is enabled and registered.""" params = params or Params() - is_sunnylink_enabled, is_registered = get_sunnylink_status(params) - return is_sunnylink_enabled and is_registered + is_sunnylink_enabled, is_registered, is_on_temporary_fault = get_sunnylink_status(params) + return is_sunnylink_enabled and is_registered and not is_on_temporary_fault def use_sunnylink_uploader(params) -> bool: """Check if the device is ready to use Sunnylink and the uploader is enabled.""" - return sunnylink_ready(params) and params.get_bool("EnableSunnylinkUploader") + return not params.get_bool("NetworkMetered") and sunnylink_ready(params) and params.get_bool("EnableSunnylinkUploader") def sunnylink_need_register(params=None) -> bool: """Check if the device needs to be registered with Sunnylink.""" params = params or Params() - is_sunnylink_enabled, is_registered = get_sunnylink_status(params) - return is_sunnylink_enabled and not is_registered + is_sunnylink_enabled, is_registered, is_on_temporary_fault = get_sunnylink_status(params) + return is_sunnylink_enabled and not is_registered and not is_on_temporary_fault def register_sunnylink(): @@ -47,8 +48,12 @@ def register_sunnylink(): "timeout": 60 } - sunnylink_id = SunnylinkApi(None).register_device(None, **extra_args) - print(f"SunnyLinkId: {sunnylink_id}") + try: + sunnylink_id = SunnylinkApi(None).register_device(None, **extra_args) + print(f"SunnyLinkId: {sunnylink_id}") + except Exception: + Params().put_bool("SunnylinkTempFault", True) + raise def get_api_token(): @@ -60,14 +65,20 @@ def get_api_token(): print(f"API Token: {token}") -def get_param_as_byte(param_name: str, params=None) -> bytes | None: +def get_param_as_byte(param_name: str, params=None, get_default=False) -> bytes | None: """Get a parameter as bytes. Returns None if the parameter does not exist.""" - params = params or Params() # Use existing Params instance if provided - param = params.get(param_name) + params = params or Params() + param = params.get(param_name) if not get_default else params.get_default_value(param_name) + if param is None: return None param_type = params.get_type(param_name) + return _to_bytes(param, param_type) + + +def _to_bytes(param: bytes, param_type: ParamKeyType) -> bytes | None: + """Convert a parameter value to bytes based on its type.""" if param_type == ParamKeyType.BYTES: return bytes(param) elif param_type == ParamKeyType.JSON: diff --git a/sunnypilot/system/sensord/.gitignore b/sunnypilot/system/sensord/.gitignore new file mode 100644 index 0000000000..e17675e254 --- /dev/null +++ b/sunnypilot/system/sensord/.gitignore @@ -0,0 +1 @@ +sensord diff --git a/sunnypilot/system/sensord/SConscript b/sunnypilot/system/sensord/SConscript new file mode 100644 index 0000000000..1222f0bfcd --- /dev/null +++ b/sunnypilot/system/sensord/SConscript @@ -0,0 +1,13 @@ +Import('env', 'arch', 'common', 'messaging') + +sensors = [ + 'sensors/i2c_sensor.cc', + 'sensors/lsm6ds3_accel.cc', + 'sensors/lsm6ds3_gyro.cc', + 'sensors/lsm6ds3_temp.cc', + 'sensors/mmc5603nj_magn.cc', +] +libs = [common, messaging, 'pthread'] +if arch == "larch64": + libs.append('i2c') +env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/sunnypilot/system/sensord/sensors/bmx055_accel.cc b/sunnypilot/system/sensord/sensors/bmx055_accel.cc new file mode 100644 index 0000000000..5db10c3b5b --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_accel.cc @@ -0,0 +1,85 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Accel::init() { + int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}); + if (ret == -1) { + goto fail; + } + + ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + + // bmx055 accel has a 1.3ms wakeup time from deep suspend mode + util::sleep_for(10); + + // High bandwidth + // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); + // if (ret < 0) { + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE); + if (ret < 0) { + goto fail; + } + + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); + if (ret < 0) { + goto fail; + } + + enabled = true; + +fail: + return ret; +} + +int BMX055_Accel::shutdown() { + if (!enabled) return 0; + + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 ACCEL in deep suspend mode!"); + } + + return ret; +} + +bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); + assert(len == 6); + + // 12 bit = +-2g + float scale = 9.81 * 2.0f / (1 << 11); + float x = -read_12_bit(buffer[0], buffer[1]) * scale; + float y = -read_12_bit(buffer[2], buffer[3]) * scale; + float z = read_12_bit(buffer[4], buffer[5]) * scale; + + auto event = msg.initEvent().initAccelerometer2(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + auto svec = event.initAcceleration(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_accel.h b/sunnypilot/system/sensord/sensors/bmx055_accel.h new file mode 100644 index 0000000000..4aea163c8b --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_accel.h @@ -0,0 +1,41 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_ACCEL_I2C_ADDR 0x18 + +// Registers of the chip +#define BMX055_ACCEL_I2C_REG_ID 0x00 +#define BMX055_ACCEL_I2C_REG_X_LSB 0x02 +#define BMX055_ACCEL_I2C_REG_TEMP 0x08 +#define BMX055_ACCEL_I2C_REG_BW 0x10 +#define BMX055_ACCEL_I2C_REG_PMU 0x11 +#define BMX055_ACCEL_I2C_REG_HBW 0x13 +#define BMX055_ACCEL_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_ACCEL_CHIP_ID 0xFA + +#define BMX055_ACCEL_HBW_ENABLE 0b10000000 +#define BMX055_ACCEL_HBW_DISABLE 0b00000000 +#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000 +#define BMX055_ACCEL_NORMAL_MODE 0b00000000 + +#define BMX055_ACCEL_BW_7_81HZ 0b01000 +#define BMX055_ACCEL_BW_15_63HZ 0b01001 +#define BMX055_ACCEL_BW_31_25HZ 0b01010 +#define BMX055_ACCEL_BW_62_5HZ 0b01011 +#define BMX055_ACCEL_BW_125HZ 0b01100 +#define BMX055_ACCEL_BW_250HZ 0b01101 +#define BMX055_ACCEL_BW_500HZ 0b01110 +#define BMX055_ACCEL_BW_1000HZ 0b01111 + +class BMX055_Accel : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Accel(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_gyro.cc b/sunnypilot/system/sensord/sensors/bmx055_gyro.cc new file mode 100644 index 0000000000..4701c70d71 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.cc @@ -0,0 +1,92 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h" + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Gyro::init() { + int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID}); + if (ret == -1) return -1; + + ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE); + if (ret < 0) { + goto fail; + } + // bmx055 gyro has a 30ms wakeup time from deep suspend mode + util::sleep_for(50); + + // High bandwidth + // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); + // if (ret < 0) { + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE); + if (ret < 0) { + goto fail; + } + + // 116 Hz filter + ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ); + if (ret < 0) { + goto fail; + } + + // +- 125 deg/s range + ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125); + if (ret < 0) { + goto fail; + } + + enabled = true; + +fail: + return ret; +} + +int BMX055_Gyro::shutdown() { + if (!enabled) return 0; + + // enter deep suspend mode (lowest power mode) + int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND); + if (ret < 0) { + LOGE("Could not move BMX055 GYRO in deep suspend mode!"); + } + + return ret; +} + +bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); + assert(len == 6); + + // 16 bit = +- 125 deg/s + float scale = 125.0f / (1 << 15); + float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + auto event = msg.initEvent().initGyroscope2(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + auto svec = event.initGyroUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_gyro.h b/sunnypilot/system/sensord/sensors/bmx055_gyro.h new file mode 100644 index 0000000000..489184bb18 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_gyro.h @@ -0,0 +1,41 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_GYRO_I2C_ADDR 0x68 + +// Registers of the chip +#define BMX055_GYRO_I2C_REG_ID 0x00 +#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02 +#define BMX055_GYRO_I2C_REG_RANGE 0x0F +#define BMX055_GYRO_I2C_REG_BW 0x10 +#define BMX055_GYRO_I2C_REG_LPM1 0x11 +#define BMX055_GYRO_I2C_REG_HBW 0x13 +#define BMX055_GYRO_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_GYRO_CHIP_ID 0x0F + +#define BMX055_GYRO_HBW_ENABLE 0b10000000 +#define BMX055_GYRO_HBW_DISABLE 0b00000000 +#define BMX055_GYRO_DEEP_SUSPEND 0b00100000 +#define BMX055_GYRO_NORMAL_MODE 0b00000000 + +#define BMX055_GYRO_RANGE_2000 0b000 +#define BMX055_GYRO_RANGE_1000 0b001 +#define BMX055_GYRO_RANGE_500 0b010 +#define BMX055_GYRO_RANGE_250 0b011 +#define BMX055_GYRO_RANGE_125 0b100 + +#define BMX055_GYRO_BW_116HZ 0b0010 + + +class BMX055_Gyro : public I2CSensor { + uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;} +public: + BMX055_Gyro(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_magn.cc b/sunnypilot/system/sensord/sensors/bmx055_magn.cc new file mode 100644 index 0000000000..00b35aa136 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_magn.cc @@ -0,0 +1,258 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_magn.h" + +#include + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) { + uint16_t process_comp_x0 = data_rhall; + int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384; + uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_x2); + int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); + int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128)); + int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128); + int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5; + int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); + int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0))); + int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); + int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; + retval = ((int16_t)(process_comp_x10 / 8192)); + retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) { + uint16_t process_comp_y0 = trim_data.dig_xyz1; + int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0; + uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_y2); + int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); + int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128); + int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128)); + int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); + int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0))); + int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); + int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); + retval = (int16_t)(process_comp_y9 / 8192); + retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) { + int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1); + int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4; + int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768); + int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2); + int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); + int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4)); + + /* saturate result to +/- 2 micro-tesla */ + retval = std::clamp(retval, -32767, 32767); + + /* Conversion of LSB to micro-tesla*/ + retval = retval / 16; + + return (int16_t)retval; +} + +BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Magn::init() { + uint8_t trim_x1y1[2] = {0}; + uint8_t trim_x2y2[2] = {0}; + uint8_t trim_xy1xy2[2] = {0}; + uint8_t trim_z1[2] = {0}; + uint8_t trim_z2[2] = {0}; + uint8_t trim_z3[2] = {0}; + uint8_t trim_z4[2] = {0}; + uint8_t trim_xyz1[2] = {0}; + + // suspend -> sleep + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + if (ret < 0) { + LOGD("Enabling power failed: %d", ret); + goto fail; + } + util::sleep_for(5); // wait until the chip is powered on + + ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID}); + if (ret == -1) { + goto fail; + } + + // Load magnetometer trim + ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2); + if (ret < 0) goto fail; + ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2); + if (ret < 0) goto fail; + + // Read trim data + trim_data.dig_x1 = trim_x1y1[0]; + trim_data.dig_y1 = trim_x1y1[1]; + + trim_data.dig_x2 = trim_x2y2[0]; + trim_data.dig_y2 = trim_x2y2[1]; + + trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped + trim_data.dig_xy2 = trim_xy1xy2[0]; + + trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]); + trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]); + trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]); + trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]); + + trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f); + assert(trim_data.dig_xyz1 != 0); + + perform_self_test(); + + // f_max = 1 / (145us * nXY + 500us * NZ + 980us) + // Chose NXY = 7, NZ = 12, which gives 125 Hz, + // and has the same ratio as the high accuracy preset + ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2); + if (ret < 0) { + goto fail; + } + + ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1); + if (ret < 0) { + goto fail; + } + + enabled = true; + return 0; + + fail: + return ret; +} + +int BMX055_Magn::shutdown() { + if (!enabled) return 0; + + // move to suspend mode + int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0); + if (ret < 0) { + LOGE("Could not move BMX055 MAGN in suspend mode!"); + } + + return ret; +} + +bool BMX055_Magn::perform_self_test() { + uint8_t buffer[8]; + int16_t x, y; + int16_t neg_z, pos_z; + + // Increase z reps for less false positives (~30 Hz ODR) + set_register(BMX055_MAGN_I2C_REG_REPXY, 1); + set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1); + + // Clean existing measurement + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + + uint8_t forced = BMX055_MAGN_FORCED; + + // Negative current + set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6)); + util::sleep_for(100); + + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + parse_xyz(buffer, &x, &y, &neg_z); + + // Positive current + set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6)); + util::sleep_for(100); + + read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + parse_xyz(buffer, &x, &y, &pos_z); + + // Put back in normal mode + set_register(BMX055_MAGN_I2C_REG_MAG, 0); + + int16_t diff = pos_z - neg_z; + bool passed = (diff > 180) && (diff < 240); + + if (!passed) { + LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff); + } + + return passed; +} + +bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) { + bool ready = buffer[6] & 0x1; + if (ready) { + int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3; + int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3; + int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1; + uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2; + assert(data_r != 0); + + *x = compensate_x(trim_data, mdata_x, data_r); + *y = compensate_y(trim_data, mdata_y, data_r); + *z = compensate_z(trim_data, mdata_z, data_r); + } + return ready; +} + + +bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[8]; + int16_t _x, _y, x, y, z; + + int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + bool parsed = parse_xyz(buffer, &_x, &_y, &z); + if (parsed) { + + auto event = msg.initEvent().initMagnetometer(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(2); + event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); + event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); + event.setTimestamp(start_time); + + // Move magnetometer into same reference frame as accel/gryo + x = -_y; + y = _x; + + // Axis convention + x = -x; + y = -y; + + float xyz[] = {(float)x, (float)y, (float)z}; + auto svec = event.initMagneticUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + } + + // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes + // up to 30 Hz. Therefore we put in forced mode, and request measurements + // at a 100 Hz. When reading the registers we have to check the ready bit + // To verify the measurement was completed this cycle. + set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED); + + return parsed; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_magn.h b/sunnypilot/system/sensord/sensors/bmx055_magn.h new file mode 100644 index 0000000000..7b1f7853d0 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_magn.h @@ -0,0 +1,64 @@ +#pragma once +#include + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define BMX055_MAGN_I2C_ADDR 0x10 + +// Registers of the chip +#define BMX055_MAGN_I2C_REG_ID 0x40 +#define BMX055_MAGN_I2C_REG_PWR_0 0x4B +#define BMX055_MAGN_I2C_REG_MAG 0x4C +#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42 +#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48 +#define BMX055_MAGN_I2C_REG_REPXY 0x51 +#define BMX055_MAGN_I2C_REG_REPZ 0x52 + +#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D +#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E +#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62 +#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63 +#define BMX055_MAGN_I2C_REG_DIG_X2 0x64 +#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65 +#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68 +#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69 +#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A +#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B +#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C +#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D +#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E +#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F +#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70 +#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71 + +// Constants +#define BMX055_MAGN_CHIP_ID 0x32 +#define BMX055_MAGN_FORCED (0b01 << 1) + +struct trim_data_t { + int8_t dig_x1; + int8_t dig_y1; + int8_t dig_x2; + int8_t dig_y2; + uint16_t dig_z1; + int16_t dig_z2; + int16_t dig_z3; + int16_t dig_z4; + uint8_t dig_xy1; + int8_t dig_xy2; + uint16_t dig_xyz1; +}; + + +class BMX055_Magn : public I2CSensor{ + uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} + trim_data_t trim_data = {0}; + bool perform_self_test(); + bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z); +public: + BMX055_Magn(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/bmx055_temp.cc b/sunnypilot/system/sensord/sensors/bmx055_temp.cc new file mode 100644 index 0000000000..3932c66fa6 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_temp.cc @@ -0,0 +1,31 @@ +#include "sunnypilot/system/sensord/sensors/bmx055_temp.h" + +#include + +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "common/swaglog.h" +#include "common/timing.h" + +BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Temp::init() { + return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0; +} + +bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[1]; + int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float temp = 23.0f + int8_t(buffer[0]) / 2.0f; + + auto event = msg.initEvent().initTemperatureSensor(); + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/bmx055_temp.h b/sunnypilot/system/sensord/sensors/bmx055_temp.h new file mode 100644 index 0000000000..02c48dc92c --- /dev/null +++ b/sunnypilot/system/sensord/sensors/bmx055_temp.h @@ -0,0 +1,13 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +class BMX055_Temp : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Temp(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } +}; diff --git a/sunnypilot/system/sensord/sensors/constants.h b/sunnypilot/system/sensord/sensors/constants.h new file mode 100644 index 0000000000..c216f838a5 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/constants.h @@ -0,0 +1,18 @@ +#pragma once + + +#define SENSOR_ACCELEROMETER 1 +#define SENSOR_MAGNETOMETER 2 +#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 +#define SENSOR_GYRO 4 +#define SENSOR_GYRO_UNCALIBRATED 5 +#define SENSOR_LIGHT 7 + +#define SENSOR_TYPE_ACCELEROMETER 1 +#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 +#define SENSOR_TYPE_GYROSCOPE 4 +#define SENSOR_TYPE_LIGHT 5 +#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13 +#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 +#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD +#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/sunnypilot/system/sensord/sensors/i2c_sensor.cc b/sunnypilot/system/sensord/sensors/i2c_sensor.cc new file mode 100644 index 0000000000..e29f98c721 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/i2c_sensor.cc @@ -0,0 +1,50 @@ +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb) { + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); + return int16_t(combined) / (1 << 4); +} + +int16_t read_16_bit(uint8_t lsb, uint8_t msb) { + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); + return int16_t(combined); +} + +int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { + uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2); + return int32_t(combined) / (1 << 4); +} + +I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : + bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} + +I2CSensor::~I2CSensor() { + if (gpio_fd != -1) { + close(gpio_fd); + } +} + +int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { + return bus->read_register(get_device_address(), register_address, buffer, len); +} + +int I2CSensor::set_register(uint register_address, uint8_t data) { + return bus->set_register(get_device_address(), register_address, data); +} + +int I2CSensor::init_gpio() { + if (shared_gpio || gpio_nr == 0) { + return 0; + } + + gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); + if (gpio_fd < 0) { + return -1; + } + + return 0; +} + +bool I2CSensor::has_interrupt_enabled() { + return gpio_nr != 0; +} diff --git a/sunnypilot/system/sensord/sensors/i2c_sensor.h b/sunnypilot/system/sensord/sensors/i2c_sensor.h new file mode 100644 index 0000000000..6513db7934 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/i2c_sensor.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/i2c.h" +#include "common/gpio.h" + +#include "common/swaglog.h" +#include "sunnypilot/system/sensord/sensors/constants.h" +#include "sunnypilot/system/sensord/sensors/sensor.h" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb); +int16_t read_16_bit(uint8_t lsb, uint8_t msb); +int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0); + + +class I2CSensor : public Sensor { +private: + I2CBus *bus; + int gpio_nr; + bool shared_gpio; + virtual uint8_t get_device_address() = 0; + +public: + I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + ~I2CSensor(); + int read_register(uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint register_address, uint8_t data); + int init_gpio(); + bool has_interrupt_enabled(); + virtual int init() = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual int shutdown() = 0; + + int verify_chip_id(uint8_t address, const std::vector &expected_ids) { + uint8_t chip_id = 0; + int ret = read_register(address, &chip_id, 1); + if (ret < 0) { + LOGD("Reading chip ID failed: %d", ret); + return -1; + } + for (int i = 0; i < expected_ids.size(); ++i) { + if (chip_id == expected_ids[i]) return chip_id; + } + LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]); + return -1; + } +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc new file mode 100644 index 0000000000..2857106944 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc @@ -0,0 +1,250 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} + +void LSM6DS3_Accel::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); +} + +void LSM6DS3_Accel::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + float scaling = 0.061f; + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + scaling = 0.122f; + } + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_ACCEL_DRDY_XLDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling; + } + } + + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Accel::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz + + // prepare sensor for self-test + + // enable block data update and automatic increment + int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU); + if (ret < 0) { + return ret; + } + + if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { + ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ; + } + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(100); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) || + (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) { + return -1; + } + } + + return ret; +} + +int LSM6DS3_Accel::init() { + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } + + int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 accel negative self-test failed!"); + if (do_self_test) goto fail; + } + + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + // enable continuous update, and automatic increase + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC); + if (ret < 0) { + goto fail; + } + + // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); + if (ret < 0) { + goto fail; + } + + ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for accel on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_ACCEL_INT1_DRDY_XL; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + +fail: + return ret; +} + +int LSM6DS3_Accel::shutdown() { + int ret = 0; + + // disable data ready interrupt for accel on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); + ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 acceleration interrupt!"); + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 accelerometer!"); + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with gyro, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { + return false; + } + + uint8_t buffer[6]; + int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 9.81 * 2.0f / (1 << 15); + float x = read_16_bit(buffer[0], buffer[1]) * scale; + float y = read_16_bit(buffer[2], buffer[3]) * scale; + float z = read_16_bit(buffer[4], buffer[5]) * scale; + + auto event = msg.initEvent().initAccelerometer(); + event.setSource(source); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(ts); + + float xyz[] = {y, -x, z}; + auto svec = event.initAcceleration(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h new file mode 100644 index 0000000000..2ac61c5a0f --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_accel.h @@ -0,0 +1,49 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_ACCEL_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B +#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F +#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D +#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 +#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12 +#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18 +#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E +#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 + +// Constants +#define LSM6DS3_ACCEL_CHIP_ID 0x69 +#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A +#define LSM6DS3_ACCEL_FS_4G (0b10 << 2) +#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4) +#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 +#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 +#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_ACCEL_IF_INC 0b00000100 +#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100 +#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000 +#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01 +#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10 +#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f +#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f + +class LSM6DS3_Accel : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); +public: + LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc new file mode 100644 index 0000000000..e881f3942a --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc @@ -0,0 +1,233 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + +LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : + I2CSensor(bus, gpio_nr, shared_gpio) {} + +void LSM6DS3_Gyro::wait_for_data_ready() { + uint8_t drdy = 0; + uint8_t buffer[6]; + + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); +} + +void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) { + uint8_t drdy = 0; + uint8_t buffer[6]; + + for (int i = 0; i < 5; i++) { + do { + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); + drdy &= LSM6DS3_GYRO_DRDY_GDA; + } while (drdy == 0); + + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + for (int j = 0; j < 3; j++) { + out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f; + } + } + + // calculate the mg average values + for (int i = 0; i < 3; i++) { + out_buf[i] /= 5.0f; + } +} + +int LSM6DS3_Gyro::self_test(int test_type) { + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + float test_val[3] = {0}; + + // prepare sensor for self-test + + // full scale: 2000dps, ODR: 208Hz + int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(150); + wait_for_data_ready(); + read_and_avg_data(val_st_off); + + // enable Self Test positive (or negative) + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type); + if (ret < 0) { + return ret; + } + + // wait for stable output, and discard first values + util::sleep_for(50); + wait_for_data_ready(); + read_and_avg_data(val_st_on); + + // disable sensor + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0); + if (ret < 0) { + return ret; + } + + // disable self test + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0); + if (ret < 0) { + return ret; + } + + // calculate the mg values for self test + for (int i = 0; i < 3; i++) { + test_val[i] = fabs(val_st_on[i] - val_st_off[i]); + } + + // verify test result + for (int i = 0; i < 3; i++) { + if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) || + (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) { + return -1; + } + } + + return ret; +} + +int LSM6DS3_Gyro::init() { + uint8_t value = 0; + bool do_self_test = false; + + const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); + if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { + do_self_test = true; + } + + int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_GYRO_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + + ret = init_gpio(); + if (ret < 0) { + goto fail; + } + + ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 gyro positive self-test failed!"); + if (do_self_test) goto fail; + } + + ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST); + if (ret < 0) { + LOGE("LSM6DS3 gyro negative self-test failed!"); + if (do_self_test) goto fail; + } + + // TODO: set scale. Default is +- 250 deg/s + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); + if (ret < 0) { + goto fail; + } + + ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE); + if (ret < 0) { + goto fail; + } + + // enable data ready interrupt for gyro on INT1 + // (without resetting existing interrupts) + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value |= LSM6DS3_GYRO_INT1_DRDY_G; + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + +fail: + return ret; +} + +int LSM6DS3_Gyro::shutdown() { + int ret = 0; + + // disable data ready interrupt for gyro on INT1 + uint8_t value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); + ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); + if (ret < 0) { + LOGE("Could not disable lsm6ds3 gyroscope interrupt!"); + goto fail; + } + + // enable power-down mode + value = 0; + ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= 0x0F; + ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value); + if (ret < 0) { + LOGE("Could not power-down lsm6ds3 gyroscope!"); + goto fail; + } + +fail: + return ret; +} + +bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { + + // INT1 shared with accel, check STATUS_REG who triggered + uint8_t status_reg = 0; + read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); + if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) { + return false; + } + + uint8_t buffer[6]; + int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = 8.75 / 1000.0; + float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + auto event = msg.initEvent().initGyroscope(); + event.setSource(source); + event.setVersion(2); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(ts); + + float xyz[] = {y, -x, z}; + auto svec = event.initGyroUncalibrated(); + svec.setV(xyz); + svec.setStatus(true); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h new file mode 100644 index 0000000000..686a6a7169 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h @@ -0,0 +1,45 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_GYRO_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B +#define LSM6DS3_GYRO_I2C_REG_ID 0x0F +#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D +#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 +#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14 +#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E +#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 +#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2) +#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2) + +// Constants +#define LSM6DS3_GYRO_CHIP_ID 0x69 +#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A +#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2) +#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) +#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4) +#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 +#define LSM6DS3_GYRO_DRDY_GDA 0b10 +#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) +#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f +#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f + + +class LSM6DS3_Gyro : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + + // self test functions + int self_test(int test_type); + void wait_for_data_ready(); + void read_and_avg_data(float* val_st_off); +public: + LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc new file mode 100644 index 0000000000..390f192a2d --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc @@ -0,0 +1,37 @@ +#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h" + +#include + +#include "common/swaglog.h" +#include "common/timing.h" + +LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int LSM6DS3_Temp::init() { + int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID}); + if (ret == -1) return -1; + + if (ret == LSM6DS3TRC_TEMP_CHIP_ID) { + source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; + } + return 0; +} + +bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[2]; + int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; + float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; + + auto event = msg.initEvent().initTemperatureSensor(); + event.setSource(source); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); + + return true; +} diff --git a/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h new file mode 100644 index 0000000000..c314d456e8 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/lsm6ds3_temp.h @@ -0,0 +1,26 @@ +#pragma once + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define LSM6DS3_TEMP_I2C_ADDR 0x6A + +// Registers of the chip +#define LSM6DS3_TEMP_I2C_REG_ID 0x0F +#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20 + +// Constants +#define LSM6DS3_TEMP_CHIP_ID 0x69 +#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A + + +class LSM6DS3_Temp : public I2CSensor { + uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} + cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; + +public: + LSM6DS3_Temp(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown() { return 0; } +}; diff --git a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc new file mode 100644 index 0000000000..12d438ebd1 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc @@ -0,0 +1,108 @@ +#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h" + +#include +#include +#include + +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} + +int MMC5603NJ_Magn::init() { + int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID}); + if (ret == -1) return -1; + + // Set ODR to 0 + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); + if (ret < 0) { + goto fail; + } + + // Set BW to 0b01 for 1-150 Hz operation + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01); + if (ret < 0) { + goto fail; + } + +fail: + return ret; +} + +int MMC5603NJ_Magn::shutdown() { + int ret = 0; + + // disable auto reset of measurements + uint8_t value = 0; + ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1); + if (ret < 0) { + goto fail; + } + + value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); + ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value); + if (ret < 0) { + goto fail; + } + + // set ODR to 0 to leave continuous mode + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); + if (ret < 0) { + goto fail; + } + return ret; + +fail: + LOGE("Could not disable mmc5603nj auto set reset"); + return ret; +} + +void MMC5603NJ_Magn::start_measurement() { + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01); + util::sleep_for(5); +} + +std::vector MMC5603NJ_Magn::read_measurement() { + int len; + uint8_t buffer[9]; + len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + float scale = 1.0 / 16384.0; + float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0; + float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0; + float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0; + std::vector xyz = {x, y, z}; + return xyz; +} + +bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + // SET - RESET cycle + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector xyz = MMC5603NJ_Magn::read_measurement(); + + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector reset_xyz = MMC5603NJ_Magn::read_measurement(); + + auto event = msg.initEvent().initMagnetometer(); + event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); + event.setVersion(1); + event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); + event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); + event.setTimestamp(start_time); + + float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]}; + bool valid = true; + if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) { + valid = false; + } + auto svec = event.initMagneticUncalibrated(); + svec.setV(vals); + svec.setStatus(valid); + return true; +} diff --git a/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h new file mode 100644 index 0000000000..9311631c06 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/mmc5603nj_magn.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include "sunnypilot/system/sensord/sensors/i2c_sensor.h" + +// Address of the chip on the bus +#define MMC5603NJ_I2C_ADDR 0x30 + +// Registers of the chip +#define MMC5603NJ_I2C_REG_XOUT0 0x00 +#define MMC5603NJ_I2C_REG_ODR 0x1A +#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B +#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C +#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D +#define MMC5603NJ_I2C_REG_ID 0x39 + +// Constants +#define MMC5603NJ_CHIP_ID 0x10 +#define MMC5603NJ_CMM_FREQ_EN (1 << 7) +#define MMC5603NJ_AUTO_SR_EN (1 << 5) +#define MMC5603NJ_CMM_EN (1 << 4) +#define MMC5603NJ_EN_PRD_SET (1 << 3) +#define MMC5603NJ_SET (1 << 3) +#define MMC5603NJ_RESET (1 << 4) + +class MMC5603NJ_Magn : public I2CSensor { +private: + uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} + void start_measurement(); + std::vector read_measurement(); +public: + MMC5603NJ_Magn(I2CBus *bus); + int init(); + bool get_event(MessageBuilder &msg, uint64_t ts = 0); + int shutdown(); +}; diff --git a/sunnypilot/system/sensord/sensors/sensor.h b/sunnypilot/system/sensord/sensors/sensor.h new file mode 100644 index 0000000000..ccf998d161 --- /dev/null +++ b/sunnypilot/system/sensord/sensors/sensor.h @@ -0,0 +1,24 @@ +#pragma once + +#include "cereal/messaging/messaging.h" + +class Sensor { +public: + int gpio_fd = -1; + bool enabled = false; + uint64_t start_ts = 0; + uint64_t init_delay = 500e6; // default dealy 500ms + + virtual ~Sensor() {} + virtual int init() = 0; + virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; + virtual bool has_interrupt_enabled() = 0; + virtual int shutdown() = 0; + + virtual bool is_data_valid(uint64_t current_ts) { + if (start_ts == 0) { + start_ts = current_ts; + } + return (current_ts - start_ts) > init_delay; + } +}; diff --git a/sunnypilot/system/sensord/sensors_qcom2.cc b/sunnypilot/system/sensord/sensors_qcom2.cc new file mode 100644 index 0000000000..f4ecc7fc21 --- /dev/null +++ b/sunnypilot/system/sensord/sensors_qcom2.cc @@ -0,0 +1,179 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "cereal/services.h" +#include "cereal/messaging/messaging.h" +#include "common/i2c.h" +#include "common/ratekeeper.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "sunnypilot/system/sensord/sensors/bmx055_accel.h" +#include "sunnypilot/system/sensord/sensors/bmx055_gyro.h" +#include "sunnypilot/system/sensord/sensors/bmx055_magn.h" +#include "sunnypilot/system/sensord/sensors/bmx055_temp.h" +#include "sunnypilot/system/sensord/sensors/constants.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_accel.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_gyro.h" +#include "sunnypilot/system/sensord/sensors/lsm6ds3_temp.h" +#include "sunnypilot/system/sensord/sensors/mmc5603nj_magn.h" + +#define I2C_BUS_IMU 1 + +ExitHandler do_exit; + +void interrupt_loop(std::vector> sensors) { + PubMaster pm({"gyroscope", "accelerometer"}); + + int fd = -1; + for (auto &[sensor, msg_name] : sensors) { + if (sensor->has_interrupt_enabled()) { + fd = sensor->gpio_fd; + break; + } + } + + uint64_t offset = nanos_since_epoch() - nanos_since_boot(); + struct pollfd fd_list[1] = {0}; + fd_list[0].fd = fd; + fd_list[0].events = POLLIN | POLLPRI; + + while (!do_exit) { + int err = poll(fd_list, 1, 100); + if (err == -1) { + if (errno == EINTR) { + continue; + } + return; + } else if (err == 0) { + LOGE("poll timed out"); + continue; + } + + if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) { + LOGE("no poll events set"); + continue; + } + + // Read all events + struct gpioevent_data evdata[16]; + err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata))); + if (err < 0 || err % sizeof(*evdata) != 0) { + LOGE("error reading event data %d", err); + continue; + } + + uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot(); + uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset; + if (diff > 10*1e6) { // 10ms + LOGW("time jumped: %lu %lu", cur_offset, offset); + offset = cur_offset; + + // we don't have a valid timestamp since the + // time jumped, so throw out this measurement. + continue; + } + + int num_events = err / sizeof(*evdata); + uint64_t ts = evdata[num_events - 1].timestamp - cur_offset; + + for (auto &[sensor, msg_name] : sensors) { + if (!sensor->has_interrupt_enabled()) { + continue; + } + + MessageBuilder msg; + if (!sensor->get_event(msg, ts)) { + continue; + } + + if (!sensor->is_data_valid(ts)) { + continue; + } + + pm.send(msg_name.c_str(), msg); + } + } +} + +void polling_loop(Sensor *sensor, std::string msg_name) { + PubMaster pm({msg_name.c_str()}); + RateKeeper rk(msg_name, services.at(msg_name).frequency); + while (!do_exit) { + MessageBuilder msg; + if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) { + pm.send(msg_name.c_str(), msg); + } + rk.keepTime(); + } +} + +int sensor_loop(I2CBus *i2c_bus_imu) { + // Sensor init + std::vector> sensors_init = { + {new BMX055_Accel(i2c_bus_imu), "accelerometer2"}, + {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"}, + {new BMX055_Magn(i2c_bus_imu), "magnetometer"}, + {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"}, + + {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"}, + {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"}, + {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"}, + + {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"}, + }; + + // Initialize sensors + std::vector threads; + for (auto &[sensor, msg_name] : sensors_init) { + int err = sensor->init(); + if (err < 0) { + continue; + } + + if (!sensor->has_interrupt_enabled()) { + threads.emplace_back(polling_loop, sensor, msg_name); + } + } + + // increase interrupt quality by pinning interrupt and process to core 1 + setpriority(PRIO_PROCESS, 0, -18); + util::set_core_affinity({1}); + + // TODO: get the IRQ number from gpiochip + std::string irq_path = "/proc/irq/336/smp_affinity_list"; + if (!util::file_exists(irq_path)) { + irq_path = "/proc/irq/335/smp_affinity_list"; + } + std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str()); + + // thread for reading events via interrupts + threads.emplace_back(&interrupt_loop, std::ref(sensors_init)); + + // wait for all threads to finish + for (auto &t : threads) { + t.join(); + } + + for (auto &[sensor, msg_name] : sensors_init) { + sensor->shutdown(); + delete sensor; + } + return 0; +} + +int main(int argc, char *argv[]) { + try { + auto i2c_bus_imu = std::make_unique(I2C_BUS_IMU); + return sensor_loop(i2c_bus_imu.get()); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } +} diff --git a/sunnypilot/system/sensord/tests/test_sensord.py b/sunnypilot/system/sensord/tests/test_sensord.py new file mode 100644 index 0000000000..dc5f93d97b --- /dev/null +++ b/sunnypilot/system/sensord/tests/test_sensord.py @@ -0,0 +1,251 @@ +import os +import pytest +import time +import numpy as np +from collections import namedtuple, defaultdict + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.common.gpio import get_irqs_for_action +from openpilot.common.timeout import Timeout +from openpilot.system.hardware import HARDWARE +from openpilot.system.manager.process_config import managed_processes + +BMX = { + ('bmx055', 'acceleration'), + ('bmx055', 'gyroUncalibrated'), + ('bmx055', 'magneticUncalibrated'), + ('bmx055', 'temperature'), +} + +LSM = { + ('lsm6ds3', 'acceleration'), + ('lsm6ds3', 'gyroUncalibrated'), + ('lsm6ds3', 'temperature'), +} +LSM_C = {(x[0]+'trc', x[1]) for x in LSM} + +MMC = { + ('mmc5603nj', 'magneticUncalibrated'), +} + +SENSOR_CONFIGURATIONS: list[set] = [ + BMX | LSM, + MMC | LSM, + BMX | LSM_C, + MMC| LSM_C, +] +if HARDWARE.get_device_type() == "mici": + SENSOR_CONFIGURATIONS = [ + LSM, + LSM_C, + ] + +Sensor = log.SensorEventData.SensorSource +SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max']) +ALL_SENSORS = { + Sensor.lsm6ds3: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.lsm6ds3trc: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.bmx055: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("magneticUncalibrated", 0, 300), + SensorConfig("temperature", 0, 60), + }, + + Sensor.mmc5603nj: { + SensorConfig("magneticUncalibrated", 0, 300), + } +} + + +def get_irq_count(irq: int): + with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f: + per_cpu = map(int, f.read().split(",")) + return sum(per_cpu) + +def read_sensor_events(duration_sec): + sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2', + 'gyroscope2', 'temperatureSensor', 'temperatureSensor2'] + socks = {} + poller = messaging.Poller() + events = defaultdict(list) + for stype in sensor_types: + socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100) + + # wait for sensors to come up + with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"): + while len(poller.poll(250)) == 0: + pass + time.sleep(1) + for s in socks.values(): + messaging.drain_sock_raw(s) + + st = time.monotonic() + while time.monotonic() - st < duration_sec: + for s in socks: + events[s] += messaging.drain_sock(socks[s]) + time.sleep(0.1) + + assert sum(map(len, events.values())) != 0, "No sensor events collected!" + + return {k: v for k, v in events.items() if len(v) > 0} + +@pytest.mark.tici +class TestSensord: + @classmethod + def setup_class(cls): + # enable LSM self test + os.environ["LSM_SELF_TEST"] = "1" + + # read initial sensor values every test case can use + os.system("pkill -f \\\\./sensord") + try: + managed_processes["sensord"].start() + cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10")) + cls.events = read_sensor_events(cls.sample_secs) + + # determine sensord's irq + cls.sensord_irq = get_irqs_for_action("sensord")[0] + finally: + # teardown won't run if this doesn't succeed + managed_processes["sensord"].stop() + + @classmethod + def teardown_class(cls): + managed_processes["sensord"].stop() + + def teardown_method(self): + managed_processes["sensord"].stop() + + def test_sensors_present(self): + # verify correct sensors configuration + seen = set() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + seen.add((str(m.source), m.which())) + + assert seen in SENSOR_CONFIGURATIONS + + def test_lsm6ds3_timing(self, subtests): + # verify measurements are sampled and published at 104Hz + + sensor_t = { + 1: [], # accel + 5: [], # gyro + } + + for measurement in self.events['accelerometer']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for measurement in self.events['gyroscope']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for s, vals in sensor_t.items(): + with subtests.test(sensor=s): + assert len(vals) > 0 + tdiffs = np.diff(vals) / 1e6 # millis + + high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs)) + assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}" + + avg_diff = sum(tdiffs)/len(tdiffs) + avg_freq = 1. / (avg_diff * 1e-3) + assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz" + + stddev = np.std(tdiffs) + assert stddev < 2.0, f"Standard-dev to big {stddev}" + + def test_sensor_frequency(self, subtests): + for s, msgs in self.events.items(): + with subtests.test(sensor=s): + freq = len(msgs) / self.sample_secs + ef = SERVICE_LIST[s].frequency + assert ef*0.85 <= freq <= ef*1.15 + + def test_logmonottime_timestamp_diff(self): + # ensure diff between the message logMonotime and sample timestamp is small + + tdiffs = list() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + + # check if gyro and accel timestamps are before logMonoTime + if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature': + err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}" + assert m.timestamp < measurement.logMonoTime, err_msg + + # negative values might occur, as non interrupt packages created + # before the sensor is read + tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6) + + # some sensors have a read procedure that will introduce an expected diff on the order of 20ms + high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs)) + assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}" + + avg_diff = round(sum(tdiffs)/len(tdiffs), 4) + assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms" + + def test_sensor_values(self): + sensor_values = dict() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + key = (m.source.raw, m.which()) + values = getattr(m, m.which()) + + if hasattr(values, 'v'): + values = values.v + values = np.atleast_1d(values) + + if key in sensor_values: + sensor_values[key].append(values) + else: + sensor_values[key] = [values] + + # Sanity check sensor values + for sensor, stype in sensor_values: + for s in ALL_SENSORS[sensor]: + if s.type != stype: + continue + + key = (sensor, s.type) + mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1)) + err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}" + assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg + + def test_sensor_verify_no_interrupts_after_stop(self): + managed_processes["sensord"].start() + time.sleep(3) + + # read /proc/interrupts to verify interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + + error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}" + assert state_one != state_two, error_msg + + managed_processes["sensord"].stop() + time.sleep(1) + + # read /proc/interrupts to verify no more interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + assert state_one == state_two, "Interrupts received after sensord stop!" diff --git a/sunnypilot/system/sensord/tests/ttff_test.py b/sunnypilot/system/sensord/tests/ttff_test.py new file mode 100755 index 0000000000..a9cc16d707 --- /dev/null +++ b/sunnypilot/system/sensord/tests/ttff_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import time +import atexit + +from cereal import messaging +from openpilot.system.manager.process_config import managed_processes + +TIMEOUT = 10*60 + +def kill(): + for proc in ['ubloxd', 'pigeond']: + managed_processes[proc].stop(retry=True, block=True) + +if __name__ == "__main__": + # start ubloxd + managed_processes['ubloxd'].start() + atexit.register(kill) + + sm = messaging.SubMaster(['ubloxGnss']) + + times = [] + for i in range(20): + # start pigeond + st = time.monotonic() + managed_processes['pigeond'].start() + + # wait for a >4 satellite fix + while True: + sm.update(0) + msg = sm['ubloxGnss'] + if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]: + report = msg.measurementReport + if report.numMeas > 4: + times.append(time.monotonic() - st) + print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m") + break + + if time.monotonic() - st > TIMEOUT: + raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m") + + time.sleep(0.1) + + # stop pigeond + managed_processes['pigeond'].stop(retry=True, block=True) + time.sleep(20) + + print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m") diff --git a/system/camerad/SConscript b/system/camerad/SConscript index fe5cf87b78..734f748a2a 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -4,7 +4,7 @@ libs = [common, 'OpenCL', messaging, visionipc, gpucommon] if arch != "Darwin": camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc', - 'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) + 'cameras/cdm.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/hw.h b/system/camerad/cameras/hw.h index d299627ce9..f20a1b3ade 100644 --- a/system/camerad/cameras/hw.h +++ b/system/camerad/cameras/hw.h @@ -13,7 +13,7 @@ typedef enum { ISP_BPS_PROCESSED, // fully processed image through the BPS } SpectraOutputType; -// For the comma 3/3X three camera platform +// For the comma 3X three camera platform struct CameraConfig { int camera_num; diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 47ae9061f4..caf7871573 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -1004,8 +1004,7 @@ bool SpectraCamera::openSensor() { }; // Figure out which sensor we have - if (!init_sensor_lambda(new AR0231) && - !init_sensor_lambda(new OX03C10) && + if (!init_sensor_lambda(new OX03C10) && !init_sensor_lambda(new OS04C10)) { LOGE("** sensor %d FAILED bringup, disabling", cc.camera_num); enabled = false; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc deleted file mode 100644 index e4ae29f079..0000000000 --- a/system/camerad/sensors/ar0231.cc +++ /dev/null @@ -1,136 +0,0 @@ -#include -#include - -#include "system/camerad/sensors/sensor.h" - -namespace { - -const size_t AR0231_REGISTERS_HEIGHT = 2; -// TODO: this extra height is universal and doesn't apply per camera -const size_t AR0231_STATS_HEIGHT = 2 + 8; - -const float sensor_analog_gains_AR0231[] = { - 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3 - 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7 - 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11 - 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass - -} // namespace - -AR0231::AR0231() { - image_sensor = cereal::FrameData::ImageSensor::AR0231; - bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR; - pixel_size_mm = 0.003; - data_word = true; - frame_width = 1928; - frame_height = 1208; - frame_stride = (frame_width * 12 / 8) + 4; - extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT; - - registers_offset = 0; - frame_offset = AR0231_REGISTERS_HEIGHT; - stats_offset = AR0231_REGISTERS_HEIGHT + frame_height; - - start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231)); - init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231)); - probe_reg_addr = 0x3000; - probe_expected_data = 0x354; - bits_per_pixel = 12; - mipi_format = CAM_FORMAT_MIPI_RAW_12; - frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead - mclk_frequency = 19200000; //Hz - - readout_time_ns = 22850000; - - dc_gain_factor = 2.5; - dc_gain_min_weight = 0; - dc_gain_max_weight = 1; - dc_gain_on_grey = 0.2; - dc_gain_off_grey = 0.3; - exposure_time_min = 2; // with HDR, fastest ss - exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms - analog_gain_min_idx = 0x1; // 0.25x - analog_gain_rec_idx = 0x6; // 0.8x - analog_gain_max_idx = 0xD; // 4.0x - analog_gain_cost_delta = 0; - analog_gain_cost_low = 0.1; - analog_gain_cost_high = 5.0; - for (int i = 0; i <= analog_gain_max_idx; i++) { - sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; - } - min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; - target_grey_factor = 1.0; - - black_level = 168; - color_correct_matrix = { - 0x000000af, 0x00000ff9, 0x00000fd8, - 0x00000fbc, 0x000000bb, 0x00000009, - 0x00000fb6, 0x00000fe0, 0x000000ea, - }; - for (int i = 0; i < 65; i++) { - float fx = i / 64.0; - const float gamma_k = 0.75; - const float gamma_b = 0.125; - const float mp = 0.01; // ideally midpoint should be adaptive - const float rk = 9 - 100*mp; - // poly approximation for s curve - fx = (fx > mp) ? - ((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) : - ((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b); - gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5)); - } - prepare_gamma_lut(); - linearization_lut = { - 0x02000000, 0x02000000, 0x02000000, 0x02000000, - 0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff, - 0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff, - 0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff, - 0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff, - 0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff, - 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, - 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, - 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, - }; - linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff}; - vignetting_lut = { - 0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2, - 0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7, - 0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d, - 0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed, - 0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1, - 0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17, - 0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93, - 0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e, - 0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6, - 0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee, - 0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8, - 0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676, - 0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22, - }; -} - -std::vector AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { - uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; - return { - {0x3366, analog_gain_reg}, - {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, - {0x3012, (uint16_t)exposure_time}, - }; -} - -int AR0231::getSlaveAddress(int port) const { - assert(port >= 0 && port <= 2); - return (int[]){0x20, 0x30, 0x20}[port]; -} - -float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { - // Cost of ev diff - float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; - // Cost of absolute gain - float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; - // Cost of changing gain - score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; - return score; -} diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h deleted file mode 100644 index e0872a673a..0000000000 --- a/system/camerad/sensors/ar0231_registers.h +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; -const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; - -const struct i2c_random_wr_payload init_array_ar0231[] = { - {0x301A, 0x0018}, // RESET_REGISTER - - // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config - - // CLOCK Settings - // input clock is 19.2 / 2 * 0x37 = 528 MHz - // pixclk is 528 / 6 = 88 MHz - // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms - // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms - {0x302A, 0x0006}, // VT_PIX_CLK_DIV - {0x302C, 0x0001}, // VT_SYS_CLK_DIV - {0x302E, 0x0002}, // PRE_PLL_CLK_DIV - {0x3030, 0x0037}, // PLL_MULTIPLIER - {0x3036, 0x000C}, // OP_PIX_CLK_DIV - {0x3038, 0x0001}, // OP_SYS_CLK_DIV - - // FORMAT - {0x3040, 0xC000}, // READ_MODE - {0x3004, 0x0000}, // X_ADDR_START_ - {0x3008, 0x0787}, // X_ADDR_END_ - {0x3002, 0x0000}, // Y_ADDR_START_ - {0x3006, 0x04B7}, // Y_ADDR_END_ - {0x3032, 0x0000}, // SCALING_MODE - {0x30A2, 0x0001}, // X_ODD_INC_ - {0x30A6, 0x0001}, // Y_ODD_INC_ - {0x3402, 0x0788}, // X_OUTPUT_CONTROL - {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL - {0x3064, 0x1982}, // SMIA_TEST - {0x30BA, 0x11F2}, // DIGITAL_CTRL - - // Enable external trigger and disable GPIO outputs - {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE - {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE - {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2 - - // Readout timing - {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR) - {0x300A, 0x0855}, // FRAME_LENGTH_LINES - {0x3042, 0x0000}, // EXTRA_DELAY - - // Readout Settings - {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI - {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 - {0x3342, 0x1212}, // MIPI_F1_PDT_EDT - {0x3346, 0x1212}, // MIPI_F2_PDT_EDT - {0x334A, 0x1212}, // MIPI_F3_PDT_EDT - {0x334E, 0x1212}, // MIPI_F4_PDT_EDT - {0x3344, 0x0011}, // MIPI_F1_VDT_VC - {0x3348, 0x0111}, // MIPI_F2_VDT_VC - {0x334C, 0x0211}, // MIPI_F3_VDT_VC - {0x3350, 0x0311}, // MIPI_F4_VDT_VC - {0x31B0, 0x0053}, // FRAME_PREAMBLE - {0x31B2, 0x003B}, // LINE_PREAMBLE - {0x301A, 0x001C}, // RESET_REGISTER - - // Noise Corrections - {0x3092, 0x0C24}, // ROW_NOISE_CONTROL - {0x337A, 0x0C80}, // DBLC_SCALE0 - {0x3370, 0x03B1}, // DBLC - {0x3044, 0x0400}, // DARK_CONTROL - - // Enable temperature sensor - {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG - {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG - - // Enable dead pixel correction using - // the 1D line correction scheme - {0x31E0, 0x0003}, - - // HDR Settings - {0x3082, 0x0004}, // OPERATION_MODE_CTRL - {0x3238, 0x0444}, // EXPOSURE_RATIO - - {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN - {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN - {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN - {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN - - // TODO: do these have to be lower than LINE_LENGTH_PCK? - {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ - {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 - - {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? - {0x33DA, 0x0000}, // COMPANDING - {0x318E, 0x0200}, // PRE_HDR_GAIN_EN - - // DLO Settings - {0x3100, 0x4000}, // DLO_CONTROL0 - {0x3280, 0x0CCC}, // T1 G1 - {0x3282, 0x0CCC}, // T1 R - {0x3284, 0x0CCC}, // T1 B - {0x3286, 0x0CCC}, // T1 G2 - {0x3288, 0x0FA0}, // T2 G1 - {0x328A, 0x0FA0}, // T2 R - {0x328C, 0x0FA0}, // T2 B - {0x328E, 0x0FA0}, // T2 G2 - - // Initial Gains - {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ - {0x3366, 0xFF77}, // ANALOG_GAIN (1x) - - {0x3060, 0x3333}, // ANALOG_COLOR_GAIN - - {0x3362, 0x0000}, // DC GAIN - - {0x305A, 0x00F8}, // red gain - {0x3058, 0x0122}, // blue gain - {0x3056, 0x009A}, // g1 gain - {0x305C, 0x009A}, // g2 gain - - {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ - - // Initial Integration Time - {0x3012, 0x0005}, -}; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index d4be3cf036..96aa8b604f 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -10,7 +10,6 @@ #include "media/cam_sensor.h" #include "cereal/gen/cpp/log.capnp.h" -#include "system/camerad/sensors/ar0231_registers.h" #include "system/camerad/sensors/ox03c10_registers.h" #include "system/camerad/sensors/os04c10_registers.h" @@ -88,17 +87,6 @@ public: }; }; -class AR0231 : public SensorInfo { -public: - AR0231(); - std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; - float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; - int getSlaveAddress(int port) const override; - -private: - mutable std::map> ar0231_register_lut; -}; - class OX03C10 : public SensorInfo { public: OX03C10(); diff --git a/system/hardware/base.py b/system/hardware/base.py index b457ea4e17..ce97bf294d 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -65,6 +65,10 @@ class ThermalConfig: return ret class LPABase(ABC): + @abstractmethod + def bootstrap(self) -> None: + pass + @abstractmethod def list_profiles(self) -> list[Profile]: pass @@ -89,6 +93,9 @@ class LPABase(ABC): def switch_profile(self, iccid: str) -> None: pass + def is_comma_profile(self, iccid: str) -> bool: + return any(iccid.startswith(prefix) for prefix in ('8985235',)) + class HardwareBase(ABC): @staticmethod def get_cmdline() -> dict[str, str]: diff --git a/system/hardware/esim.py b/system/hardware/esim.py index 58ead6593f..909ad41e03 100755 --- a/system/hardware/esim.py +++ b/system/hardware/esim.py @@ -3,10 +3,32 @@ import argparse import time from openpilot.system.hardware import HARDWARE +from openpilot.system.hardware.base import LPABase + + +def bootstrap(lpa: LPABase) -> None: + print('┌──────────────────────────────────────────────────────────────────────────────┐') + print('│ WARNING, PLEASE READ BEFORE PROCEEDING │') + print('│ │') + print('│ this is an irreversible operation that will remove the comma-provisioned │') + print('│ profile. │') + print('│ │') + print('│ after this operation, you must purchase a new eSIM from comma in order to │') + print('│ use the comma prime subscription again. │') + print('└──────────────────────────────────────────────────────────────────────────────┘') + print() + for severity in ('sure', '100% sure'): + print(f'are you {severity} you want to proceed? (y/N) ', end='') + confirm = input() + if confirm != 'y': + print('aborting') + exit(0) + lpa.bootstrap() if __name__ == '__main__': parser = argparse.ArgumentParser(prog='esim.py', description='manage eSIM profiles on your comma device', epilog='comma.ai') + parser.add_argument('--bootstrap', action='store_true', help='bootstrap the eUICC (required before downloading profiles)') parser.add_argument('--backend', choices=['qmi', 'at'], default='qmi', help='use the specified backend, defaults to qmi') parser.add_argument('--switch', metavar='iccid', help='switch to profile') parser.add_argument('--delete', metavar='iccid', help='delete profile (warning: this cannot be undone)') @@ -16,7 +38,10 @@ if __name__ == '__main__': mutated = False lpa = HARDWARE.get_sim_lpa() - if args.switch: + if args.bootstrap: + bootstrap(lpa) + mutated = True + elif args.switch: lpa.switch_profile(args.switch) mutated = True elif args.delete: diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index df8ae23ab5..0144a9df03 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -6,7 +6,6 @@ import struct import threading import time from collections import OrderedDict, namedtuple -from pathlib import Path import psutil @@ -342,12 +341,6 @@ def hardware_thread(end_event, hw_queue) -> None: show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"] set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text) - # TODO: this should move to TICI.initialize_hardware, but we currently can't import params there - if TICI and HARDWARE.get_device_type() == "tici": - if not os.path.isfile("/persist/comma/living-in-the-moment"): - if not Path("/data/media").is_mount(): - set_offroad_alert_if_changed("Offroad_StorageMissing", True) - # Handle offroad/onroad transition should_start = all(onroad_conditions.values()) if started_ts is None: diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 941a4956bf..d93963cf2c 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -23,14 +23,14 @@ }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz", - "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", - "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", + "url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz", + "hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", + "hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", "size": 274432, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6" + "ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee" }, { "name": "aop", @@ -56,29 +56,29 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz", - "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4", - "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4", - "size": 18515968, + "url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz", + "hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb", + "hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb", + "size": 17442816, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0" + "ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz", - "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126", - "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087", - "size": 5368709120, + "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz", + "hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9", + "hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc", + "size": 4718592000, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7", + "ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727", "alt": { - "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087", - "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img", - "size": 5368709120 + "hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc", + "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img", + "size": 4718592000 } } ] \ No newline at end of file diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json index 5891e2748a..ebffc01dfd 100644 --- a/system/hardware/tici/all-partitions.json +++ b/system/hardware/tici/all-partitions.json @@ -152,14 +152,14 @@ }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz", - "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", - "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", + "url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz", + "hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", + "hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee", "size": 274432, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6" + "ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee" }, { "name": "aop", @@ -339,62 +339,62 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz", - "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4", - "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4", - "size": 18515968, + "url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz", + "hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb", + "hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb", + "size": 17442816, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0" + "ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz", - "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126", - "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087", - "size": 5368709120, + "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz", + "hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9", + "hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc", + "size": 4718592000, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7", + "ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727", "alt": { - "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087", - "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img", - "size": 5368709120 + "hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc", + "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img", + "size": 4718592000 } }, { "name": "userdata_90", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a.img.xz", - "hash": "6a11d448bac50467791809339051eed2894aae971c37bf6284b3b972a99ba3ac", - "hash_raw": "602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9.img.xz", + "hash": "bea163e6fb6ac6224c7f32619affb5afb834cd859971b0cab6d8297dd0098f0a", + "hash_raw": "b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9", "size": 96636764160, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "e014d92940a696bf8582807259820ab73948b950656ed83a45da738f26083705" + "ondevice_hash": "f4841c6ae3207197886e5efbd50f44cc24822680d7b785fa2d2743c657f23287" }, { "name": "userdata_89", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48.img.xz", - "hash": "748e31a5fc01fc256c012e359c3382d1f98cce98feafe8ecc0fca3e47caef116", - "hash_raw": "4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167.img.xz", + "hash": "b5458a29dd7d4a4c9b7ad77b8baa5f804142ac78d97c6668839bf2a650e32518", + "hash_raw": "3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167", "size": 95563022336, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "c181b93050787adcfef730c086bcb780f28508d84e6376d9b80d37e5dc02b55e" + "ondevice_hash": "1dc10c542d3b019258fc08dc7dfdb49d9abad065e46d030b89bc1a2e0197f526" }, { "name": "userdata_30", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668.img.xz", - "hash": "09ff390e639e4373d772e1688d05a5ac77a573463ed1deeff86390686fa686f9", - "hash_raw": "80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c.img.xz", + "hash": "687d178cfc91be5d7e8aa1333405b610fdce01775b8333bd0985b81642b94eea", + "hash_raw": "1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c", "size": 32212254720, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "2c01ab470c02121c721ff6afc25582437e821686207f3afef659387afb69c507" + "ondevice_hash": "9ddbd1dae6ee7dc919f018364cf2f29dad138c9203c5a49aea0cbb9bf2e137e5" } ] \ No newline at end of file diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py index f6b29ec0ce..bfdcc6ddaf 100755 --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -61,18 +61,6 @@ BASE_CONFIG = [ ] CONFIGS = { - "tici": [ - AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), - AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), - AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), - AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), - - *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)), - *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)), - *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)), - *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)), - *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)), - ], "tizi": [ AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), diff --git a/system/hardware/tici/esim.py b/system/hardware/tici/esim.py index b489286f50..391ba45531 100644 --- a/system/hardware/tici/esim.py +++ b/system/hardware/tici/esim.py @@ -40,6 +40,7 @@ class TiciLPA(LPABase): self._process_notifications() def download_profile(self, qr: str, nickname: str | None = None) -> None: + self._check_bootstrapped() msgs = self._invoke('profile', 'download', '-a', qr) self._validate_successful(msgs) new_profile = next((m for m in msgs if m['payload']['message'] == 'es8p_meatadata_parse'), None) @@ -54,6 +55,7 @@ class TiciLPA(LPABase): self._validate_successful(self._invoke('profile', 'nickname', iccid, nickname)) def switch_profile(self, iccid: str) -> None: + self._check_bootstrapped() self._validate_profile_exists(iccid) latest = self.get_active_profile() if latest and latest.iccid == iccid: @@ -61,6 +63,33 @@ class TiciLPA(LPABase): self._validate_successful(self._invoke('profile', 'enable', iccid)) self._process_notifications() + def bootstrap(self) -> None: + """ + find all comma-provisioned profiles and delete them. they conflict with user-provisioned profiles + and must be deleted. + + **note**: this is a **very** destructive operation. you **must** purchase a new comma SIM in order + to use comma prime again. + """ + if self._is_bootstrapped(): + return + + for p in self.list_profiles(): + if self.is_comma_profile(p.iccid): + self._disable_profile(p.iccid) + self.delete_profile(p.iccid) + + def _disable_profile(self, iccid: str) -> None: + self._validate_successful(self._invoke('profile', 'disable', iccid)) + self._process_notifications() + + def _check_bootstrapped(self) -> None: + assert self._is_bootstrapped(), 'eUICC is not bootstrapped, please bootstrap before performing this operation' + + def _is_bootstrapped(self) -> bool: + """ check if any comma provisioned profiles are on the eUICC """ + return not any(self.is_comma_profile(iccid) for iccid in (p.iccid for p in self.list_profiles())) + def _invoke(self, *cmd: str): proc = subprocess.Popen(['sudo', '-E', 'lpac'] + list(cmd), stdout=subprocess.PIPE, stderr=subprocess.PIPE, env=self.env) try: diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 35f9916c31..0f50acdc38 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -425,9 +425,6 @@ class Tici(HardwareBase): # pandad core affine_irq(3, "spi_geni") # SPI - if "tici" in self.get_device_type(): - affine_irq(3, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB) - affine_irq(3, "xhci-hcd:usb1") # internal panda USB (also modem) try: pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip() subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid]) @@ -446,22 +443,20 @@ class Tici(HardwareBase): cmds = [] - if self.get_device_type() in ("tici", "tizi"): + if self.get_device_type() in ("tizi", ): # clear out old blue prime initial APN os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn="') cmds += [ + # SIM hot swap + 'AT+QSIMDET=1,0', + 'AT+QSIMSTAT=1', + # configure modem as data-centric 'AT+QNVW=5280,0,"0102000000000000"', 'AT+QNVFW="/nv/item_files/ims/IMS_enable",00', 'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01', ] - if self.get_device_type() == "tizi": - # SIM hot swap, not routed on tici - cmds += [ - 'AT+QSIMDET=1,0', - 'AT+QSIMSTAT=1', - ] elif manufacturer == 'Cavli Inc.': cmds += [ 'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM @@ -492,7 +487,7 @@ class Tici(HardwareBase): # eSIM prime dest = "/etc/NetworkManager/system-connections/esim.nmconnection" - if sim_id.startswith('8985235') and not os.path.exists(dest): + if self.get_sim_lpa().is_comma_profile(sim_id) and not os.path.exists(dest): with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf: dat = f.read() dat = dat.replace("sim-id=", f"sim-id={sim_id}") @@ -503,6 +498,14 @@ class Tici(HardwareBase): os.system(f"sudo cp {tf.name} {dest}") os.system(f"sudo nmcli con load {dest}") + def reboot_modem(self): + modem = self.get_modem() + for state in (0, 1): + try: + modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + except Exception: + pass + def get_networks(self): r = {} diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py index 747278d1ec..bdbea591fb 100644 --- a/system/hardware/tici/pins.py +++ b/system/hardware/tici/pins.py @@ -1,5 +1,3 @@ -# TODO: these are also defined in a header - # GPIO pin definitions class GPIO: # both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset @@ -26,7 +24,4 @@ class GPIO: CAM2_RSTN = 12 # Sensor interrupts - BMX055_ACCEL_INT = 21 - BMX055_GYRO_INT = 23 - BMX055_MAGN_INT = 87 LSM_INT = 84 diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index db0fab884c..4fbde81673 100644 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -31,9 +31,9 @@ class Proc: PROCS = [ - Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), - Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']), - Proc(['dmonitoringmodeld'], 0.6, msgs=['driverStateV2']), + Proc(['camerad'], 1.65, atol=0.4, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), + Proc(['modeld'], 1.24, atol=0.2, msgs=['modelV2']), + Proc(['dmonitoringmodeld'], 0.65, atol=0.35, msgs=['driverStateV2']), Proc(['encoderd'], 0.23, msgs=[]), ] diff --git a/system/journald.py b/system/journald.py new file mode 100755 index 0000000000..37158b9251 --- /dev/null +++ b/system/journald.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import json +import subprocess + +import cereal.messaging as messaging +from openpilot.common.swaglog import cloudlog + + +def main(): + pm = messaging.PubMaster(['androidLog']) + cmd = ['journalctl', '-f', '-o', 'json'] + proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, text=True) + assert proc.stdout is not None + try: + for line in proc.stdout: + line = line.strip() + if not line: + continue + try: + kv = json.loads(line) + except json.JSONDecodeError: + cloudlog.exception("failed to parse journalctl output") + continue + + msg = messaging.new_message('androidLog') + entry = msg.androidLog + entry.ts = int(kv.get('__REALTIME_TIMESTAMP', 0)) + entry.message = json.dumps(kv) + if '_PID' in kv: + entry.pid = int(kv['_PID']) + if 'PRIORITY' in kv: + entry.priority = int(kv['PRIORITY']) + if 'SYSLOG_IDENTIFIER' in kv: + entry.tag = kv['SYSLOG_IDENTIFIER'] + + pm.send('androidLog', msg) + finally: + proc.terminate() + proc.wait() + + +if __name__ == '__main__': + main() diff --git a/system/logcatd/.gitignore b/system/logcatd/.gitignore deleted file mode 100644 index c66f7622d9..0000000000 --- a/system/logcatd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -logcatd diff --git a/system/logcatd/SConscript b/system/logcatd/SConscript deleted file mode 100644 index 39c45d1093..0000000000 --- a/system/logcatd/SConscript +++ /dev/null @@ -1,3 +0,0 @@ -Import('env', 'messaging', 'common') - -env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[messaging, common, 'systemd']) diff --git a/system/logcatd/logcatd_systemd.cc b/system/logcatd/logcatd_systemd.cc deleted file mode 100644 index 54b3782132..0000000000 --- a/system/logcatd/logcatd_systemd.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include - -#include -#include -#include -#include - -#include "third_party/json11/json11.hpp" - -#include "cereal/messaging/messaging.h" -#include "common/timing.h" -#include "common/util.h" - -ExitHandler do_exit; -int main(int argc, char *argv[]) { - - PubMaster pm({"androidLog"}); - - sd_journal *journal; - int err = sd_journal_open(&journal, 0); - assert(err >= 0); - err = sd_journal_get_fd(journal); // needed so sd_journal_wait() works properly if files rotate - assert(err >= 0); - err = sd_journal_seek_tail(journal); - assert(err >= 0); - - // workaround for bug https://github.com/systemd/systemd/issues/9934 - // call sd_journal_previous_skip after sd_journal_seek_tail (like journalctl -f does) to makes things work. - sd_journal_previous_skip(journal, 1); - - while (!do_exit) { - err = sd_journal_next(journal); - assert(err >= 0); - - // Wait for new message if we didn't receive anything - if (err == 0) { - err = sd_journal_wait(journal, 1000 * 1000); - assert(err >= 0); - continue; // Try again - } - - uint64_t timestamp = 0; - err = sd_journal_get_realtime_usec(journal, ×tamp); - assert(err >= 0); - - const void *data; - size_t length; - std::map kv; - - SD_JOURNAL_FOREACH_DATA(journal, data, length) { - std::string str((char*)data, length); - - // Split "KEY=VALUE"" on "=" and put in map - std::size_t found = str.find("="); - if (found != std::string::npos) { - kv[str.substr(0, found)] = str.substr(found + 1, std::string::npos); - } - } - - MessageBuilder msg; - - // Build message - auto androidEntry = msg.initEvent().initAndroidLog(); - androidEntry.setTs(timestamp); - androidEntry.setMessage(json11::Json(kv).dump()); - if (kv.count("_PID")) androidEntry.setPid(std::atoi(kv["_PID"].c_str())); - if (kv.count("PRIORITY")) androidEntry.setPriority(std::atoi(kv["PRIORITY"].c_str())); - if (kv.count("SYSLOG_IDENTIFIER")) androidEntry.setTag(kv["SYSLOG_IDENTIFIER"]); - - pm.send("androidLog", msg); - } - - sd_journal_close(journal); - return 0; -} diff --git a/system/loggerd/tests/vidc_debug.sh b/system/loggerd/tests/vidc_debug.sh new file mode 100755 index 0000000000..7471f2ab08 --- /dev/null +++ b/system/loggerd/tests/vidc_debug.sh @@ -0,0 +1,13 @@ +#!/usr/bin/env bash +set -e + +cd /sys/kernel/debug/tracing +echo "" > trace +echo 1 > tracing_on +echo 1 > /sys/kernel/debug/tracing/events/msm_vidc/enable + +echo 0xff > /sys/module/videobuf2_core/parameters/debug +echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level +echo 0xff > /sys/devices/platform/soc/aa00000.qcom,vidc/video4linux/video33/dev_debug + +cat /sys/kernel/debug/tracing/trace_pipe diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 6130916887..c5f20e511b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -95,6 +95,12 @@ def is_stock_model(started, params, CP: car.CarParams) -> bool: def mapd_ready(started: bool, params: Params, CP: car.CarParams) -> bool: return bool(os.path.exists(Paths.mapd_root())) +def uploader_ready(started: bool, params: Params, CP: car.CarParams) -> bool: + if not params.get_bool("OnroadUploads"): + return only_offroad(started, params, CP) + + return always_run(started, params, CP) + def or_(*fns): return lambda *args: operator.or_(*(fn(*args) for fn in fns)) @@ -111,8 +117,8 @@ procs = [ NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM), PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM), - NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad, platform.system() != "Darwin"), - NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad, platform.system() != "Darwin"), + PythonProcess("proclogd", "system.proclogd", only_onroad, enabled=platform.system() != "Darwin"), + PythonProcess("journald", "system.journald", only_onroad, platform.system() != "Darwin"), PythonProcess("micd", "system.micd", iscar), PythonProcess("timed", "system.timed", always_run, enabled=not PC), @@ -137,7 +143,7 @@ procs = [ PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad), - NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), + PythonProcess("ubloxd", "system.ubloxd.ubloxd", ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver), PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver), @@ -145,7 +151,7 @@ procs = [ PythonProcess("hardwared", "system.hardware.hardwared", always_run), PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC), PythonProcess("updated", "system.updated.updated", only_offroad, enabled=not PC), - PythonProcess("uploader", "system.loggerd.uploader", always_run), + PythonProcess("uploader", "system.loggerd.uploader", uploader_ready), PythonProcess("statsd", "system.statsd", always_run), PythonProcess("feedbackd", "selfdrive.ui.feedback.feedbackd", only_onroad), @@ -173,6 +179,9 @@ procs += [ # mapd NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), + + # locationd + NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad), ] if os.path.exists("./github_runner.sh"): diff --git a/system/proclogd.py b/system/proclogd.py new file mode 100755 index 0000000000..3279425b7b --- /dev/null +++ b/system/proclogd.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +import os +from typing import NoReturn, TypedDict + +from cereal import messaging +from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog + +JIFFY = os.sysconf(os.sysconf_names['SC_CLK_TCK']) +PAGE_SIZE = os.sysconf(os.sysconf_names['SC_PAGE_SIZE']) + + +def _cpu_times() -> list[dict[str, float]]: + cpu_times: list[dict[str, float]] = [] + try: + with open('/proc/stat') as f: + lines = f.readlines()[1:] + for line in lines: + if not line.startswith('cpu') or len(line) < 4 or not line[3].isdigit(): + break + parts = line.split() + cpu_times.append({ + 'cpuNum': int(parts[0][3:]), + 'user': float(parts[1]) / JIFFY, + 'nice': float(parts[2]) / JIFFY, + 'system': float(parts[3]) / JIFFY, + 'idle': float(parts[4]) / JIFFY, + 'iowait': float(parts[5]) / JIFFY, + 'irq': float(parts[6]) / JIFFY, + 'softirq': float(parts[7]) / JIFFY, + }) + except Exception: + cloudlog.exception("failed to read /proc/stat") + return cpu_times + + +def _mem_info() -> dict[str, int]: + keys = ["MemTotal:", "MemFree:", "MemAvailable:", "Buffers:", "Cached:", "Active:", "Inactive:", "Shmem:"] + info: dict[str, int] = dict.fromkeys(keys, 0) + try: + with open('/proc/meminfo') as f: + for line in f: + parts = line.split() + if parts and parts[0] in info: + info[parts[0]] = int(parts[1]) * 1024 + except Exception: + cloudlog.exception("failed to read /proc/meminfo") + return info + + +_STAT_POS = { + 'pid': 1, + 'state': 3, + 'ppid': 4, + 'utime': 14, + 'stime': 15, + 'cutime': 16, + 'cstime': 17, + 'priority': 18, + 'nice': 19, + 'num_threads': 20, + 'starttime': 22, + 'vsize': 23, + 'rss': 24, + 'processor': 39, +} + +class ProcStat(TypedDict): + name: str + pid: int + state: str + ppid: int + utime: int + stime: int + cutime: int + cstime: int + priority: int + nice: int + num_threads: int + starttime: int + vms: int + rss: int + processor: int + + +def _parse_proc_stat(stat: str) -> ProcStat | None: + open_paren = stat.find('(') + close_paren = stat.rfind(')') + if open_paren == -1 or close_paren == -1 or open_paren > close_paren: + return None + name = stat[open_paren + 1:close_paren] + stat = stat[:open_paren] + stat[open_paren:close_paren].replace(' ', '_') + stat[close_paren:] + parts = stat.split() + if len(parts) < 52: + return None + try: + return { + 'name': name, + 'pid': int(parts[_STAT_POS['pid'] - 1]), + 'state': parts[_STAT_POS['state'] - 1][0], + 'ppid': int(parts[_STAT_POS['ppid'] - 1]), + 'utime': int(parts[_STAT_POS['utime'] - 1]), + 'stime': int(parts[_STAT_POS['stime'] - 1]), + 'cutime': int(parts[_STAT_POS['cutime'] - 1]), + 'cstime': int(parts[_STAT_POS['cstime'] - 1]), + 'priority': int(parts[_STAT_POS['priority'] - 1]), + 'nice': int(parts[_STAT_POS['nice'] - 1]), + 'num_threads': int(parts[_STAT_POS['num_threads'] - 1]), + 'starttime': int(parts[_STAT_POS['starttime'] - 1]), + 'vms': int(parts[_STAT_POS['vsize'] - 1]), + 'rss': int(parts[_STAT_POS['rss'] - 1]), + 'processor': int(parts[_STAT_POS['processor'] - 1]), + } + except Exception: + cloudlog.exception("failed to parse /proc//stat") + return None + +class ProcExtra(TypedDict): + pid: int + name: str + exe: str + cmdline: list[str] + + +_proc_cache: dict[int, ProcExtra] = {} + + +def _get_proc_extra(pid: int, name: str) -> ProcExtra: + cache: ProcExtra | None = _proc_cache.get(pid) + if cache is None or cache.get('name') != name: + exe = '' + cmdline: list[str] = [] + try: + exe = os.readlink(f'/proc/{pid}/exe') + except OSError: + pass + try: + with open(f'/proc/{pid}/cmdline', 'rb') as f: + cmdline = [c.decode('utf-8', errors='replace') for c in f.read().split(b'\0') if c] + except OSError: + pass + cache = {'pid': pid, 'name': name, 'exe': exe, 'cmdline': cmdline} + _proc_cache[pid] = cache + return cache + + +def _procs() -> list[ProcStat]: + stats: list[ProcStat] = [] + for pid_str in os.listdir('/proc'): + if not pid_str.isdigit(): + continue + try: + with open(f'/proc/{pid_str}/stat') as f: + stat = f.read() + parsed = _parse_proc_stat(stat) + if parsed is not None: + stats.append(parsed) + except OSError: + continue + return stats + + +def build_proc_log_message(msg) -> None: + pl = msg.procLog + + procs = _procs() + l = pl.init('procs', len(procs)) + for i, r in enumerate(procs): + proc = l[i] + proc.pid = r['pid'] + proc.state = ord(r['state'][0]) + proc.ppid = r['ppid'] + proc.cpuUser = r['utime'] / JIFFY + proc.cpuSystem = r['stime'] / JIFFY + proc.cpuChildrenUser = r['cutime'] / JIFFY + proc.cpuChildrenSystem = r['cstime'] / JIFFY + proc.priority = r['priority'] + proc.nice = r['nice'] + proc.numThreads = r['num_threads'] + proc.startTime = r['starttime'] / JIFFY + proc.memVms = r['vms'] + proc.memRss = r['rss'] * PAGE_SIZE + proc.processor = r['processor'] + proc.name = r['name'] + + extra = _get_proc_extra(r['pid'], r['name']) + proc.exe = extra['exe'] + cmdline = proc.init('cmdline', len(extra['cmdline'])) + for j, arg in enumerate(extra['cmdline']): + cmdline[j] = arg + + cpu_times = _cpu_times() + cpu_list = pl.init('cpuTimes', len(cpu_times)) + for i, ct in enumerate(cpu_times): + cpu = cpu_list[i] + cpu.cpuNum = ct['cpuNum'] + cpu.user = ct['user'] + cpu.nice = ct['nice'] + cpu.system = ct['system'] + cpu.idle = ct['idle'] + cpu.iowait = ct['iowait'] + cpu.irq = ct['irq'] + cpu.softirq = ct['softirq'] + + mem_info = _mem_info() + pl.mem.total = mem_info["MemTotal:"] + pl.mem.free = mem_info["MemFree:"] + pl.mem.available = mem_info["MemAvailable:"] + pl.mem.buffers = mem_info["Buffers:"] + pl.mem.cached = mem_info["Cached:"] + pl.mem.active = mem_info["Active:"] + pl.mem.inactive = mem_info["Inactive:"] + pl.mem.shared = mem_info["Shmem:"] + + +def main() -> NoReturn: + pm = messaging.PubMaster(['procLog']) + rk = Ratekeeper(0.5) + while True: + msg = messaging.new_message('procLog', valid=True) + build_proc_log_message(msg) + pm.send('procLog', msg) + rk.keep_time() + + +if __name__ == '__main__': + main() diff --git a/system/proclogd/SConscript b/system/proclogd/SConscript deleted file mode 100644 index 08814d5ccb..0000000000 --- a/system/proclogd/SConscript +++ /dev/null @@ -1,6 +0,0 @@ -Import('env', 'messaging', 'common') -libs = [messaging, 'pthread', common] -env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs) - -if GetOption('extras'): - env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs) diff --git a/system/proclogd/main.cc b/system/proclogd/main.cc deleted file mode 100644 index 3f8a889eea..0000000000 --- a/system/proclogd/main.cc +++ /dev/null @@ -1,25 +0,0 @@ - -#include - -#include "common/ratekeeper.h" -#include "common/util.h" -#include "system/proclogd/proclog.h" - -ExitHandler do_exit; - -int main(int argc, char **argv) { - setpriority(PRIO_PROCESS, 0, -15); - - RateKeeper rk("proclogd", 0.5); - PubMaster publisher({"procLog"}); - - while (!do_exit) { - MessageBuilder msg; - buildProcLogMessage(msg); - publisher.send("procLog", msg); - - rk.keepTime(); - } - - return 0; -} diff --git a/system/proclogd/proclog.cc b/system/proclogd/proclog.cc deleted file mode 100644 index 09ab4f559e..0000000000 --- a/system/proclogd/proclog.cc +++ /dev/null @@ -1,239 +0,0 @@ -#include "system/proclogd/proclog.h" - -#include - -#include -#include -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -namespace Parser { - -// parse /proc/stat -std::vector cpuTimes(std::istream &stream) { - std::vector cpu_times; - std::string line; - // skip the first line for cpu total - std::getline(stream, line); - while (std::getline(stream, line)) { - if (line.compare(0, 3, "cpu") != 0) break; - - CPUTime t = {}; - std::istringstream iss(line); - if (iss.ignore(3) >> t.id >> t.utime >> t.ntime >> t.stime >> t.itime >> t.iowtime >> t.irqtime >> t.sirqtime) - cpu_times.push_back(t); - } - return cpu_times; -} - -// parse /proc/meminfo -std::unordered_map memInfo(std::istream &stream) { - std::unordered_map mem_info; - std::string line, key; - while (std::getline(stream, line)) { - uint64_t val = 0; - std::istringstream iss(line); - if (iss >> key >> val) { - mem_info[key] = val * 1024; - } - } - return mem_info; -} - -// field position (https://man7.org/linux/man-pages/man5/proc.5.html) -enum StatPos { - pid = 1, - state = 3, - ppid = 4, - utime = 14, - stime = 15, - cutime = 16, - cstime = 17, - priority = 18, - nice = 19, - num_threads = 20, - starttime = 22, - vsize = 23, - rss = 24, - processor = 39, - MAX_FIELD = 52, -}; - -// parse /proc/pid/stat -std::optional procStat(std::string stat) { - // To avoid being fooled by names containing a closing paren, scan backwards. - auto open_paren = stat.find('('); - auto close_paren = stat.rfind(')'); - if (open_paren == std::string::npos || close_paren == std::string::npos || open_paren > close_paren) { - return std::nullopt; - } - - std::string name = stat.substr(open_paren + 1, close_paren - open_paren - 1); - // replace space in name with _ - std::replace(&stat[open_paren], &stat[close_paren], ' ', '_'); - std::istringstream iss(stat); - std::vector v{std::istream_iterator(iss), - std::istream_iterator()}; - try { - if (v.size() != StatPos::MAX_FIELD) { - throw std::invalid_argument("stat"); - } - ProcStat p = { - .name = name, - .pid = stoi(v[StatPos::pid - 1]), - .state = v[StatPos::state - 1][0], - .ppid = stoi(v[StatPos::ppid - 1]), - .utime = stoul(v[StatPos::utime - 1]), - .stime = stoul(v[StatPos::stime - 1]), - .cutime = stol(v[StatPos::cutime - 1]), - .cstime = stol(v[StatPos::cstime - 1]), - .priority = stol(v[StatPos::priority - 1]), - .nice = stol(v[StatPos::nice - 1]), - .num_threads = stol(v[StatPos::num_threads - 1]), - .starttime = stoull(v[StatPos::starttime - 1]), - .vms = stoul(v[StatPos::vsize - 1]), - .rss = stol(v[StatPos::rss - 1]), - .processor = stoi(v[StatPos::processor - 1]), - }; - return p; - } catch (const std::invalid_argument &e) { - LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str()); - } catch (const std::out_of_range &e) { - LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str()); - } - return std::nullopt; -} - -// return list of PIDs from /proc -std::vector pids() { - std::vector ids; - DIR *d = opendir("/proc"); - assert(d); - char *p_end; - struct dirent *de = NULL; - while ((de = readdir(d))) { - if (de->d_type == DT_DIR) { - int pid = strtol(de->d_name, &p_end, 10); - if (p_end == (de->d_name + strlen(de->d_name))) { - ids.push_back(pid); - } - } - } - closedir(d); - return ids; -} - -// null-delimited cmdline arguments to vector -std::vector cmdline(std::istream &stream) { - std::vector ret; - std::string line; - while (std::getline(stream, line, '\0')) { - if (!line.empty()) { - ret.push_back(line); - } - } - return ret; -} - -const ProcCache &getProcExtraInfo(int pid, const std::string &name) { - static std::unordered_map proc_cache; - ProcCache &cache = proc_cache[pid]; - if (cache.pid != pid || cache.name != name) { - cache.pid = pid; - cache.name = name; - std::string proc_path = "/proc/" + std::to_string(pid); - cache.exe = util::readlink(proc_path + "/exe"); - std::ifstream stream(proc_path + "/cmdline"); - cache.cmdline = cmdline(stream); - } - return cache; -} - -} // namespace Parser - -const double jiffy = sysconf(_SC_CLK_TCK); -const size_t page_size = sysconf(_SC_PAGE_SIZE); - -void buildCPUTimes(cereal::ProcLog::Builder &builder) { - std::ifstream stream("/proc/stat"); - std::vector stats = Parser::cpuTimes(stream); - - auto log_cpu_times = builder.initCpuTimes(stats.size()); - for (int i = 0; i < stats.size(); ++i) { - auto l = log_cpu_times[i]; - const CPUTime &r = stats[i]; - l.setCpuNum(r.id); - l.setUser(r.utime / jiffy); - l.setNice(r.ntime / jiffy); - l.setSystem(r.stime / jiffy); - l.setIdle(r.itime / jiffy); - l.setIowait(r.iowtime / jiffy); - l.setIrq(r.irqtime / jiffy); - l.setSoftirq(r.sirqtime / jiffy); - } -} - -void buildMemInfo(cereal::ProcLog::Builder &builder) { - std::ifstream stream("/proc/meminfo"); - auto mem_info = Parser::memInfo(stream); - - auto mem = builder.initMem(); - mem.setTotal(mem_info["MemTotal:"]); - mem.setFree(mem_info["MemFree:"]); - mem.setAvailable(mem_info["MemAvailable:"]); - mem.setBuffers(mem_info["Buffers:"]); - mem.setCached(mem_info["Cached:"]); - mem.setActive(mem_info["Active:"]); - mem.setInactive(mem_info["Inactive:"]); - mem.setShared(mem_info["Shmem:"]); -} - -void buildProcs(cereal::ProcLog::Builder &builder) { - auto pids = Parser::pids(); - std::vector proc_stats; - proc_stats.reserve(pids.size()); - for (int pid : pids) { - std::string path = "/proc/" + std::to_string(pid) + "/stat"; - if (auto stat = Parser::procStat(util::read_file(path))) { - proc_stats.push_back(*stat); - } - } - - auto procs = builder.initProcs(proc_stats.size()); - for (size_t i = 0; i < proc_stats.size(); i++) { - auto l = procs[i]; - const ProcStat &r = proc_stats[i]; - l.setPid(r.pid); - l.setState(r.state); - l.setPpid(r.ppid); - l.setCpuUser(r.utime / jiffy); - l.setCpuSystem(r.stime / jiffy); - l.setCpuChildrenUser(r.cutime / jiffy); - l.setCpuChildrenSystem(r.cstime / jiffy); - l.setPriority(r.priority); - l.setNice(r.nice); - l.setNumThreads(r.num_threads); - l.setStartTime(r.starttime / jiffy); - l.setMemVms(r.vms); - l.setMemRss((uint64_t)r.rss * page_size); - l.setProcessor(r.processor); - l.setName(r.name); - - const ProcCache &extra_info = Parser::getProcExtraInfo(r.pid, r.name); - l.setExe(extra_info.exe); - auto lcmdline = l.initCmdline(extra_info.cmdline.size()); - for (size_t j = 0; j < lcmdline.size(); j++) { - lcmdline.set(j, extra_info.cmdline[j]); - } - } -} - -void buildProcLogMessage(MessageBuilder &msg) { - auto procLog = msg.initEvent().initProcLog(); - buildProcs(procLog); - buildCPUTimes(procLog); - buildMemInfo(procLog); -} diff --git a/system/proclogd/proclog.h b/system/proclogd/proclog.h deleted file mode 100644 index 49f97cdd36..0000000000 --- a/system/proclogd/proclog.h +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" - -struct CPUTime { - int id; - unsigned long utime, ntime, stime, itime; - unsigned long iowtime, irqtime, sirqtime; -}; - -struct ProcCache { - int pid; - std::string name, exe; - std::vector cmdline; -}; - -struct ProcStat { - int pid, ppid, processor; - char state; - long cutime, cstime, priority, nice, num_threads, rss; - unsigned long utime, stime, vms; - unsigned long long starttime; - std::string name; -}; - -namespace Parser { - -std::vector pids(); -std::optional procStat(std::string stat); -std::vector cmdline(std::istream &stream); -std::vector cpuTimes(std::istream &stream); -std::unordered_map memInfo(std::istream &stream); -const ProcCache &getProcExtraInfo(int pid, const std::string &name); - -}; // namespace Parser - -void buildProcLogMessage(MessageBuilder &msg); diff --git a/system/proclogd/tests/.gitignore b/system/proclogd/tests/.gitignore deleted file mode 100644 index 5230b1598d..0000000000 --- a/system/proclogd/tests/.gitignore +++ /dev/null @@ -1 +0,0 @@ -test_proclog diff --git a/system/proclogd/tests/test_proclog.cc b/system/proclogd/tests/test_proclog.cc deleted file mode 100644 index b86229a499..0000000000 --- a/system/proclogd/tests/test_proclog.cc +++ /dev/null @@ -1,142 +0,0 @@ -#define CATCH_CONFIG_MAIN -#include "catch2/catch.hpp" -#include "common/util.h" -#include "system/proclogd/proclog.h" - -const std::string allowed_states = "RSDTZtWXxKWPI"; - -TEST_CASE("Parser::procStat") { - SECTION("from string") { - const std::string stat_str = - "33012 (code )) S 32978 6620 6620 0 -1 4194368 2042377 0 144 0 24510 11627 0 " - "0 20 0 39 0 53077 830029824 62214 18446744073709551615 94257242783744 94257366235808 " - "140735738643248 0 0 0 0 4098 1073808632 0 0 0 17 2 0 0 2 0 0 94257370858656 94257371248232 " - "94257404952576 140735738648768 140735738648823 140735738648823 140735738650595 0"; - auto stat = Parser::procStat(stat_str); - REQUIRE(stat); - REQUIRE(stat->pid == 33012); - REQUIRE(stat->name == "code )"); - REQUIRE(stat->state == 'S'); - REQUIRE(stat->ppid == 32978); - REQUIRE(stat->utime == 24510); - REQUIRE(stat->stime == 11627); - REQUIRE(stat->cutime == 0); - REQUIRE(stat->cstime == 0); - REQUIRE(stat->priority == 20); - REQUIRE(stat->nice == 0); - REQUIRE(stat->num_threads == 39); - REQUIRE(stat->starttime == 53077); - REQUIRE(stat->vms == 830029824); - REQUIRE(stat->rss == 62214); - REQUIRE(stat->processor == 2); - } - SECTION("all processes") { - std::vector pids = Parser::pids(); - REQUIRE(pids.size() > 1); - for (int pid : pids) { - std::string stat_path = "/proc/" + std::to_string(pid) + "/stat"; - INFO(stat_path); - if (auto stat = Parser::procStat(util::read_file(stat_path))) { - REQUIRE(stat->pid == pid); - REQUIRE(allowed_states.find(stat->state) != std::string::npos); - } else { - REQUIRE(util::file_exists(stat_path) == false); - } - } - } -} - -TEST_CASE("Parser::cpuTimes") { - SECTION("from string") { - std::string stat = - "cpu 0 0 0 0 0 0 0 0 0 0\n" - "cpu0 1 2 3 4 5 6 7 8 9 10\n" - "cpu1 1 2 3 4 5 6 7 8 9 10\n"; - std::istringstream stream(stat); - auto stats = Parser::cpuTimes(stream); - REQUIRE(stats.size() == 2); - for (int i = 0; i < stats.size(); ++i) { - REQUIRE(stats[i].id == i); - REQUIRE(stats[i].utime == 1); - REQUIRE(stats[i].ntime ==2); - REQUIRE(stats[i].stime == 3); - REQUIRE(stats[i].itime == 4); - REQUIRE(stats[i].iowtime == 5); - REQUIRE(stats[i].irqtime == 6); - REQUIRE(stats[i].sirqtime == 7); - } - } - SECTION("all cpus") { - std::istringstream stream(util::read_file("/proc/stat")); - auto stats = Parser::cpuTimes(stream); - REQUIRE(stats.size() == sysconf(_SC_NPROCESSORS_ONLN)); - for (int i = 0; i < stats.size(); ++i) { - REQUIRE(stats[i].id == i); - } - } -} - -TEST_CASE("Parser::memInfo") { - SECTION("from string") { - std::istringstream stream("MemTotal: 1024 kb\nMemFree: 2048 kb\n"); - auto meminfo = Parser::memInfo(stream); - REQUIRE(meminfo["MemTotal:"] == 1024 * 1024); - REQUIRE(meminfo["MemFree:"] == 2048 * 1024); - } - SECTION("from /proc/meminfo") { - std::string require_keys[] = {"MemTotal:", "MemFree:", "MemAvailable:", "Buffers:", "Cached:", "Active:", "Inactive:", "Shmem:"}; - std::istringstream stream(util::read_file("/proc/meminfo")); - auto meminfo = Parser::memInfo(stream); - for (auto &key : require_keys) { - REQUIRE(meminfo.find(key) != meminfo.end()); - REQUIRE(meminfo[key] > 0); - } - } -} - -void test_cmdline(std::string cmdline, const std::vector requires) { - std::stringstream ss; - ss.write(&cmdline[0], cmdline.size()); - auto cmds = Parser::cmdline(ss); - REQUIRE(cmds.size() == requires.size()); - for (int i = 0; i < requires.size(); ++i) { - REQUIRE(cmds[i] == requires[i]); - } -} -TEST_CASE("Parser::cmdline") { - test_cmdline(std::string("a\0b\0c\0", 7), {"a", "b", "c"}); - test_cmdline(std::string("a\0\0c\0", 6), {"a", "c"}); - test_cmdline(std::string("a\0b\0c\0\0\0", 9), {"a", "b", "c"}); -} - -TEST_CASE("buildProcLoggerMessage") { - MessageBuilder msg; - buildProcLogMessage(msg); - - kj::Array buf = capnp::messageToFlatArray(msg); - capnp::FlatArrayMessageReader reader(buf); - auto log = reader.getRoot().getProcLog(); - REQUIRE(log.totalSize().wordCount > 0); - - // test cereal::ProcLog::CPUTimes - auto cpu_times = log.getCpuTimes(); - REQUIRE(cpu_times.size() == sysconf(_SC_NPROCESSORS_ONLN)); - REQUIRE(cpu_times[cpu_times.size() - 1].getCpuNum() == cpu_times.size() - 1); - - // test cereal::ProcLog::Mem - auto mem = log.getMem(); - REQUIRE(mem.getTotal() > 0); - REQUIRE(mem.getShared() > 0); - - // test cereal::ProcLog::Process - auto procs = log.getProcs(); - for (auto p : procs) { - REQUIRE(allowed_states.find(p.getState()) != std::string::npos); - if (p.getPid() == ::getpid()) { - REQUIRE(p.getName() == "test_proclog"); - REQUIRE(p.getState() == 'R'); - REQUIRE_THAT(p.getExe().cStr(), Catch::Matchers::Contains("test_proclog")); - REQUIRE_THAT(p.getCmdline()[0], Catch::Matchers::Contains("test_proclog")); - } - } -} diff --git a/system/ubloxd/.gitignore b/system/ubloxd/.gitignore deleted file mode 100644 index 05263ff67c..0000000000 --- a/system/ubloxd/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -ubloxd -tests/test_glonass_runner diff --git a/system/ubloxd/SConscript b/system/ubloxd/SConscript index ce09e235e6..9eb50760ba 100644 --- a/system/ubloxd/SConscript +++ b/system/ubloxd/SConscript @@ -1,20 +1,11 @@ -Import('env', 'common', 'messaging') - -loc_libs = [messaging, common, 'kaitai', 'pthread'] +Import('env') if GetOption('kaitai'): - generated = Dir('generated').srcnode().abspath - cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES" - env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd) - env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd) - glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd) - + current_dir = Dir('./generated/').srcnode().abspath + python_cmd = f"kaitai-struct-compiler --target python --outdir {current_dir} $SOURCES" + env.Command(File('./generated/ubx.py'), 'ubx.ksy', python_cmd) + env.Command(File('./generated/gps.py'), 'gps.ksy', python_cmd) + env.Command(File('./generated/glonass.py'), 'glonass.ksy', python_cmd) # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910 - patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES') - env.Depends(patch, glonass) - -glonass_obj = env.Object('generated/glonass.cpp') -env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs) - -if GetOption('extras'): - env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file + py_glonass_fix = env.Command(None, File('./generated/glonass.py'), "sed -i 's/self._io.align_to_byte()/# self._io.align_to_byte()/' $SOURCES") + env.Depends(py_glonass_fix, File('./generated/glonass.py')) diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp deleted file mode 100644 index cd0f96ab68..0000000000 --- a/system/ubloxd/generated/glonass.cpp +++ /dev/null @@ -1,353 +0,0 @@ -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "glonass.h" - -glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = this; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::_read() { - m_idle_chip = m__io->read_bits_int_be(1); - m_string_number = m__io->read_bits_int_be(4); - //m__io->align_to_byte(); - switch (string_number()) { - case 4: { - m_data = new string_4_t(m__io, this, m__root); - break; - } - case 1: { - m_data = new string_1_t(m__io, this, m__root); - break; - } - case 3: { - m_data = new string_3_t(m__io, this, m__root); - break; - } - case 5: { - m_data = new string_5_t(m__io, this, m__root); - break; - } - case 2: { - m_data = new string_2_t(m__io, this, m__root); - break; - } - default: { - m_data = new string_non_immediate_t(m__io, this, m__root); - break; - } - } - m_hamming_code = m__io->read_bits_int_be(8); - m_pad_1 = m__io->read_bits_int_be(11); - m_superframe_number = m__io->read_bits_int_be(16); - m_pad_2 = m__io->read_bits_int_be(8); - m_frame_number = m__io->read_bits_int_be(8); -} - -glonass_t::~glonass_t() { - _clean_up(); -} - -void glonass_t::_clean_up() { - if (m_data) { - delete m_data; m_data = 0; - } -} - -glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_tau_n = false; - f_delta_tau_n = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_4_t::_read() { - m_tau_n_sign = m__io->read_bits_int_be(1); - m_tau_n_value = m__io->read_bits_int_be(21); - m_delta_tau_n_sign = m__io->read_bits_int_be(1); - m_delta_tau_n_value = m__io->read_bits_int_be(4); - m_e_n = m__io->read_bits_int_be(5); - m_not_used_1 = m__io->read_bits_int_be(14); - m_p4 = m__io->read_bits_int_be(1); - m_f_t = m__io->read_bits_int_be(4); - m_not_used_2 = m__io->read_bits_int_be(3); - m_n_t = m__io->read_bits_int_be(11); - m_n = m__io->read_bits_int_be(5); - m_m = m__io->read_bits_int_be(2); -} - -glonass_t::string_4_t::~string_4_t() { - _clean_up(); -} - -void glonass_t::string_4_t::_clean_up() { -} - -int32_t glonass_t::string_4_t::tau_n() { - if (f_tau_n) - return m_tau_n; - m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value())); - f_tau_n = true; - return m_tau_n; -} - -int32_t glonass_t::string_4_t::delta_tau_n() { - if (f_delta_tau_n) - return m_delta_tau_n; - m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value())); - f_delta_tau_n = true; - return m_delta_tau_n; -} - -glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_non_immediate_t::_read() { - m_data_1 = m__io->read_bits_int_be(64); - m_data_2 = m__io->read_bits_int_be(8); -} - -glonass_t::string_non_immediate_t::~string_non_immediate_t() { - _clean_up(); -} - -void glonass_t::string_non_immediate_t::_clean_up() { -} - -glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_5_t::_read() { - m_n_a = m__io->read_bits_int_be(11); - m_tau_c = m__io->read_bits_int_be(32); - m_not_used = m__io->read_bits_int_be(1); - m_n_4 = m__io->read_bits_int_be(5); - m_tau_gps = m__io->read_bits_int_be(22); - m_l_n = m__io->read_bits_int_be(1); -} - -glonass_t::string_5_t::~string_5_t() { - _clean_up(); -} - -void glonass_t::string_5_t::_clean_up() { -} - -glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_x_vel = false; - f_x_accel = false; - f_x = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_1_t::_read() { - m_not_used = m__io->read_bits_int_be(2); - m_p1 = m__io->read_bits_int_be(2); - m_t_k = m__io->read_bits_int_be(12); - m_x_vel_sign = m__io->read_bits_int_be(1); - m_x_vel_value = m__io->read_bits_int_be(23); - m_x_accel_sign = m__io->read_bits_int_be(1); - m_x_accel_value = m__io->read_bits_int_be(4); - m_x_sign = m__io->read_bits_int_be(1); - m_x_value = m__io->read_bits_int_be(26); -} - -glonass_t::string_1_t::~string_1_t() { - _clean_up(); -} - -void glonass_t::string_1_t::_clean_up() { -} - -int32_t glonass_t::string_1_t::x_vel() { - if (f_x_vel) - return m_x_vel; - m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value())); - f_x_vel = true; - return m_x_vel; -} - -int32_t glonass_t::string_1_t::x_accel() { - if (f_x_accel) - return m_x_accel; - m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value())); - f_x_accel = true; - return m_x_accel; -} - -int32_t glonass_t::string_1_t::x() { - if (f_x) - return m_x; - m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value())); - f_x = true; - return m_x; -} - -glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_y_vel = false; - f_y_accel = false; - f_y = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_2_t::_read() { - m_b_n = m__io->read_bits_int_be(3); - m_p2 = m__io->read_bits_int_be(1); - m_t_b = m__io->read_bits_int_be(7); - m_not_used = m__io->read_bits_int_be(5); - m_y_vel_sign = m__io->read_bits_int_be(1); - m_y_vel_value = m__io->read_bits_int_be(23); - m_y_accel_sign = m__io->read_bits_int_be(1); - m_y_accel_value = m__io->read_bits_int_be(4); - m_y_sign = m__io->read_bits_int_be(1); - m_y_value = m__io->read_bits_int_be(26); -} - -glonass_t::string_2_t::~string_2_t() { - _clean_up(); -} - -void glonass_t::string_2_t::_clean_up() { -} - -int32_t glonass_t::string_2_t::y_vel() { - if (f_y_vel) - return m_y_vel; - m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value())); - f_y_vel = true; - return m_y_vel; -} - -int32_t glonass_t::string_2_t::y_accel() { - if (f_y_accel) - return m_y_accel; - m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value())); - f_y_accel = true; - return m_y_accel; -} - -int32_t glonass_t::string_2_t::y() { - if (f_y) - return m_y; - m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value())); - f_y = true; - return m_y; -} - -glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_gamma_n = false; - f_z_vel = false; - f_z_accel = false; - f_z = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void glonass_t::string_3_t::_read() { - m_p3 = m__io->read_bits_int_be(1); - m_gamma_n_sign = m__io->read_bits_int_be(1); - m_gamma_n_value = m__io->read_bits_int_be(10); - m_not_used = m__io->read_bits_int_be(1); - m_p = m__io->read_bits_int_be(2); - m_l_n = m__io->read_bits_int_be(1); - m_z_vel_sign = m__io->read_bits_int_be(1); - m_z_vel_value = m__io->read_bits_int_be(23); - m_z_accel_sign = m__io->read_bits_int_be(1); - m_z_accel_value = m__io->read_bits_int_be(4); - m_z_sign = m__io->read_bits_int_be(1); - m_z_value = m__io->read_bits_int_be(26); -} - -glonass_t::string_3_t::~string_3_t() { - _clean_up(); -} - -void glonass_t::string_3_t::_clean_up() { -} - -int32_t glonass_t::string_3_t::gamma_n() { - if (f_gamma_n) - return m_gamma_n; - m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value())); - f_gamma_n = true; - return m_gamma_n; -} - -int32_t glonass_t::string_3_t::z_vel() { - if (f_z_vel) - return m_z_vel; - m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value())); - f_z_vel = true; - return m_z_vel; -} - -int32_t glonass_t::string_3_t::z_accel() { - if (f_z_accel) - return m_z_accel; - m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value())); - f_z_accel = true; - return m_z_accel; -} - -int32_t glonass_t::string_3_t::z() { - if (f_z) - return m_z; - m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value())); - f_z = true; - return m_z; -} diff --git a/system/ubloxd/generated/glonass.h b/system/ubloxd/generated/glonass.h deleted file mode 100644 index 19867ba22b..0000000000 --- a/system/ubloxd/generated/glonass.h +++ /dev/null @@ -1,375 +0,0 @@ -#ifndef GLONASS_H_ -#define GLONASS_H_ - -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "kaitai/kaitaistruct.h" -#include - -#if KAITAI_STRUCT_VERSION < 9000L -#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" -#endif - -class glonass_t : public kaitai::kstruct { - -public: - class string_4_t; - class string_non_immediate_t; - class string_5_t; - class string_1_t; - class string_2_t; - class string_3_t; - - glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0); - -private: - void _read(); - void _clean_up(); - -public: - ~glonass_t(); - - class string_4_t : public kaitai::kstruct { - - public: - - string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_4_t(); - - private: - bool f_tau_n; - int32_t m_tau_n; - - public: - int32_t tau_n(); - - private: - bool f_delta_tau_n; - int32_t m_delta_tau_n; - - public: - int32_t delta_tau_n(); - - private: - bool m_tau_n_sign; - uint64_t m_tau_n_value; - bool m_delta_tau_n_sign; - uint64_t m_delta_tau_n_value; - uint64_t m_e_n; - uint64_t m_not_used_1; - bool m_p4; - uint64_t m_f_t; - uint64_t m_not_used_2; - uint64_t m_n_t; - uint64_t m_n; - uint64_t m_m; - glonass_t* m__root; - glonass_t* m__parent; - - public: - bool tau_n_sign() const { return m_tau_n_sign; } - uint64_t tau_n_value() const { return m_tau_n_value; } - bool delta_tau_n_sign() const { return m_delta_tau_n_sign; } - uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; } - uint64_t e_n() const { return m_e_n; } - uint64_t not_used_1() const { return m_not_used_1; } - bool p4() const { return m_p4; } - uint64_t f_t() const { return m_f_t; } - uint64_t not_used_2() const { return m_not_used_2; } - uint64_t n_t() const { return m_n_t; } - uint64_t n() const { return m_n; } - uint64_t m() const { return m_m; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - - class string_non_immediate_t : public kaitai::kstruct { - - public: - - string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_non_immediate_t(); - - private: - uint64_t m_data_1; - uint64_t m_data_2; - glonass_t* m__root; - glonass_t* m__parent; - - public: - uint64_t data_1() const { return m_data_1; } - uint64_t data_2() const { return m_data_2; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - - class string_5_t : public kaitai::kstruct { - - public: - - string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_5_t(); - - private: - uint64_t m_n_a; - uint64_t m_tau_c; - bool m_not_used; - uint64_t m_n_4; - uint64_t m_tau_gps; - bool m_l_n; - glonass_t* m__root; - glonass_t* m__parent; - - public: - uint64_t n_a() const { return m_n_a; } - uint64_t tau_c() const { return m_tau_c; } - bool not_used() const { return m_not_used; } - uint64_t n_4() const { return m_n_4; } - uint64_t tau_gps() const { return m_tau_gps; } - bool l_n() const { return m_l_n; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - - class string_1_t : public kaitai::kstruct { - - public: - - string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_1_t(); - - private: - bool f_x_vel; - int32_t m_x_vel; - - public: - int32_t x_vel(); - - private: - bool f_x_accel; - int32_t m_x_accel; - - public: - int32_t x_accel(); - - private: - bool f_x; - int32_t m_x; - - public: - int32_t x(); - - private: - uint64_t m_not_used; - uint64_t m_p1; - uint64_t m_t_k; - bool m_x_vel_sign; - uint64_t m_x_vel_value; - bool m_x_accel_sign; - uint64_t m_x_accel_value; - bool m_x_sign; - uint64_t m_x_value; - glonass_t* m__root; - glonass_t* m__parent; - - public: - uint64_t not_used() const { return m_not_used; } - uint64_t p1() const { return m_p1; } - uint64_t t_k() const { return m_t_k; } - bool x_vel_sign() const { return m_x_vel_sign; } - uint64_t x_vel_value() const { return m_x_vel_value; } - bool x_accel_sign() const { return m_x_accel_sign; } - uint64_t x_accel_value() const { return m_x_accel_value; } - bool x_sign() const { return m_x_sign; } - uint64_t x_value() const { return m_x_value; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - - class string_2_t : public kaitai::kstruct { - - public: - - string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_2_t(); - - private: - bool f_y_vel; - int32_t m_y_vel; - - public: - int32_t y_vel(); - - private: - bool f_y_accel; - int32_t m_y_accel; - - public: - int32_t y_accel(); - - private: - bool f_y; - int32_t m_y; - - public: - int32_t y(); - - private: - uint64_t m_b_n; - bool m_p2; - uint64_t m_t_b; - uint64_t m_not_used; - bool m_y_vel_sign; - uint64_t m_y_vel_value; - bool m_y_accel_sign; - uint64_t m_y_accel_value; - bool m_y_sign; - uint64_t m_y_value; - glonass_t* m__root; - glonass_t* m__parent; - - public: - uint64_t b_n() const { return m_b_n; } - bool p2() const { return m_p2; } - uint64_t t_b() const { return m_t_b; } - uint64_t not_used() const { return m_not_used; } - bool y_vel_sign() const { return m_y_vel_sign; } - uint64_t y_vel_value() const { return m_y_vel_value; } - bool y_accel_sign() const { return m_y_accel_sign; } - uint64_t y_accel_value() const { return m_y_accel_value; } - bool y_sign() const { return m_y_sign; } - uint64_t y_value() const { return m_y_value; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - - class string_3_t : public kaitai::kstruct { - - public: - - string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~string_3_t(); - - private: - bool f_gamma_n; - int32_t m_gamma_n; - - public: - int32_t gamma_n(); - - private: - bool f_z_vel; - int32_t m_z_vel; - - public: - int32_t z_vel(); - - private: - bool f_z_accel; - int32_t m_z_accel; - - public: - int32_t z_accel(); - - private: - bool f_z; - int32_t m_z; - - public: - int32_t z(); - - private: - bool m_p3; - bool m_gamma_n_sign; - uint64_t m_gamma_n_value; - bool m_not_used; - uint64_t m_p; - bool m_l_n; - bool m_z_vel_sign; - uint64_t m_z_vel_value; - bool m_z_accel_sign; - uint64_t m_z_accel_value; - bool m_z_sign; - uint64_t m_z_value; - glonass_t* m__root; - glonass_t* m__parent; - - public: - bool p3() const { return m_p3; } - bool gamma_n_sign() const { return m_gamma_n_sign; } - uint64_t gamma_n_value() const { return m_gamma_n_value; } - bool not_used() const { return m_not_used; } - uint64_t p() const { return m_p; } - bool l_n() const { return m_l_n; } - bool z_vel_sign() const { return m_z_vel_sign; } - uint64_t z_vel_value() const { return m_z_vel_value; } - bool z_accel_sign() const { return m_z_accel_sign; } - uint64_t z_accel_value() const { return m_z_accel_value; } - bool z_sign() const { return m_z_sign; } - uint64_t z_value() const { return m_z_value; } - glonass_t* _root() const { return m__root; } - glonass_t* _parent() const { return m__parent; } - }; - -private: - bool m_idle_chip; - uint64_t m_string_number; - kaitai::kstruct* m_data; - uint64_t m_hamming_code; - uint64_t m_pad_1; - uint64_t m_superframe_number; - uint64_t m_pad_2; - uint64_t m_frame_number; - glonass_t* m__root; - kaitai::kstruct* m__parent; - -public: - bool idle_chip() const { return m_idle_chip; } - uint64_t string_number() const { return m_string_number; } - kaitai::kstruct* data() const { return m_data; } - uint64_t hamming_code() const { return m_hamming_code; } - uint64_t pad_1() const { return m_pad_1; } - uint64_t superframe_number() const { return m_superframe_number; } - uint64_t pad_2() const { return m_pad_2; } - uint64_t frame_number() const { return m_frame_number; } - glonass_t* _root() const { return m__root; } - kaitai::kstruct* _parent() const { return m__parent; } -}; - -#endif // GLONASS_H_ diff --git a/system/ubloxd/generated/glonass.py b/system/ubloxd/generated/glonass.py new file mode 100644 index 0000000000..40aa16bb6f --- /dev/null +++ b/system/ubloxd/generated/glonass.py @@ -0,0 +1,247 @@ +# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +import kaitaistruct +from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO + + +if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9): + raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__)) + +class Glonass(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.idle_chip = self._io.read_bits_int_be(1) != 0 + self.string_number = self._io.read_bits_int_be(4) + # workaround for kaitai bit alignment issue (see glonass_fix.patch for C++) + # self._io.align_to_byte() + _on = self.string_number + if _on == 4: + self.data = Glonass.String4(self._io, self, self._root) + elif _on == 1: + self.data = Glonass.String1(self._io, self, self._root) + elif _on == 3: + self.data = Glonass.String3(self._io, self, self._root) + elif _on == 5: + self.data = Glonass.String5(self._io, self, self._root) + elif _on == 2: + self.data = Glonass.String2(self._io, self, self._root) + else: + self.data = Glonass.StringNonImmediate(self._io, self, self._root) + self.hamming_code = self._io.read_bits_int_be(8) + self.pad_1 = self._io.read_bits_int_be(11) + self.superframe_number = self._io.read_bits_int_be(16) + self.pad_2 = self._io.read_bits_int_be(8) + self.frame_number = self._io.read_bits_int_be(8) + + class String4(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.tau_n_sign = self._io.read_bits_int_be(1) != 0 + self.tau_n_value = self._io.read_bits_int_be(21) + self.delta_tau_n_sign = self._io.read_bits_int_be(1) != 0 + self.delta_tau_n_value = self._io.read_bits_int_be(4) + self.e_n = self._io.read_bits_int_be(5) + self.not_used_1 = self._io.read_bits_int_be(14) + self.p4 = self._io.read_bits_int_be(1) != 0 + self.f_t = self._io.read_bits_int_be(4) + self.not_used_2 = self._io.read_bits_int_be(3) + self.n_t = self._io.read_bits_int_be(11) + self.n = self._io.read_bits_int_be(5) + self.m = self._io.read_bits_int_be(2) + + @property + def tau_n(self): + if hasattr(self, '_m_tau_n'): + return self._m_tau_n + + self._m_tau_n = ((self.tau_n_value * -1) if self.tau_n_sign else self.tau_n_value) + return getattr(self, '_m_tau_n', None) + + @property + def delta_tau_n(self): + if hasattr(self, '_m_delta_tau_n'): + return self._m_delta_tau_n + + self._m_delta_tau_n = ((self.delta_tau_n_value * -1) if self.delta_tau_n_sign else self.delta_tau_n_value) + return getattr(self, '_m_delta_tau_n', None) + + + class StringNonImmediate(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.data_1 = self._io.read_bits_int_be(64) + self.data_2 = self._io.read_bits_int_be(8) + + + class String5(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.n_a = self._io.read_bits_int_be(11) + self.tau_c = self._io.read_bits_int_be(32) + self.not_used = self._io.read_bits_int_be(1) != 0 + self.n_4 = self._io.read_bits_int_be(5) + self.tau_gps = self._io.read_bits_int_be(22) + self.l_n = self._io.read_bits_int_be(1) != 0 + + + class String1(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.not_used = self._io.read_bits_int_be(2) + self.p1 = self._io.read_bits_int_be(2) + self.t_k = self._io.read_bits_int_be(12) + self.x_vel_sign = self._io.read_bits_int_be(1) != 0 + self.x_vel_value = self._io.read_bits_int_be(23) + self.x_accel_sign = self._io.read_bits_int_be(1) != 0 + self.x_accel_value = self._io.read_bits_int_be(4) + self.x_sign = self._io.read_bits_int_be(1) != 0 + self.x_value = self._io.read_bits_int_be(26) + + @property + def x_vel(self): + if hasattr(self, '_m_x_vel'): + return self._m_x_vel + + self._m_x_vel = ((self.x_vel_value * -1) if self.x_vel_sign else self.x_vel_value) + return getattr(self, '_m_x_vel', None) + + @property + def x_accel(self): + if hasattr(self, '_m_x_accel'): + return self._m_x_accel + + self._m_x_accel = ((self.x_accel_value * -1) if self.x_accel_sign else self.x_accel_value) + return getattr(self, '_m_x_accel', None) + + @property + def x(self): + if hasattr(self, '_m_x'): + return self._m_x + + self._m_x = ((self.x_value * -1) if self.x_sign else self.x_value) + return getattr(self, '_m_x', None) + + + class String2(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.b_n = self._io.read_bits_int_be(3) + self.p2 = self._io.read_bits_int_be(1) != 0 + self.t_b = self._io.read_bits_int_be(7) + self.not_used = self._io.read_bits_int_be(5) + self.y_vel_sign = self._io.read_bits_int_be(1) != 0 + self.y_vel_value = self._io.read_bits_int_be(23) + self.y_accel_sign = self._io.read_bits_int_be(1) != 0 + self.y_accel_value = self._io.read_bits_int_be(4) + self.y_sign = self._io.read_bits_int_be(1) != 0 + self.y_value = self._io.read_bits_int_be(26) + + @property + def y_vel(self): + if hasattr(self, '_m_y_vel'): + return self._m_y_vel + + self._m_y_vel = ((self.y_vel_value * -1) if self.y_vel_sign else self.y_vel_value) + return getattr(self, '_m_y_vel', None) + + @property + def y_accel(self): + if hasattr(self, '_m_y_accel'): + return self._m_y_accel + + self._m_y_accel = ((self.y_accel_value * -1) if self.y_accel_sign else self.y_accel_value) + return getattr(self, '_m_y_accel', None) + + @property + def y(self): + if hasattr(self, '_m_y'): + return self._m_y + + self._m_y = ((self.y_value * -1) if self.y_sign else self.y_value) + return getattr(self, '_m_y', None) + + + class String3(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.p3 = self._io.read_bits_int_be(1) != 0 + self.gamma_n_sign = self._io.read_bits_int_be(1) != 0 + self.gamma_n_value = self._io.read_bits_int_be(10) + self.not_used = self._io.read_bits_int_be(1) != 0 + self.p = self._io.read_bits_int_be(2) + self.l_n = self._io.read_bits_int_be(1) != 0 + self.z_vel_sign = self._io.read_bits_int_be(1) != 0 + self.z_vel_value = self._io.read_bits_int_be(23) + self.z_accel_sign = self._io.read_bits_int_be(1) != 0 + self.z_accel_value = self._io.read_bits_int_be(4) + self.z_sign = self._io.read_bits_int_be(1) != 0 + self.z_value = self._io.read_bits_int_be(26) + + @property + def gamma_n(self): + if hasattr(self, '_m_gamma_n'): + return self._m_gamma_n + + self._m_gamma_n = ((self.gamma_n_value * -1) if self.gamma_n_sign else self.gamma_n_value) + return getattr(self, '_m_gamma_n', None) + + @property + def z_vel(self): + if hasattr(self, '_m_z_vel'): + return self._m_z_vel + + self._m_z_vel = ((self.z_vel_value * -1) if self.z_vel_sign else self.z_vel_value) + return getattr(self, '_m_z_vel', None) + + @property + def z_accel(self): + if hasattr(self, '_m_z_accel'): + return self._m_z_accel + + self._m_z_accel = ((self.z_accel_value * -1) if self.z_accel_sign else self.z_accel_value) + return getattr(self, '_m_z_accel', None) + + @property + def z(self): + if hasattr(self, '_m_z'): + return self._m_z + + self._m_z = ((self.z_value * -1) if self.z_sign else self.z_value) + return getattr(self, '_m_z', None) + + diff --git a/system/ubloxd/generated/gps.cpp b/system/ubloxd/generated/gps.cpp deleted file mode 100644 index 8e1cb85b95..0000000000 --- a/system/ubloxd/generated/gps.cpp +++ /dev/null @@ -1,325 +0,0 @@ -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "gps.h" -#include "kaitai/exceptions.h" - -gps_t::gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = this; - m_tlm = 0; - m_how = 0; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::_read() { - m_tlm = new tlm_t(m__io, this, m__root); - m_how = new how_t(m__io, this, m__root); - n_body = true; - switch (how()->subframe_id()) { - case 1: { - n_body = false; - m_body = new subframe_1_t(m__io, this, m__root); - break; - } - case 2: { - n_body = false; - m_body = new subframe_2_t(m__io, this, m__root); - break; - } - case 3: { - n_body = false; - m_body = new subframe_3_t(m__io, this, m__root); - break; - } - case 4: { - n_body = false; - m_body = new subframe_4_t(m__io, this, m__root); - break; - } - } -} - -gps_t::~gps_t() { - _clean_up(); -} - -void gps_t::_clean_up() { - if (m_tlm) { - delete m_tlm; m_tlm = 0; - } - if (m_how) { - delete m_how; m_how = 0; - } - if (!n_body) { - if (m_body) { - delete m_body; m_body = 0; - } - } -} - -gps_t::subframe_1_t::subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_af_0 = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::subframe_1_t::_read() { - m_week_no = m__io->read_bits_int_be(10); - m_code = m__io->read_bits_int_be(2); - m_sv_accuracy = m__io->read_bits_int_be(4); - m_sv_health = m__io->read_bits_int_be(6); - m_iodc_msb = m__io->read_bits_int_be(2); - m_l2_p_data_flag = m__io->read_bits_int_be(1); - m_reserved1 = m__io->read_bits_int_be(23); - m_reserved2 = m__io->read_bits_int_be(24); - m_reserved3 = m__io->read_bits_int_be(24); - m_reserved4 = m__io->read_bits_int_be(16); - m__io->align_to_byte(); - m_t_gd = m__io->read_s1(); - m_iodc_lsb = m__io->read_u1(); - m_t_oc = m__io->read_u2be(); - m_af_2 = m__io->read_s1(); - m_af_1 = m__io->read_s2be(); - m_af_0_sign = m__io->read_bits_int_be(1); - m_af_0_value = m__io->read_bits_int_be(21); - m_reserved5 = m__io->read_bits_int_be(2); -} - -gps_t::subframe_1_t::~subframe_1_t() { - _clean_up(); -} - -void gps_t::subframe_1_t::_clean_up() { -} - -int32_t gps_t::subframe_1_t::af_0() { - if (f_af_0) - return m_af_0; - m_af_0 = ((af_0_sign()) ? ((af_0_value() - (1 << 21))) : (af_0_value())); - f_af_0 = true; - return m_af_0; -} - -gps_t::subframe_3_t::subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - f_omega_dot = false; - f_idot = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::subframe_3_t::_read() { - m_c_ic = m__io->read_s2be(); - m_omega_0 = m__io->read_s4be(); - m_c_is = m__io->read_s2be(); - m_i_0 = m__io->read_s4be(); - m_c_rc = m__io->read_s2be(); - m_omega = m__io->read_s4be(); - m_omega_dot_sign = m__io->read_bits_int_be(1); - m_omega_dot_value = m__io->read_bits_int_be(23); - m__io->align_to_byte(); - m_iode = m__io->read_u1(); - m_idot_sign = m__io->read_bits_int_be(1); - m_idot_value = m__io->read_bits_int_be(13); - m_reserved = m__io->read_bits_int_be(2); -} - -gps_t::subframe_3_t::~subframe_3_t() { - _clean_up(); -} - -void gps_t::subframe_3_t::_clean_up() { -} - -int32_t gps_t::subframe_3_t::omega_dot() { - if (f_omega_dot) - return m_omega_dot; - m_omega_dot = ((omega_dot_sign()) ? ((omega_dot_value() - (1 << 23))) : (omega_dot_value())); - f_omega_dot = true; - return m_omega_dot; -} - -int32_t gps_t::subframe_3_t::idot() { - if (f_idot) - return m_idot; - m_idot = ((idot_sign()) ? ((idot_value() - (1 << 13))) : (idot_value())); - f_idot = true; - return m_idot; -} - -gps_t::subframe_4_t::subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::subframe_4_t::_read() { - m_data_id = m__io->read_bits_int_be(2); - m_page_id = m__io->read_bits_int_be(6); - m__io->align_to_byte(); - n_body = true; - switch (page_id()) { - case 56: { - n_body = false; - m_body = new ionosphere_data_t(m__io, this, m__root); - break; - } - } -} - -gps_t::subframe_4_t::~subframe_4_t() { - _clean_up(); -} - -void gps_t::subframe_4_t::_clean_up() { - if (!n_body) { - if (m_body) { - delete m_body; m_body = 0; - } - } -} - -gps_t::subframe_4_t::ionosphere_data_t::ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::subframe_4_t::ionosphere_data_t::_read() { - m_a0 = m__io->read_s1(); - m_a1 = m__io->read_s1(); - m_a2 = m__io->read_s1(); - m_a3 = m__io->read_s1(); - m_b0 = m__io->read_s1(); - m_b1 = m__io->read_s1(); - m_b2 = m__io->read_s1(); - m_b3 = m__io->read_s1(); -} - -gps_t::subframe_4_t::ionosphere_data_t::~ionosphere_data_t() { - _clean_up(); -} - -void gps_t::subframe_4_t::ionosphere_data_t::_clean_up() { -} - -gps_t::how_t::how_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::how_t::_read() { - m_tow_count = m__io->read_bits_int_be(17); - m_alert = m__io->read_bits_int_be(1); - m_anti_spoof = m__io->read_bits_int_be(1); - m_subframe_id = m__io->read_bits_int_be(3); - m_reserved = m__io->read_bits_int_be(2); -} - -gps_t::how_t::~how_t() { - _clean_up(); -} - -void gps_t::how_t::_clean_up() { -} - -gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::tlm_t::_read() { - m_preamble = m__io->read_bytes(1); - if (!(preamble() == std::string("\x8B", 1))) { - throw kaitai::validation_not_equal_error(std::string("\x8B", 1), preamble(), _io(), std::string("/types/tlm/seq/0")); - } - m_tlm = m__io->read_bits_int_be(14); - m_integrity_status = m__io->read_bits_int_be(1); - m_reserved = m__io->read_bits_int_be(1); -} - -gps_t::tlm_t::~tlm_t() { - _clean_up(); -} - -void gps_t::tlm_t::_clean_up() { -} - -gps_t::subframe_2_t::subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void gps_t::subframe_2_t::_read() { - m_iode = m__io->read_u1(); - m_c_rs = m__io->read_s2be(); - m_delta_n = m__io->read_s2be(); - m_m_0 = m__io->read_s4be(); - m_c_uc = m__io->read_s2be(); - m_e = m__io->read_s4be(); - m_c_us = m__io->read_s2be(); - m_sqrt_a = m__io->read_u4be(); - m_t_oe = m__io->read_u2be(); - m_fit_interval_flag = m__io->read_bits_int_be(1); - m_aoda = m__io->read_bits_int_be(5); - m_reserved = m__io->read_bits_int_be(2); -} - -gps_t::subframe_2_t::~subframe_2_t() { - _clean_up(); -} - -void gps_t::subframe_2_t::_clean_up() { -} diff --git a/system/ubloxd/generated/gps.h b/system/ubloxd/generated/gps.h deleted file mode 100644 index 9dfc5031f5..0000000000 --- a/system/ubloxd/generated/gps.h +++ /dev/null @@ -1,359 +0,0 @@ -#ifndef GPS_H_ -#define GPS_H_ - -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "kaitai/kaitaistruct.h" -#include - -#if KAITAI_STRUCT_VERSION < 9000L -#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" -#endif - -class gps_t : public kaitai::kstruct { - -public: - class subframe_1_t; - class subframe_3_t; - class subframe_4_t; - class how_t; - class tlm_t; - class subframe_2_t; - - gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0); - -private: - void _read(); - void _clean_up(); - -public: - ~gps_t(); - - class subframe_1_t : public kaitai::kstruct { - - public: - - subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~subframe_1_t(); - - private: - bool f_af_0; - int32_t m_af_0; - - public: - int32_t af_0(); - - private: - uint64_t m_week_no; - uint64_t m_code; - uint64_t m_sv_accuracy; - uint64_t m_sv_health; - uint64_t m_iodc_msb; - bool m_l2_p_data_flag; - uint64_t m_reserved1; - uint64_t m_reserved2; - uint64_t m_reserved3; - uint64_t m_reserved4; - int8_t m_t_gd; - uint8_t m_iodc_lsb; - uint16_t m_t_oc; - int8_t m_af_2; - int16_t m_af_1; - bool m_af_0_sign; - uint64_t m_af_0_value; - uint64_t m_reserved5; - gps_t* m__root; - gps_t* m__parent; - - public: - uint64_t week_no() const { return m_week_no; } - uint64_t code() const { return m_code; } - uint64_t sv_accuracy() const { return m_sv_accuracy; } - uint64_t sv_health() const { return m_sv_health; } - uint64_t iodc_msb() const { return m_iodc_msb; } - bool l2_p_data_flag() const { return m_l2_p_data_flag; } - uint64_t reserved1() const { return m_reserved1; } - uint64_t reserved2() const { return m_reserved2; } - uint64_t reserved3() const { return m_reserved3; } - uint64_t reserved4() const { return m_reserved4; } - int8_t t_gd() const { return m_t_gd; } - uint8_t iodc_lsb() const { return m_iodc_lsb; } - uint16_t t_oc() const { return m_t_oc; } - int8_t af_2() const { return m_af_2; } - int16_t af_1() const { return m_af_1; } - bool af_0_sign() const { return m_af_0_sign; } - uint64_t af_0_value() const { return m_af_0_value; } - uint64_t reserved5() const { return m_reserved5; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - - class subframe_3_t : public kaitai::kstruct { - - public: - - subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~subframe_3_t(); - - private: - bool f_omega_dot; - int32_t m_omega_dot; - - public: - int32_t omega_dot(); - - private: - bool f_idot; - int32_t m_idot; - - public: - int32_t idot(); - - private: - int16_t m_c_ic; - int32_t m_omega_0; - int16_t m_c_is; - int32_t m_i_0; - int16_t m_c_rc; - int32_t m_omega; - bool m_omega_dot_sign; - uint64_t m_omega_dot_value; - uint8_t m_iode; - bool m_idot_sign; - uint64_t m_idot_value; - uint64_t m_reserved; - gps_t* m__root; - gps_t* m__parent; - - public: - int16_t c_ic() const { return m_c_ic; } - int32_t omega_0() const { return m_omega_0; } - int16_t c_is() const { return m_c_is; } - int32_t i_0() const { return m_i_0; } - int16_t c_rc() const { return m_c_rc; } - int32_t omega() const { return m_omega; } - bool omega_dot_sign() const { return m_omega_dot_sign; } - uint64_t omega_dot_value() const { return m_omega_dot_value; } - uint8_t iode() const { return m_iode; } - bool idot_sign() const { return m_idot_sign; } - uint64_t idot_value() const { return m_idot_value; } - uint64_t reserved() const { return m_reserved; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - - class subframe_4_t : public kaitai::kstruct { - - public: - class ionosphere_data_t; - - subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~subframe_4_t(); - - class ionosphere_data_t : public kaitai::kstruct { - - public: - - ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~ionosphere_data_t(); - - private: - int8_t m_a0; - int8_t m_a1; - int8_t m_a2; - int8_t m_a3; - int8_t m_b0; - int8_t m_b1; - int8_t m_b2; - int8_t m_b3; - gps_t* m__root; - gps_t::subframe_4_t* m__parent; - - public: - int8_t a0() const { return m_a0; } - int8_t a1() const { return m_a1; } - int8_t a2() const { return m_a2; } - int8_t a3() const { return m_a3; } - int8_t b0() const { return m_b0; } - int8_t b1() const { return m_b1; } - int8_t b2() const { return m_b2; } - int8_t b3() const { return m_b3; } - gps_t* _root() const { return m__root; } - gps_t::subframe_4_t* _parent() const { return m__parent; } - }; - - private: - uint64_t m_data_id; - uint64_t m_page_id; - ionosphere_data_t* m_body; - bool n_body; - - public: - bool _is_null_body() { body(); return n_body; }; - - private: - gps_t* m__root; - gps_t* m__parent; - - public: - uint64_t data_id() const { return m_data_id; } - uint64_t page_id() const { return m_page_id; } - ionosphere_data_t* body() const { return m_body; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - - class how_t : public kaitai::kstruct { - - public: - - how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~how_t(); - - private: - uint64_t m_tow_count; - bool m_alert; - bool m_anti_spoof; - uint64_t m_subframe_id; - uint64_t m_reserved; - gps_t* m__root; - gps_t* m__parent; - - public: - uint64_t tow_count() const { return m_tow_count; } - bool alert() const { return m_alert; } - bool anti_spoof() const { return m_anti_spoof; } - uint64_t subframe_id() const { return m_subframe_id; } - uint64_t reserved() const { return m_reserved; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - - class tlm_t : public kaitai::kstruct { - - public: - - tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~tlm_t(); - - private: - std::string m_preamble; - uint64_t m_tlm; - bool m_integrity_status; - bool m_reserved; - gps_t* m__root; - gps_t* m__parent; - - public: - std::string preamble() const { return m_preamble; } - uint64_t tlm() const { return m_tlm; } - bool integrity_status() const { return m_integrity_status; } - bool reserved() const { return m_reserved; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - - class subframe_2_t : public kaitai::kstruct { - - public: - - subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~subframe_2_t(); - - private: - uint8_t m_iode; - int16_t m_c_rs; - int16_t m_delta_n; - int32_t m_m_0; - int16_t m_c_uc; - int32_t m_e; - int16_t m_c_us; - uint32_t m_sqrt_a; - uint16_t m_t_oe; - bool m_fit_interval_flag; - uint64_t m_aoda; - uint64_t m_reserved; - gps_t* m__root; - gps_t* m__parent; - - public: - uint8_t iode() const { return m_iode; } - int16_t c_rs() const { return m_c_rs; } - int16_t delta_n() const { return m_delta_n; } - int32_t m_0() const { return m_m_0; } - int16_t c_uc() const { return m_c_uc; } - int32_t e() const { return m_e; } - int16_t c_us() const { return m_c_us; } - uint32_t sqrt_a() const { return m_sqrt_a; } - uint16_t t_oe() const { return m_t_oe; } - bool fit_interval_flag() const { return m_fit_interval_flag; } - uint64_t aoda() const { return m_aoda; } - uint64_t reserved() const { return m_reserved; } - gps_t* _root() const { return m__root; } - gps_t* _parent() const { return m__parent; } - }; - -private: - tlm_t* m_tlm; - how_t* m_how; - kaitai::kstruct* m_body; - bool n_body; - -public: - bool _is_null_body() { body(); return n_body; }; - -private: - gps_t* m__root; - kaitai::kstruct* m__parent; - -public: - tlm_t* tlm() const { return m_tlm; } - how_t* how() const { return m_how; } - kaitai::kstruct* body() const { return m_body; } - gps_t* _root() const { return m__root; } - kaitai::kstruct* _parent() const { return m__parent; } -}; - -#endif // GPS_H_ diff --git a/system/ubloxd/generated/gps.py b/system/ubloxd/generated/gps.py new file mode 100644 index 0000000000..a999016f3e --- /dev/null +++ b/system/ubloxd/generated/gps.py @@ -0,0 +1,193 @@ +# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +import kaitaistruct +from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO + + +if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9): + raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__)) + +class Gps(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.tlm = Gps.Tlm(self._io, self, self._root) + self.how = Gps.How(self._io, self, self._root) + _on = self.how.subframe_id + if _on == 1: + self.body = Gps.Subframe1(self._io, self, self._root) + elif _on == 2: + self.body = Gps.Subframe2(self._io, self, self._root) + elif _on == 3: + self.body = Gps.Subframe3(self._io, self, self._root) + elif _on == 4: + self.body = Gps.Subframe4(self._io, self, self._root) + + class Subframe1(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.week_no = self._io.read_bits_int_be(10) + self.code = self._io.read_bits_int_be(2) + self.sv_accuracy = self._io.read_bits_int_be(4) + self.sv_health = self._io.read_bits_int_be(6) + self.iodc_msb = self._io.read_bits_int_be(2) + self.l2_p_data_flag = self._io.read_bits_int_be(1) != 0 + self.reserved1 = self._io.read_bits_int_be(23) + self.reserved2 = self._io.read_bits_int_be(24) + self.reserved3 = self._io.read_bits_int_be(24) + self.reserved4 = self._io.read_bits_int_be(16) + self._io.align_to_byte() + self.t_gd = self._io.read_s1() + self.iodc_lsb = self._io.read_u1() + self.t_oc = self._io.read_u2be() + self.af_2 = self._io.read_s1() + self.af_1 = self._io.read_s2be() + self.af_0_sign = self._io.read_bits_int_be(1) != 0 + self.af_0_value = self._io.read_bits_int_be(21) + self.reserved5 = self._io.read_bits_int_be(2) + + @property + def af_0(self): + if hasattr(self, '_m_af_0'): + return self._m_af_0 + + self._m_af_0 = ((self.af_0_value - (1 << 21)) if self.af_0_sign else self.af_0_value) + return getattr(self, '_m_af_0', None) + + + class Subframe3(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.c_ic = self._io.read_s2be() + self.omega_0 = self._io.read_s4be() + self.c_is = self._io.read_s2be() + self.i_0 = self._io.read_s4be() + self.c_rc = self._io.read_s2be() + self.omega = self._io.read_s4be() + self.omega_dot_sign = self._io.read_bits_int_be(1) != 0 + self.omega_dot_value = self._io.read_bits_int_be(23) + self._io.align_to_byte() + self.iode = self._io.read_u1() + self.idot_sign = self._io.read_bits_int_be(1) != 0 + self.idot_value = self._io.read_bits_int_be(13) + self.reserved = self._io.read_bits_int_be(2) + + @property + def omega_dot(self): + if hasattr(self, '_m_omega_dot'): + return self._m_omega_dot + + self._m_omega_dot = ((self.omega_dot_value - (1 << 23)) if self.omega_dot_sign else self.omega_dot_value) + return getattr(self, '_m_omega_dot', None) + + @property + def idot(self): + if hasattr(self, '_m_idot'): + return self._m_idot + + self._m_idot = ((self.idot_value - (1 << 13)) if self.idot_sign else self.idot_value) + return getattr(self, '_m_idot', None) + + + class Subframe4(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.data_id = self._io.read_bits_int_be(2) + self.page_id = self._io.read_bits_int_be(6) + self._io.align_to_byte() + _on = self.page_id + if _on == 56: + self.body = Gps.Subframe4.IonosphereData(self._io, self, self._root) + + class IonosphereData(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.a0 = self._io.read_s1() + self.a1 = self._io.read_s1() + self.a2 = self._io.read_s1() + self.a3 = self._io.read_s1() + self.b0 = self._io.read_s1() + self.b1 = self._io.read_s1() + self.b2 = self._io.read_s1() + self.b3 = self._io.read_s1() + + + + class How(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.tow_count = self._io.read_bits_int_be(17) + self.alert = self._io.read_bits_int_be(1) != 0 + self.anti_spoof = self._io.read_bits_int_be(1) != 0 + self.subframe_id = self._io.read_bits_int_be(3) + self.reserved = self._io.read_bits_int_be(2) + + + class Tlm(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.preamble = self._io.read_bytes(1) + if not self.preamble == b"\x8B": + raise kaitaistruct.ValidationNotEqualError(b"\x8B", self.preamble, self._io, u"/types/tlm/seq/0") + self.tlm = self._io.read_bits_int_be(14) + self.integrity_status = self._io.read_bits_int_be(1) != 0 + self.reserved = self._io.read_bits_int_be(1) != 0 + + + class Subframe2(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.iode = self._io.read_u1() + self.c_rs = self._io.read_s2be() + self.delta_n = self._io.read_s2be() + self.m_0 = self._io.read_s4be() + self.c_uc = self._io.read_s2be() + self.e = self._io.read_s4be() + self.c_us = self._io.read_s2be() + self.sqrt_a = self._io.read_u4be() + self.t_oe = self._io.read_u2be() + self.fit_interval_flag = self._io.read_bits_int_be(1) != 0 + self.aoda = self._io.read_bits_int_be(5) + self.reserved = self._io.read_bits_int_be(2) + + + diff --git a/system/ubloxd/generated/ubx.cpp b/system/ubloxd/generated/ubx.cpp deleted file mode 100644 index 81b82ccafc..0000000000 --- a/system/ubloxd/generated/ubx.cpp +++ /dev/null @@ -1,424 +0,0 @@ -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "ubx.h" -#include "kaitai/exceptions.h" - -ubx_t::ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = this; - f_checksum = false; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::_read() { - m_magic = m__io->read_bytes(2); - if (!(magic() == std::string("\xB5\x62", 2))) { - throw kaitai::validation_not_equal_error(std::string("\xB5\x62", 2), magic(), _io(), std::string("/seq/0")); - } - m_msg_type = m__io->read_u2be(); - m_length = m__io->read_u2le(); - n_body = true; - switch (msg_type()) { - case 2569: { - n_body = false; - m_body = new mon_hw_t(m__io, this, m__root); - break; - } - case 533: { - n_body = false; - m_body = new rxm_rawx_t(m__io, this, m__root); - break; - } - case 531: { - n_body = false; - m_body = new rxm_sfrbx_t(m__io, this, m__root); - break; - } - case 309: { - n_body = false; - m_body = new nav_sat_t(m__io, this, m__root); - break; - } - case 2571: { - n_body = false; - m_body = new mon_hw2_t(m__io, this, m__root); - break; - } - case 263: { - n_body = false; - m_body = new nav_pvt_t(m__io, this, m__root); - break; - } - } -} - -ubx_t::~ubx_t() { - _clean_up(); -} - -void ubx_t::_clean_up() { - if (!n_body) { - if (m_body) { - delete m_body; m_body = 0; - } - } - if (f_checksum) { - } -} - -ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - m_meas = 0; - m__raw_meas = 0; - m__io__raw_meas = 0; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::rxm_rawx_t::_read() { - m_rcv_tow = m__io->read_f8le(); - m_week = m__io->read_u2le(); - m_leap_s = m__io->read_s1(); - m_num_meas = m__io->read_u1(); - m_rec_stat = m__io->read_u1(); - m_reserved1 = m__io->read_bytes(3); - m__raw_meas = new std::vector(); - m__io__raw_meas = new std::vector(); - m_meas = new std::vector(); - const int l_meas = num_meas(); - for (int i = 0; i < l_meas; i++) { - m__raw_meas->push_back(m__io->read_bytes(32)); - kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1)); - m__io__raw_meas->push_back(io__raw_meas); - m_meas->push_back(new measurement_t(io__raw_meas, this, m__root)); - } -} - -ubx_t::rxm_rawx_t::~rxm_rawx_t() { - _clean_up(); -} - -void ubx_t::rxm_rawx_t::_clean_up() { - if (m__raw_meas) { - delete m__raw_meas; m__raw_meas = 0; - } - if (m__io__raw_meas) { - for (std::vector::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) { - delete *it; - } - delete m__io__raw_meas; m__io__raw_meas = 0; - } - if (m_meas) { - for (std::vector::iterator it = m_meas->begin(); it != m_meas->end(); ++it) { - delete *it; - } - delete m_meas; m_meas = 0; - } -} - -ubx_t::rxm_rawx_t::measurement_t::measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::rxm_rawx_t::measurement_t::_read() { - m_pr_mes = m__io->read_f8le(); - m_cp_mes = m__io->read_f8le(); - m_do_mes = m__io->read_f4le(); - m_gnss_id = static_cast(m__io->read_u1()); - m_sv_id = m__io->read_u1(); - m_reserved2 = m__io->read_bytes(1); - m_freq_id = m__io->read_u1(); - m_lock_time = m__io->read_u2le(); - m_cno = m__io->read_u1(); - m_pr_stdev = m__io->read_u1(); - m_cp_stdev = m__io->read_u1(); - m_do_stdev = m__io->read_u1(); - m_trk_stat = m__io->read_u1(); - m_reserved3 = m__io->read_bytes(1); -} - -ubx_t::rxm_rawx_t::measurement_t::~measurement_t() { - _clean_up(); -} - -void ubx_t::rxm_rawx_t::measurement_t::_clean_up() { -} - -ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - m_body = 0; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::rxm_sfrbx_t::_read() { - m_gnss_id = static_cast(m__io->read_u1()); - m_sv_id = m__io->read_u1(); - m_reserved1 = m__io->read_bytes(1); - m_freq_id = m__io->read_u1(); - m_num_words = m__io->read_u1(); - m_reserved2 = m__io->read_bytes(1); - m_version = m__io->read_u1(); - m_reserved3 = m__io->read_bytes(1); - m_body = new std::vector(); - const int l_body = num_words(); - for (int i = 0; i < l_body; i++) { - m_body->push_back(m__io->read_u4le()); - } -} - -ubx_t::rxm_sfrbx_t::~rxm_sfrbx_t() { - _clean_up(); -} - -void ubx_t::rxm_sfrbx_t::_clean_up() { - if (m_body) { - delete m_body; m_body = 0; - } -} - -ubx_t::nav_sat_t::nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - m_svs = 0; - m__raw_svs = 0; - m__io__raw_svs = 0; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::nav_sat_t::_read() { - m_itow = m__io->read_u4le(); - m_version = m__io->read_u1(); - m_num_svs = m__io->read_u1(); - m_reserved = m__io->read_bytes(2); - m__raw_svs = new std::vector(); - m__io__raw_svs = new std::vector(); - m_svs = new std::vector(); - const int l_svs = num_svs(); - for (int i = 0; i < l_svs; i++) { - m__raw_svs->push_back(m__io->read_bytes(12)); - kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1)); - m__io__raw_svs->push_back(io__raw_svs); - m_svs->push_back(new nav_t(io__raw_svs, this, m__root)); - } -} - -ubx_t::nav_sat_t::~nav_sat_t() { - _clean_up(); -} - -void ubx_t::nav_sat_t::_clean_up() { - if (m__raw_svs) { - delete m__raw_svs; m__raw_svs = 0; - } - if (m__io__raw_svs) { - for (std::vector::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) { - delete *it; - } - delete m__io__raw_svs; m__io__raw_svs = 0; - } - if (m_svs) { - for (std::vector::iterator it = m_svs->begin(); it != m_svs->end(); ++it) { - delete *it; - } - delete m_svs; m_svs = 0; - } -} - -ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::nav_sat_t::nav_t::_read() { - m_gnss_id = static_cast(m__io->read_u1()); - m_sv_id = m__io->read_u1(); - m_cno = m__io->read_u1(); - m_elev = m__io->read_s1(); - m_azim = m__io->read_s2le(); - m_pr_res = m__io->read_s2le(); - m_flags = m__io->read_u4le(); -} - -ubx_t::nav_sat_t::nav_t::~nav_t() { - _clean_up(); -} - -void ubx_t::nav_sat_t::nav_t::_clean_up() { -} - -ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::nav_pvt_t::_read() { - m_i_tow = m__io->read_u4le(); - m_year = m__io->read_u2le(); - m_month = m__io->read_u1(); - m_day = m__io->read_u1(); - m_hour = m__io->read_u1(); - m_min = m__io->read_u1(); - m_sec = m__io->read_u1(); - m_valid = m__io->read_u1(); - m_t_acc = m__io->read_u4le(); - m_nano = m__io->read_s4le(); - m_fix_type = m__io->read_u1(); - m_flags = m__io->read_u1(); - m_flags2 = m__io->read_u1(); - m_num_sv = m__io->read_u1(); - m_lon = m__io->read_s4le(); - m_lat = m__io->read_s4le(); - m_height = m__io->read_s4le(); - m_h_msl = m__io->read_s4le(); - m_h_acc = m__io->read_u4le(); - m_v_acc = m__io->read_u4le(); - m_vel_n = m__io->read_s4le(); - m_vel_e = m__io->read_s4le(); - m_vel_d = m__io->read_s4le(); - m_g_speed = m__io->read_s4le(); - m_head_mot = m__io->read_s4le(); - m_s_acc = m__io->read_s4le(); - m_head_acc = m__io->read_u4le(); - m_p_dop = m__io->read_u2le(); - m_flags3 = m__io->read_u1(); - m_reserved1 = m__io->read_bytes(5); - m_head_veh = m__io->read_s4le(); - m_mag_dec = m__io->read_s2le(); - m_mag_acc = m__io->read_u2le(); -} - -ubx_t::nav_pvt_t::~nav_pvt_t() { - _clean_up(); -} - -void ubx_t::nav_pvt_t::_clean_up() { -} - -ubx_t::mon_hw2_t::mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::mon_hw2_t::_read() { - m_ofs_i = m__io->read_s1(); - m_mag_i = m__io->read_u1(); - m_ofs_q = m__io->read_s1(); - m_mag_q = m__io->read_u1(); - m_cfg_source = static_cast(m__io->read_u1()); - m_reserved1 = m__io->read_bytes(3); - m_low_lev_cfg = m__io->read_u4le(); - m_reserved2 = m__io->read_bytes(8); - m_post_status = m__io->read_u4le(); - m_reserved3 = m__io->read_bytes(4); -} - -ubx_t::mon_hw2_t::~mon_hw2_t() { - _clean_up(); -} - -void ubx_t::mon_hw2_t::_clean_up() { -} - -ubx_t::mon_hw_t::mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { - m__parent = p__parent; - m__root = p__root; - - try { - _read(); - } catch(...) { - _clean_up(); - throw; - } -} - -void ubx_t::mon_hw_t::_read() { - m_pin_sel = m__io->read_u4le(); - m_pin_bank = m__io->read_u4le(); - m_pin_dir = m__io->read_u4le(); - m_pin_val = m__io->read_u4le(); - m_noise_per_ms = m__io->read_u2le(); - m_agc_cnt = m__io->read_u2le(); - m_a_status = static_cast(m__io->read_u1()); - m_a_power = static_cast(m__io->read_u1()); - m_flags = m__io->read_u1(); - m_reserved1 = m__io->read_bytes(1); - m_used_mask = m__io->read_u4le(); - m_vp = m__io->read_bytes(17); - m_jam_ind = m__io->read_u1(); - m_reserved2 = m__io->read_bytes(2); - m_pin_irq = m__io->read_u4le(); - m_pull_h = m__io->read_u4le(); - m_pull_l = m__io->read_u4le(); -} - -ubx_t::mon_hw_t::~mon_hw_t() { - _clean_up(); -} - -void ubx_t::mon_hw_t::_clean_up() { -} - -uint16_t ubx_t::checksum() { - if (f_checksum) - return m_checksum; - std::streampos _pos = m__io->pos(); - m__io->seek((length() + 6)); - m_checksum = m__io->read_u2le(); - m__io->seek(_pos); - f_checksum = true; - return m_checksum; -} diff --git a/system/ubloxd/generated/ubx.h b/system/ubloxd/generated/ubx.h deleted file mode 100644 index 022108489f..0000000000 --- a/system/ubloxd/generated/ubx.h +++ /dev/null @@ -1,484 +0,0 @@ -#ifndef UBX_H_ -#define UBX_H_ - -// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild - -#include "kaitai/kaitaistruct.h" -#include -#include - -#if KAITAI_STRUCT_VERSION < 9000L -#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required" -#endif - -class ubx_t : public kaitai::kstruct { - -public: - class rxm_rawx_t; - class rxm_sfrbx_t; - class nav_sat_t; - class nav_pvt_t; - class mon_hw2_t; - class mon_hw_t; - - enum gnss_type_t { - GNSS_TYPE_GPS = 0, - GNSS_TYPE_SBAS = 1, - GNSS_TYPE_GALILEO = 2, - GNSS_TYPE_BEIDOU = 3, - GNSS_TYPE_IMES = 4, - GNSS_TYPE_QZSS = 5, - GNSS_TYPE_GLONASS = 6 - }; - - ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0); - -private: - void _read(); - void _clean_up(); - -public: - ~ubx_t(); - - class rxm_rawx_t : public kaitai::kstruct { - - public: - class measurement_t; - - rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~rxm_rawx_t(); - - class measurement_t : public kaitai::kstruct { - - public: - - measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~measurement_t(); - - private: - double m_pr_mes; - double m_cp_mes; - float m_do_mes; - gnss_type_t m_gnss_id; - uint8_t m_sv_id; - std::string m_reserved2; - uint8_t m_freq_id; - uint16_t m_lock_time; - uint8_t m_cno; - uint8_t m_pr_stdev; - uint8_t m_cp_stdev; - uint8_t m_do_stdev; - uint8_t m_trk_stat; - std::string m_reserved3; - ubx_t* m__root; - ubx_t::rxm_rawx_t* m__parent; - - public: - double pr_mes() const { return m_pr_mes; } - double cp_mes() const { return m_cp_mes; } - float do_mes() const { return m_do_mes; } - gnss_type_t gnss_id() const { return m_gnss_id; } - uint8_t sv_id() const { return m_sv_id; } - std::string reserved2() const { return m_reserved2; } - uint8_t freq_id() const { return m_freq_id; } - uint16_t lock_time() const { return m_lock_time; } - uint8_t cno() const { return m_cno; } - uint8_t pr_stdev() const { return m_pr_stdev; } - uint8_t cp_stdev() const { return m_cp_stdev; } - uint8_t do_stdev() const { return m_do_stdev; } - uint8_t trk_stat() const { return m_trk_stat; } - std::string reserved3() const { return m_reserved3; } - ubx_t* _root() const { return m__root; } - ubx_t::rxm_rawx_t* _parent() const { return m__parent; } - }; - - private: - double m_rcv_tow; - uint16_t m_week; - int8_t m_leap_s; - uint8_t m_num_meas; - uint8_t m_rec_stat; - std::string m_reserved1; - std::vector* m_meas; - ubx_t* m__root; - ubx_t* m__parent; - std::vector* m__raw_meas; - std::vector* m__io__raw_meas; - - public: - double rcv_tow() const { return m_rcv_tow; } - uint16_t week() const { return m_week; } - int8_t leap_s() const { return m_leap_s; } - uint8_t num_meas() const { return m_num_meas; } - uint8_t rec_stat() const { return m_rec_stat; } - std::string reserved1() const { return m_reserved1; } - std::vector* meas() const { return m_meas; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - std::vector* _raw_meas() const { return m__raw_meas; } - std::vector* _io__raw_meas() const { return m__io__raw_meas; } - }; - - class rxm_sfrbx_t : public kaitai::kstruct { - - public: - - rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~rxm_sfrbx_t(); - - private: - gnss_type_t m_gnss_id; - uint8_t m_sv_id; - std::string m_reserved1; - uint8_t m_freq_id; - uint8_t m_num_words; - std::string m_reserved2; - uint8_t m_version; - std::string m_reserved3; - std::vector* m_body; - ubx_t* m__root; - ubx_t* m__parent; - - public: - gnss_type_t gnss_id() const { return m_gnss_id; } - uint8_t sv_id() const { return m_sv_id; } - std::string reserved1() const { return m_reserved1; } - uint8_t freq_id() const { return m_freq_id; } - uint8_t num_words() const { return m_num_words; } - std::string reserved2() const { return m_reserved2; } - uint8_t version() const { return m_version; } - std::string reserved3() const { return m_reserved3; } - std::vector* body() const { return m_body; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - }; - - class nav_sat_t : public kaitai::kstruct { - - public: - class nav_t; - - nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~nav_sat_t(); - - class nav_t : public kaitai::kstruct { - - public: - - nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~nav_t(); - - private: - gnss_type_t m_gnss_id; - uint8_t m_sv_id; - uint8_t m_cno; - int8_t m_elev; - int16_t m_azim; - int16_t m_pr_res; - uint32_t m_flags; - ubx_t* m__root; - ubx_t::nav_sat_t* m__parent; - - public: - gnss_type_t gnss_id() const { return m_gnss_id; } - uint8_t sv_id() const { return m_sv_id; } - uint8_t cno() const { return m_cno; } - int8_t elev() const { return m_elev; } - int16_t azim() const { return m_azim; } - int16_t pr_res() const { return m_pr_res; } - uint32_t flags() const { return m_flags; } - ubx_t* _root() const { return m__root; } - ubx_t::nav_sat_t* _parent() const { return m__parent; } - }; - - private: - uint32_t m_itow; - uint8_t m_version; - uint8_t m_num_svs; - std::string m_reserved; - std::vector* m_svs; - ubx_t* m__root; - ubx_t* m__parent; - std::vector* m__raw_svs; - std::vector* m__io__raw_svs; - - public: - uint32_t itow() const { return m_itow; } - uint8_t version() const { return m_version; } - uint8_t num_svs() const { return m_num_svs; } - std::string reserved() const { return m_reserved; } - std::vector* svs() const { return m_svs; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - std::vector* _raw_svs() const { return m__raw_svs; } - std::vector* _io__raw_svs() const { return m__io__raw_svs; } - }; - - class nav_pvt_t : public kaitai::kstruct { - - public: - - nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~nav_pvt_t(); - - private: - uint32_t m_i_tow; - uint16_t m_year; - uint8_t m_month; - uint8_t m_day; - uint8_t m_hour; - uint8_t m_min; - uint8_t m_sec; - uint8_t m_valid; - uint32_t m_t_acc; - int32_t m_nano; - uint8_t m_fix_type; - uint8_t m_flags; - uint8_t m_flags2; - uint8_t m_num_sv; - int32_t m_lon; - int32_t m_lat; - int32_t m_height; - int32_t m_h_msl; - uint32_t m_h_acc; - uint32_t m_v_acc; - int32_t m_vel_n; - int32_t m_vel_e; - int32_t m_vel_d; - int32_t m_g_speed; - int32_t m_head_mot; - int32_t m_s_acc; - uint32_t m_head_acc; - uint16_t m_p_dop; - uint8_t m_flags3; - std::string m_reserved1; - int32_t m_head_veh; - int16_t m_mag_dec; - uint16_t m_mag_acc; - ubx_t* m__root; - ubx_t* m__parent; - - public: - uint32_t i_tow() const { return m_i_tow; } - uint16_t year() const { return m_year; } - uint8_t month() const { return m_month; } - uint8_t day() const { return m_day; } - uint8_t hour() const { return m_hour; } - uint8_t min() const { return m_min; } - uint8_t sec() const { return m_sec; } - uint8_t valid() const { return m_valid; } - uint32_t t_acc() const { return m_t_acc; } - int32_t nano() const { return m_nano; } - uint8_t fix_type() const { return m_fix_type; } - uint8_t flags() const { return m_flags; } - uint8_t flags2() const { return m_flags2; } - uint8_t num_sv() const { return m_num_sv; } - int32_t lon() const { return m_lon; } - int32_t lat() const { return m_lat; } - int32_t height() const { return m_height; } - int32_t h_msl() const { return m_h_msl; } - uint32_t h_acc() const { return m_h_acc; } - uint32_t v_acc() const { return m_v_acc; } - int32_t vel_n() const { return m_vel_n; } - int32_t vel_e() const { return m_vel_e; } - int32_t vel_d() const { return m_vel_d; } - int32_t g_speed() const { return m_g_speed; } - int32_t head_mot() const { return m_head_mot; } - int32_t s_acc() const { return m_s_acc; } - uint32_t head_acc() const { return m_head_acc; } - uint16_t p_dop() const { return m_p_dop; } - uint8_t flags3() const { return m_flags3; } - std::string reserved1() const { return m_reserved1; } - int32_t head_veh() const { return m_head_veh; } - int16_t mag_dec() const { return m_mag_dec; } - uint16_t mag_acc() const { return m_mag_acc; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - }; - - class mon_hw2_t : public kaitai::kstruct { - - public: - - enum config_source_t { - CONFIG_SOURCE_FLASH = 102, - CONFIG_SOURCE_OTP = 111, - CONFIG_SOURCE_CONFIG_PINS = 112, - CONFIG_SOURCE_ROM = 113 - }; - - mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~mon_hw2_t(); - - private: - int8_t m_ofs_i; - uint8_t m_mag_i; - int8_t m_ofs_q; - uint8_t m_mag_q; - config_source_t m_cfg_source; - std::string m_reserved1; - uint32_t m_low_lev_cfg; - std::string m_reserved2; - uint32_t m_post_status; - std::string m_reserved3; - ubx_t* m__root; - ubx_t* m__parent; - - public: - int8_t ofs_i() const { return m_ofs_i; } - uint8_t mag_i() const { return m_mag_i; } - int8_t ofs_q() const { return m_ofs_q; } - uint8_t mag_q() const { return m_mag_q; } - config_source_t cfg_source() const { return m_cfg_source; } - std::string reserved1() const { return m_reserved1; } - uint32_t low_lev_cfg() const { return m_low_lev_cfg; } - std::string reserved2() const { return m_reserved2; } - uint32_t post_status() const { return m_post_status; } - std::string reserved3() const { return m_reserved3; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - }; - - class mon_hw_t : public kaitai::kstruct { - - public: - - enum antenna_status_t { - ANTENNA_STATUS_INIT = 0, - ANTENNA_STATUS_DONTKNOW = 1, - ANTENNA_STATUS_OK = 2, - ANTENNA_STATUS_SHORT = 3, - ANTENNA_STATUS_OPEN = 4 - }; - - enum antenna_power_t { - ANTENNA_POWER_FALSE = 0, - ANTENNA_POWER_TRUE = 1, - ANTENNA_POWER_DONTKNOW = 2 - }; - - mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); - - private: - void _read(); - void _clean_up(); - - public: - ~mon_hw_t(); - - private: - uint32_t m_pin_sel; - uint32_t m_pin_bank; - uint32_t m_pin_dir; - uint32_t m_pin_val; - uint16_t m_noise_per_ms; - uint16_t m_agc_cnt; - antenna_status_t m_a_status; - antenna_power_t m_a_power; - uint8_t m_flags; - std::string m_reserved1; - uint32_t m_used_mask; - std::string m_vp; - uint8_t m_jam_ind; - std::string m_reserved2; - uint32_t m_pin_irq; - uint32_t m_pull_h; - uint32_t m_pull_l; - ubx_t* m__root; - ubx_t* m__parent; - - public: - uint32_t pin_sel() const { return m_pin_sel; } - uint32_t pin_bank() const { return m_pin_bank; } - uint32_t pin_dir() const { return m_pin_dir; } - uint32_t pin_val() const { return m_pin_val; } - uint16_t noise_per_ms() const { return m_noise_per_ms; } - uint16_t agc_cnt() const { return m_agc_cnt; } - antenna_status_t a_status() const { return m_a_status; } - antenna_power_t a_power() const { return m_a_power; } - uint8_t flags() const { return m_flags; } - std::string reserved1() const { return m_reserved1; } - uint32_t used_mask() const { return m_used_mask; } - std::string vp() const { return m_vp; } - uint8_t jam_ind() const { return m_jam_ind; } - std::string reserved2() const { return m_reserved2; } - uint32_t pin_irq() const { return m_pin_irq; } - uint32_t pull_h() const { return m_pull_h; } - uint32_t pull_l() const { return m_pull_l; } - ubx_t* _root() const { return m__root; } - ubx_t* _parent() const { return m__parent; } - }; - -private: - bool f_checksum; - uint16_t m_checksum; - -public: - uint16_t checksum(); - -private: - std::string m_magic; - uint16_t m_msg_type; - uint16_t m_length; - kaitai::kstruct* m_body; - bool n_body; - -public: - bool _is_null_body() { body(); return n_body; }; - -private: - ubx_t* m__root; - kaitai::kstruct* m__parent; - -public: - std::string magic() const { return m_magic; } - uint16_t msg_type() const { return m_msg_type; } - uint16_t length() const { return m_length; } - kaitai::kstruct* body() const { return m_body; } - ubx_t* _root() const { return m__root; } - kaitai::kstruct* _parent() const { return m__parent; } -}; - -#endif // UBX_H_ diff --git a/system/ubloxd/generated/ubx.py b/system/ubloxd/generated/ubx.py new file mode 100644 index 0000000000..9946584388 --- /dev/null +++ b/system/ubloxd/generated/ubx.py @@ -0,0 +1,273 @@ +# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild + +import kaitaistruct +from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO +from enum import Enum + + +if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9): + raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__)) + +class Ubx(KaitaiStruct): + + class GnssType(Enum): + gps = 0 + sbas = 1 + galileo = 2 + beidou = 3 + imes = 4 + qzss = 5 + glonass = 6 + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.magic = self._io.read_bytes(2) + if not self.magic == b"\xB5\x62": + raise kaitaistruct.ValidationNotEqualError(b"\xB5\x62", self.magic, self._io, u"/seq/0") + self.msg_type = self._io.read_u2be() + self.length = self._io.read_u2le() + _on = self.msg_type + if _on == 2569: + self.body = Ubx.MonHw(self._io, self, self._root) + elif _on == 533: + self.body = Ubx.RxmRawx(self._io, self, self._root) + elif _on == 531: + self.body = Ubx.RxmSfrbx(self._io, self, self._root) + elif _on == 309: + self.body = Ubx.NavSat(self._io, self, self._root) + elif _on == 2571: + self.body = Ubx.MonHw2(self._io, self, self._root) + elif _on == 263: + self.body = Ubx.NavPvt(self._io, self, self._root) + + class RxmRawx(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.rcv_tow = self._io.read_f8le() + self.week = self._io.read_u2le() + self.leap_s = self._io.read_s1() + self.num_meas = self._io.read_u1() + self.rec_stat = self._io.read_u1() + self.reserved1 = self._io.read_bytes(3) + self._raw_meas = [] + self.meas = [] + for i in range(self.num_meas): + self._raw_meas.append(self._io.read_bytes(32)) + _io__raw_meas = KaitaiStream(BytesIO(self._raw_meas[i])) + self.meas.append(Ubx.RxmRawx.Measurement(_io__raw_meas, self, self._root)) + + + class Measurement(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.pr_mes = self._io.read_f8le() + self.cp_mes = self._io.read_f8le() + self.do_mes = self._io.read_f4le() + self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1()) + self.sv_id = self._io.read_u1() + self.reserved2 = self._io.read_bytes(1) + self.freq_id = self._io.read_u1() + self.lock_time = self._io.read_u2le() + self.cno = self._io.read_u1() + self.pr_stdev = self._io.read_u1() + self.cp_stdev = self._io.read_u1() + self.do_stdev = self._io.read_u1() + self.trk_stat = self._io.read_u1() + self.reserved3 = self._io.read_bytes(1) + + + + class RxmSfrbx(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1()) + self.sv_id = self._io.read_u1() + self.reserved1 = self._io.read_bytes(1) + self.freq_id = self._io.read_u1() + self.num_words = self._io.read_u1() + self.reserved2 = self._io.read_bytes(1) + self.version = self._io.read_u1() + self.reserved3 = self._io.read_bytes(1) + self.body = [] + for i in range(self.num_words): + self.body.append(self._io.read_u4le()) + + + + class NavSat(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.itow = self._io.read_u4le() + self.version = self._io.read_u1() + self.num_svs = self._io.read_u1() + self.reserved = self._io.read_bytes(2) + self._raw_svs = [] + self.svs = [] + for i in range(self.num_svs): + self._raw_svs.append(self._io.read_bytes(12)) + _io__raw_svs = KaitaiStream(BytesIO(self._raw_svs[i])) + self.svs.append(Ubx.NavSat.Nav(_io__raw_svs, self, self._root)) + + + class Nav(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1()) + self.sv_id = self._io.read_u1() + self.cno = self._io.read_u1() + self.elev = self._io.read_s1() + self.azim = self._io.read_s2le() + self.pr_res = self._io.read_s2le() + self.flags = self._io.read_u4le() + + + + class NavPvt(KaitaiStruct): + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.i_tow = self._io.read_u4le() + self.year = self._io.read_u2le() + self.month = self._io.read_u1() + self.day = self._io.read_u1() + self.hour = self._io.read_u1() + self.min = self._io.read_u1() + self.sec = self._io.read_u1() + self.valid = self._io.read_u1() + self.t_acc = self._io.read_u4le() + self.nano = self._io.read_s4le() + self.fix_type = self._io.read_u1() + self.flags = self._io.read_u1() + self.flags2 = self._io.read_u1() + self.num_sv = self._io.read_u1() + self.lon = self._io.read_s4le() + self.lat = self._io.read_s4le() + self.height = self._io.read_s4le() + self.h_msl = self._io.read_s4le() + self.h_acc = self._io.read_u4le() + self.v_acc = self._io.read_u4le() + self.vel_n = self._io.read_s4le() + self.vel_e = self._io.read_s4le() + self.vel_d = self._io.read_s4le() + self.g_speed = self._io.read_s4le() + self.head_mot = self._io.read_s4le() + self.s_acc = self._io.read_s4le() + self.head_acc = self._io.read_u4le() + self.p_dop = self._io.read_u2le() + self.flags3 = self._io.read_u1() + self.reserved1 = self._io.read_bytes(5) + self.head_veh = self._io.read_s4le() + self.mag_dec = self._io.read_s2le() + self.mag_acc = self._io.read_u2le() + + + class MonHw2(KaitaiStruct): + + class ConfigSource(Enum): + flash = 102 + otp = 111 + config_pins = 112 + rom = 113 + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.ofs_i = self._io.read_s1() + self.mag_i = self._io.read_u1() + self.ofs_q = self._io.read_s1() + self.mag_q = self._io.read_u1() + self.cfg_source = KaitaiStream.resolve_enum(Ubx.MonHw2.ConfigSource, self._io.read_u1()) + self.reserved1 = self._io.read_bytes(3) + self.low_lev_cfg = self._io.read_u4le() + self.reserved2 = self._io.read_bytes(8) + self.post_status = self._io.read_u4le() + self.reserved3 = self._io.read_bytes(4) + + + class MonHw(KaitaiStruct): + + class AntennaStatus(Enum): + init = 0 + dontknow = 1 + ok = 2 + short = 3 + open = 4 + + class AntennaPower(Enum): + false = 0 + true = 1 + dontknow = 2 + def __init__(self, _io, _parent=None, _root=None): + self._io = _io + self._parent = _parent + self._root = _root if _root else self + self._read() + + def _read(self): + self.pin_sel = self._io.read_u4le() + self.pin_bank = self._io.read_u4le() + self.pin_dir = self._io.read_u4le() + self.pin_val = self._io.read_u4le() + self.noise_per_ms = self._io.read_u2le() + self.agc_cnt = self._io.read_u2le() + self.a_status = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaStatus, self._io.read_u1()) + self.a_power = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaPower, self._io.read_u1()) + self.flags = self._io.read_u1() + self.reserved1 = self._io.read_bytes(1) + self.used_mask = self._io.read_u4le() + self.vp = self._io.read_bytes(17) + self.jam_ind = self._io.read_u1() + self.reserved2 = self._io.read_bytes(2) + self.pin_irq = self._io.read_u4le() + self.pull_h = self._io.read_u4le() + self.pull_l = self._io.read_u4le() + + + @property + def checksum(self): + if hasattr(self, '_m_checksum'): + return self._m_checksum + + _pos = self._io.pos() + self._io.seek((self.length + 6)) + self._m_checksum = self._io.read_u2le() + self._io.seek(_pos) + return getattr(self, '_m_checksum', None) + + diff --git a/system/ubloxd/glonass_fix.patch b/system/ubloxd/glonass_fix.patch deleted file mode 100644 index 7eb973a348..0000000000 --- a/system/ubloxd/glonass_fix.patch +++ /dev/null @@ -1,13 +0,0 @@ -diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp -index 5b17bc327..b5c6aa610 100644 ---- a/system/ubloxd/generated/glonass.cpp -+++ b/system/ubloxd/generated/glonass.cpp -@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass - void glonass_t::_read() { - m_idle_chip = m__io->read_bits_int_be(1); - m_string_number = m__io->read_bits_int_be(4); -- m__io->align_to_byte(); -+ //m__io->align_to_byte(); - switch (string_number()) { - case 4: { - m_data = new string_4_t(m__io, this, m__root); diff --git a/system/ubloxd/tests/print_gps_stats.py b/system/ubloxd/tests/print_gps_stats.py deleted file mode 100755 index 8d190f9ec1..0000000000 --- a/system/ubloxd/tests/print_gps_stats.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python3 -import time -import cereal.messaging as messaging - -if __name__ == "__main__": - sm = messaging.SubMaster(['ubloxGnss', 'gpsLocationExternal']) - - while 1: - ug = sm['ubloxGnss'] - gle = sm['gpsLocationExternal'] - - try: - cnos = [] - for m in ug.measurementReport.measurements: - cnos.append(m.cno) - print(f"Sats: {ug.measurementReport.numMeas} Accuracy: {gle.horizontalAccuracy:.2f} m cnos", sorted(cnos)) - except Exception: - pass - sm.update() - time.sleep(0.1) diff --git a/system/ubloxd/tests/test_glonass_kaitai.cc b/system/ubloxd/tests/test_glonass_kaitai.cc deleted file mode 100644 index 96f43742b4..0000000000 --- a/system/ubloxd/tests/test_glonass_kaitai.cc +++ /dev/null @@ -1,360 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "catch2/catch.hpp" -#include "system/ubloxd/generated/glonass.h" - -typedef std::vector> string_data; - -#define IDLE_CHIP_IDX 0 -#define STRING_NUMBER_IDX 1 -// string data 1-5 -#define HC_IDX 0 -#define PAD1_IDX 1 -#define SUPERFRAME_IDX 2 -#define PAD2_IDX 3 -#define FRAME_IDX 4 - -// Indexes for string number 1 -#define ST1_NU_IDX 2 -#define ST1_P1_IDX 3 -#define ST1_T_K_IDX 4 -#define ST1_X_VEL_S_IDX 5 -#define ST1_X_VEL_V_IDX 6 -#define ST1_X_ACCEL_S_IDX 7 -#define ST1_X_ACCEL_V_IDX 8 -#define ST1_X_S_IDX 9 -#define ST1_X_V_IDX 10 -#define ST1_HC_OFF 11 - -// Indexes for string number 2 -#define ST2_BN_IDX 2 -#define ST2_P2_IDX 3 -#define ST2_TB_IDX 4 -#define ST2_NU_IDX 5 -#define ST2_Y_VEL_S_IDX 6 -#define ST2_Y_VEL_V_IDX 7 -#define ST2_Y_ACCEL_S_IDX 8 -#define ST2_Y_ACCEL_V_IDX 9 -#define ST2_Y_S_IDX 10 -#define ST2_Y_V_IDX 11 -#define ST2_HC_OFF 12 - -// Indexes for string number 3 -#define ST3_P3_IDX 2 -#define ST3_GAMMA_N_S_IDX 3 -#define ST3_GAMMA_N_V_IDX 4 -#define ST3_NU_1_IDX 5 -#define ST3_P_IDX 6 -#define ST3_L_N_IDX 7 -#define ST3_Z_VEL_S_IDX 8 -#define ST3_Z_VEL_V_IDX 9 -#define ST3_Z_ACCEL_S_IDX 10 -#define ST3_Z_ACCEL_V_IDX 11 -#define ST3_Z_S_IDX 12 -#define ST3_Z_V_IDX 13 -#define ST3_HC_OFF 14 - -// Indexes for string number 4 -#define ST4_TAU_N_S_IDX 2 -#define ST4_TAU_N_V_IDX 3 -#define ST4_DELTA_TAU_N_S_IDX 4 -#define ST4_DELTA_TAU_N_V_IDX 5 -#define ST4_E_N_IDX 6 -#define ST4_NU_1_IDX 7 -#define ST4_P4_IDX 8 -#define ST4_F_T_IDX 9 -#define ST4_NU_2_IDX 10 -#define ST4_N_T_IDX 11 -#define ST4_N_IDX 12 -#define ST4_M_IDX 13 -#define ST4_HC_OFF 14 - -// Indexes for string number 5 -#define ST5_N_A_IDX 2 -#define ST5_TAU_C_IDX 3 -#define ST5_NU_IDX 4 -#define ST5_N_4_IDX 5 -#define ST5_TAU_GPS_IDX 6 -#define ST5_L_N_IDX 7 -#define ST5_HC_OFF 8 - -// Indexes for non immediate -#define ST6_DATA_1_IDX 2 -#define ST6_DATA_2_IDX 3 -#define ST6_HC_OFF 4 - - -std::string generate_inp_data(string_data& data) { - std::string inp_data = ""; - for (auto& [b, v] : data) { - std::string tmp = std::bitset<64>(v).to_string(); - inp_data += tmp.substr(64-b, b); - } - assert(inp_data.size() == 128); - - std::string string_data; - string_data.reserve(16); - for (int i = 0; i < 128; i+=8) { - std::string substr = inp_data.substr(i, 8); - string_data.push_back((uint8_t)std::stoi(substr.c_str(), 0, 2)); - } - - return string_data; -} - -string_data generate_string_data(uint8_t string_number) { - - srand((unsigned)time(0)); - string_data data; // - data.push_back({1, 0}); // idle chip - data.push_back({4, string_number}); // string number - - if (string_number == 1) { - data.push_back({2, 3}); // not_used - data.push_back({2, 1}); // p1 - data.push_back({12, 113}); // t_k - data.push_back({1, rand() & 1}); // x_vel_sign - data.push_back({23, 7122}); // x_vel_value - data.push_back({1, rand() & 1}); // x_accel_sign - data.push_back({4, 3}); // x_accel_value - data.push_back({1, rand() & 1}); // x_sign - data.push_back({26, 33554431}); // x_value - } else if (string_number == 2) { - data.push_back({3, 3}); // b_n - data.push_back({1, 1}); // p2 - data.push_back({7, 123}); // t_b - data.push_back({5, 31}); // not_used - data.push_back({1, rand() & 1}); // y_vel_sign - data.push_back({23, 7422}); // y_vel_value - data.push_back({1, rand() & 1}); // y_accel_sign - data.push_back({4, 3}); // y_accel_value - data.push_back({1, rand() & 1}); // y_sign - data.push_back({26, 67108863}); // y_value - } else if (string_number == 3) { - data.push_back({1, 0}); // p3 - data.push_back({1, 1}); // gamma_n_sign - data.push_back({10, 123}); // gamma_n_value - data.push_back({1, 0}); // not_used - data.push_back({2, 2}); // p - data.push_back({1, 1}); // l_n - data.push_back({1, rand() & 1}); // z_vel_sign - data.push_back({23, 1337}); // z_vel_value - data.push_back({1, rand() & 1}); // z_accel_sign - data.push_back({4, 9}); // z_accel_value - data.push_back({1, rand() & 1}); // z_sign - data.push_back({26, 100023}); // z_value - } else if (string_number == 4) { - data.push_back({1, rand() & 1}); // tau_n_sign - data.push_back({21, 197152}); // tau_n_value - data.push_back({1, rand() & 1}); // delta_tau_n_sign - data.push_back({4, 4}); // delta_tau_n_value - data.push_back({5, 0}); // e_n - data.push_back({14, 2}); // not_used_1 - data.push_back({1, 1}); // p4 - data.push_back({4, 9}); // f_t - data.push_back({3, 3}); // not_used_2 - data.push_back({11, 2047}); // n_t - data.push_back({5, 2}); // n - data.push_back({2, 1}); // m - } else if (string_number == 5) { - data.push_back({11, 2047}); // n_a - data.push_back({32, 4294767295}); // tau_c - data.push_back({1, 0}); // not_used_1 - data.push_back({5, 2}); // n_4 - data.push_back({22, 4114304}); // tau_gps - data.push_back({1, 0}); // l_n - } else { // non-immediate data is not parsed - data.push_back({64, rand()}); // data_1 - data.push_back({8, 6}); // data_2 - } - - data.push_back({8, rand() & 0xFF}); // hamming code - data.push_back({11, rand() & 0x7FF}); // pad - data.push_back({16, rand() & 0xFFFF}); // superframe - data.push_back({8, rand() & 0xFF}); // pad - data.push_back({8, rand() & 0xFF}); // frame - return data; -} - -TEST_CASE("parse_string_number_1"){ - string_data data = generate_string_data(1); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); - - kaitai::kstream str1(inp_data); - glonass_t str1_data(&str1); - glonass_t::string_1_t* s1 = static_cast(str1_data.data()); - - REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); - REQUIRE(s1->p1() == data[ST1_P1_IDX].second); - REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); - - int mul = s1->x_vel_sign() ? (-1) : 1; - REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); - mul = s1->x_accel_sign() ? (-1) : 1; - REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); - mul = s1->x_sign() ? (-1) : 1; - REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); -} - -TEST_CASE("parse_string_number_2"){ - string_data data = generate_string_data(2); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); - - kaitai::kstream str2(inp_data); - glonass_t str2_data(&str2); - glonass_t::string_2_t* s2 = static_cast(str2_data.data()); - - REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); - REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); - REQUIRE(s2->p2() == data[ST2_P2_IDX].second); - REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); - int mul = s2->y_vel_sign() ? (-1) : 1; - REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); - mul = s2->y_accel_sign() ? (-1) : 1; - REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); - mul = s2->y_sign() ? (-1) : 1; - REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); -} - -TEST_CASE("parse_string_number_3"){ - string_data data = generate_string_data(3); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); - - kaitai::kstream str3(inp_data); - glonass_t str3_data(&str3); - glonass_t::string_3_t* s3 = static_cast(str3_data.data()); - - REQUIRE(s3->p3() == data[ST3_P3_IDX].second); - int mul = s3->gamma_n_sign() ? (-1) : 1; - REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); - REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); - REQUIRE(s3->p() == data[ST3_P_IDX].second); - REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); - mul = s3->z_vel_sign() ? (-1) : 1; - REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); - mul = s3->z_accel_sign() ? (-1) : 1; - REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); - mul = s3->z_sign() ? (-1) : 1; - REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); -} - -TEST_CASE("parse_string_number_4"){ - string_data data = generate_string_data(4); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); - - kaitai::kstream str4(inp_data); - glonass_t str4_data(&str4); - glonass_t::string_4_t* s4 = static_cast(str4_data.data()); - - int mul = s4->tau_n_sign() ? (-1) : 1; - REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); - mul = s4->delta_tau_n_sign() ? (-1) : 1; - REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); - REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); - REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); - REQUIRE(s4->p4() == data[ST4_P4_IDX].second); - REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); - REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); - REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); - REQUIRE(s4->n() == data[ST4_N_IDX].second); - REQUIRE(s4->m() == data[ST4_M_IDX].second); -} - -TEST_CASE("parse_string_number_5"){ - string_data data = generate_string_data(5); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); - - kaitai::kstream str5(inp_data); - glonass_t str5_data(&str5); - glonass_t::string_5_t* s5 = static_cast(str5_data.data()); - - REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); - REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); - REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); - REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); - REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); - REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); -} - -TEST_CASE("parse_string_number_NI"){ - string_data data = generate_string_data((rand() % 10) + 6); - std::string inp_data = generate_inp_data(data); - - kaitai::kstream stream(inp_data); - glonass_t gl_string(&stream); - - REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); - REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); - REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); - REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); - REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); - REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); - REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); - - kaitai::kstream strni(inp_data); - glonass_t strni_data(&strni); - glonass_t::string_non_immediate_t* sni = static_cast(strni_data.data()); - - REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); - REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); -} diff --git a/system/ubloxd/tests/test_glonass_runner.cc b/system/ubloxd/tests/test_glonass_runner.cc deleted file mode 100644 index 62bf7476a1..0000000000 --- a/system/ubloxd/tests/test_glonass_runner.cc +++ /dev/null @@ -1,2 +0,0 @@ -#define CATCH_CONFIG_MAIN -#include "catch2/catch.hpp" diff --git a/system/ubloxd/tests/ubloxd.py b/system/ubloxd/tests/ubloxd.py deleted file mode 100755 index c17387114f..0000000000 --- a/system/ubloxd/tests/ubloxd.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python3 -# type: ignore - -from openpilot.selfdrive.locationd.test import ublox -import struct - -baudrate = 460800 -rate = 100 # send new data every 100ms - - -def configure_ublox(dev): - # configure ports and solution parameters and rate - dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB - dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC - - payload = struct.pack(' - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/swaglog.h" - -const double gpsPi = 3.1415926535898; -#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) - -inline static bool bit_to_bool(uint8_t val, int shifts) { - return (bool)(val & (1 << shifts)); -} - -inline int UbloxMsgParser::needed_bytes() { - // Msg header incomplete? - if (bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE) - return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; - uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE; - // too much data - if (needed < (uint16_t)bytes_in_parse_buf) - return -1; - return needed - (uint16_t)bytes_in_parse_buf; -} - -inline bool UbloxMsgParser::valid_cheksum() { - uint8_t ck_a = 0, ck_b = 0; - for (int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE; i++) { - ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; - ck_b = (ck_b + ck_a) & 0xFF; - } - if (ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) { - LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]); - return false; - } - if (ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) { - LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]); - return false; - } - return true; -} - -inline bool UbloxMsgParser::valid() { - return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE && - needed_bytes() == 0 && valid_cheksum(); -} - -inline bool UbloxMsgParser::valid_so_far() { - if (bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) { - return false; - } - if (bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) { - return false; - } - if (needed_bytes() == 0 && !valid()) { - return false; - } - return true; -} - -bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { - last_log_time = log_time; - int needed = needed_bytes(); - if (needed > 0) { - bytes_consumed = std::min((uint32_t)needed, incoming_data_len); - // Add data to buffer - memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed); - bytes_in_parse_buf += bytes_consumed; - } else { - bytes_consumed = incoming_data_len; - } - - // Validate msg format, detect invalid header and invalid checksum. - while (!valid_so_far() && bytes_in_parse_buf != 0) { - // Corrupted msg, drop a byte. - bytes_in_parse_buf -= 1; - if (bytes_in_parse_buf > 0) - memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); - } - - // There is redundant data at the end of buffer, reset the buffer. - if (needed_bytes() == -1) { - bytes_in_parse_buf = 0; - } - return valid(); -} - - -std::pair> UbloxMsgParser::gen_msg() { - std::string dat = data(); - kaitai::kstream stream(dat); - - ubx_t ubx_message(&stream); - auto body = ubx_message.body(); - - switch (ubx_message.msg_type()) { - case 0x0107: - return {"gpsLocationExternal", gen_nav_pvt(static_cast(body))}; - case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) - return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; - case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) - return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; - case 0x0a09: - return {"ubloxGnss", gen_mon_hw(static_cast(body))}; - case 0x0a0b: - return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; - case 0x0135: - return {"ubloxGnss", gen_nav_sat(static_cast(body))}; - default: - LOGE("Unknown message type %x", ubx_message.msg_type()); - return {"ubloxGnss", kj::Array()}; - } -} - - -kj::Array UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { - MessageBuilder msg_builder; - auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); - gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); - gpsLoc.setFlags(msg->flags()); - gpsLoc.setHasFix((msg->flags() % 2) == 1); - gpsLoc.setLatitude(msg->lat() * 1e-07); - gpsLoc.setLongitude(msg->lon() * 1e-07); - gpsLoc.setAltitude(msg->height() * 1e-03); - gpsLoc.setSpeed(msg->g_speed() * 1e-03); - gpsLoc.setBearingDeg(msg->head_mot() * 1e-5); - gpsLoc.setHorizontalAccuracy(msg->h_acc() * 1e-03); - gpsLoc.setSatelliteCount(msg->num_sv()); - std::tm timeinfo = std::tm(); - timeinfo.tm_year = msg->year() - 1900; - timeinfo.tm_mon = msg->month() - 1; - timeinfo.tm_mday = msg->day(); - timeinfo.tm_hour = msg->hour(); - timeinfo.tm_min = msg->min(); - timeinfo.tm_sec = msg->sec(); - - std::time_t utc_tt = timegm(&timeinfo); - gpsLoc.setUnixTimestampMillis(utc_tt * 1e+03 + msg->nano() * 1e-06); - float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f }; - gpsLoc.setVNED(f); - gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03); - gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03); - gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) { - // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes - // We will first need to separate the data from the padding and parity - auto body = *msg->body(); - assert(body.size() == 10); - - std::string subframe_data; - subframe_data.reserve(30); - for (uint32_t word : body) { - word = word >> 6; // TODO: Verify parity - subframe_data.push_back(word >> 16); - subframe_data.push_back(word >> 8); - subframe_data.push_back(word >> 0); - } - - // Collect subframes in map and parse when we have all the parts - { - kaitai::kstream stream(subframe_data); - gps_t subframe(&stream); - - int subframe_id = subframe.how()->subframe_id(); - if (subframe_id > 3 || subframe_id < 1) { - // don't parse almanac subframes - return kj::Array(); - } - gps_subframes[msg->sv_id()][subframe_id] = subframe_data; - } - - // publish if subframes 1-3 have been collected - if (gps_subframes[msg->sv_id()].size() == 3) { - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); - eph.setSvId(msg->sv_id()); - - int iode_s2 = 0; - int iode_s3 = 0; - int iodc_lsb = 0; - int week; - - // Subframe 1 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); - gps_t subframe(&stream); - gps_t::subframe_1_t* subframe_1 = static_cast(subframe.body()); - - // Each message is incremented to be greater or equal than week 1877 (2015-12-27). - // To skip this use the current_time argument - week = subframe_1->week_no(); - week += 1024; - if (week < 1877) { - week += 1024; - } - //eph.setGpsWeek(subframe_1->week_no()); - eph.setTgd(subframe_1->t_gd() * pow(2, -31)); - eph.setToc(subframe_1->t_oc() * pow(2, 4)); - eph.setAf2(subframe_1->af_2() * pow(2, -55)); - eph.setAf1(subframe_1->af_1() * pow(2, -43)); - eph.setAf0(subframe_1->af_0() * pow(2, -31)); - eph.setSvHealth(subframe_1->sv_health()); - eph.setTowCount(subframe.how()->tow_count()); - iodc_lsb = subframe_1->iodc_lsb(); - } - - // Subframe 2 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][2]); - gps_t subframe(&stream); - gps_t::subframe_2_t* subframe_2 = static_cast(subframe.body()); - - // GPS week refers to current week, the ephemeris can be valid for the next - // if toe equals 0, this can be verified by the TOW count if it is within the - // last 2 hours of the week (gps ephemeris valid for 4hours) - if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){ - week += 1; - } - eph.setCrs(subframe_2->c_rs() * pow(2, -5)); - eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi); - eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi); - eph.setCuc(subframe_2->c_uc() * pow(2, -29)); - eph.setEcc(subframe_2->e() * pow(2, -33)); - eph.setCus(subframe_2->c_us() * pow(2, -29)); - eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); - eph.setToe(subframe_2->t_oe() * pow(2, 4)); - iode_s2 = subframe_2->iode(); - } - - // Subframe 3 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][3]); - gps_t subframe(&stream); - gps_t::subframe_3_t* subframe_3 = static_cast(subframe.body()); - - eph.setCic(subframe_3->c_ic() * pow(2, -29)); - eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi); - eph.setCis(subframe_3->c_is() * pow(2, -29)); - eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi); - eph.setCrc(subframe_3->c_rc() * pow(2, -5)); - eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi); - eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); - eph.setIode(subframe_3->iode()); - eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); - iode_s3 = subframe_3->iode(); - } - - eph.setToeWeek(week); - eph.setTocWeek(week); - - gps_subframes[msg->sv_id()].clear(); - if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { - // data set cutover, reject ephemeris - return kj::Array(); - } - return capnp::messageToFlatArray(msg_builder); - } - return kj::Array(); -} - -kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) { - // This parser assumes that no 2 satellites of the same frequency - // can be in view at the same time - auto body = *msg->body(); - assert(body.size() == 4); - { - std::string string_data; - string_data.reserve(16); - for (uint32_t word : body) { - for (int i = 3; i >= 0; i--) - string_data.push_back(word >> 8*i); - } - - kaitai::kstream stream(string_data); - glonass_t gl_string(&stream); - int string_number = gl_string.string_number(); - if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) { - // don't parse non immediate data, idle_chip == 0 - return kj::Array(); - } - - // Check if new string either has same superframe_id or log transmission times make sense - bool superframe_unknown = false; - bool needs_clear = false; - for (int i = 1; i <= 5; i++) { - if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end()) - continue; - if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) { - superframe_unknown = true; - } else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) { - needs_clear = true; - } - // Check if string times add up to being from the same frame - // If superframe is known this is redundant - // Strings are sent 2s apart and frames are 30s apart - if (superframe_unknown && - std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10) - needs_clear = true; - } - if (needs_clear) { - glonass_strings[msg->freq_id()].clear(); - glonass_string_superframes[msg->freq_id()].clear(); - glonass_string_times[msg->freq_id()].clear(); - } - glonass_strings[msg->freq_id()][string_number] = string_data; - glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number(); - glonass_string_times[msg->freq_id()][string_number] = last_log_time; - } - if (msg->sv_id() == 255) { - // data can be decoded before identifying the SV number, in this case 255 - // is returned, which means "unknown" (ublox p32) - return kj::Array(); - } - - // publish if strings 1-5 have been collected - if (glonass_strings[msg->freq_id()].size() != 5) { - return kj::Array(); - } - - MessageBuilder msg_builder; - auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris(); - eph.setSvId(msg->sv_id()); - eph.setFreqNum(msg->freq_id() - 7); - - uint16_t current_day = 0; - uint16_t tk = 0; - - // string number 1 - { - kaitai::kstream stream(glonass_strings[msg->freq_id()][1]); - glonass_t gl_stream(&stream); - glonass_t::string_1_t* data = static_cast(gl_stream.data()); - - eph.setP1(data->p1()); - tk = data->t_k(); - eph.setTkDEPRECATED(tk); - eph.setXVel(data->x_vel() * pow(2, -20)); - eph.setXAccel(data->x_accel() * pow(2, -30)); - eph.setX(data->x() * pow(2, -11)); - } - - // string number 2 - { - kaitai::kstream stream(glonass_strings[msg->freq_id()][2]); - glonass_t gl_stream(&stream); - glonass_t::string_2_t* data = static_cast(gl_stream.data()); - - eph.setSvHealth(data->b_n()>>2); // MSB indicates health - eph.setP2(data->p2()); - eph.setTb(data->t_b()); - eph.setYVel(data->y_vel() * pow(2, -20)); - eph.setYAccel(data->y_accel() * pow(2, -30)); - eph.setY(data->y() * pow(2, -11)); - } - - // string number 3 - { - kaitai::kstream stream(glonass_strings[msg->freq_id()][3]); - glonass_t gl_stream(&stream); - glonass_t::string_3_t* data = static_cast(gl_stream.data()); - - eph.setP3(data->p3()); - eph.setGammaN(data->gamma_n() * pow(2, -40)); - eph.setSvHealth(eph.getSvHealth() | data->l_n()); - eph.setZVel(data->z_vel() * pow(2, -20)); - eph.setZAccel(data->z_accel() * pow(2, -30)); - eph.setZ(data->z() * pow(2, -11)); - } - - // string number 4 - { - kaitai::kstream stream(glonass_strings[msg->freq_id()][4]); - glonass_t gl_stream(&stream); - glonass_t::string_4_t* data = static_cast(gl_stream.data()); - - current_day = data->n_t(); - eph.setNt(current_day); - eph.setTauN(data->tau_n() * pow(2, -30)); - eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30)); - eph.setAge(data->e_n()); - eph.setP4(data->p4()); - eph.setSvURA(glonass_URA_lookup.at(data->f_t())); - if (msg->sv_id() != data->n()) { - LOGE("SV_ID != SLOT_NUMBER: %d %" PRIu64, msg->sv_id(), data->n()); - } - eph.setSvType(data->m()); - } - - // string number 5 - { - kaitai::kstream stream(glonass_strings[msg->freq_id()][5]); - glonass_t gl_stream(&stream); - glonass_t::string_5_t* data = static_cast(gl_stream.data()); - - // string5 parsing is only needed to get the year, this can be removed and - // the year can be fetched later in laika (note rollovers and leap year) - eph.setN4(data->n_4()); - int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30; - eph.setTkSeconds(tk_seconds); - } - - glonass_strings[msg->freq_id()].clear(); - return capnp::messageToFlatArray(msg_builder); -} - - -kj::Array UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) { - switch (msg->gnss_id()) { - case ubx_t::gnss_type_t::GNSS_TYPE_GPS: - return parse_gps_ephemeris(msg); - case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS: - return parse_glonass_ephemeris(msg); - default: - return kj::Array(); - } -} - -kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { - MessageBuilder msg_builder; - auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport(); - mr.setRcvTow(msg->rcv_tow()); - mr.setGpsWeek(msg->week()); - mr.setLeapSeconds(msg->leap_s()); - mr.setGpsWeek(msg->week()); - - auto mb = mr.initMeasurements(msg->num_meas()); - auto measurements = *msg->meas(); - for (int8_t i = 0; i < msg->num_meas(); i++) { - mb[i].setSvId(measurements[i]->sv_id()); - mb[i].setPseudorange(measurements[i]->pr_mes()); - mb[i].setCarrierCycles(measurements[i]->cp_mes()); - mb[i].setDoppler(measurements[i]->do_mes()); - mb[i].setGnssId(measurements[i]->gnss_id()); - mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id()); - mb[i].setLocktime(measurements[i]->lock_time()); - mb[i].setCno(measurements[i]->cno()); - mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong - mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15)); - mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong - - auto ts = mb[i].initTrackingStatus(); - auto trk_stat = measurements[i]->trk_stat(); - ts.setPseudorangeValid(bit_to_bool(trk_stat, 0)); - ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1)); - ts.setHalfCycleValid(bit_to_bool(trk_stat, 2)); - ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3)); - } - - mr.setNumMeas(msg->num_meas()); - auto rs = mr.initReceiverStatus(); - rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0)); - rs.setClkReset(bit_to_bool(msg->rec_stat(), 2)); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) { - MessageBuilder msg_builder; - auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport(); - sr.setITow(msg->itow()); - - auto svs = sr.initSvs(msg->num_svs()); - auto svs_data = *msg->svs(); - for (int8_t i = 0; i < msg->num_svs(); i++) { - svs[i].setSvId(svs_data[i]->sv_id()); - svs[i].setGnssId(svs_data[i]->gnss_id()); - svs[i].setFlagsBitfield(svs_data[i]->flags()); - svs[i].setCno(svs_data[i]->cno()); - svs[i].setElevationDeg(svs_data[i]->elev()); - svs[i].setAzimuthDeg(svs_data[i]->azim()); - svs[i].setPseudorangeResidual(svs_data[i]->pr_res() * 0.1); - } - - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) { - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus(); - hwStatus.setNoisePerMS(msg->noise_per_ms()); - hwStatus.setFlags(msg->flags()); - hwStatus.setAgcCnt(msg->agc_cnt()); - hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status()); - hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power()); - hwStatus.setJamInd(msg->jam_ind()); - return capnp::messageToFlatArray(msg_builder); -} - -kj::Array UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) { - MessageBuilder msg_builder; - auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); - hwStatus.setOfsI(msg->ofs_i()); - hwStatus.setMagI(msg->mag_i()); - hwStatus.setOfsQ(msg->ofs_q()); - hwStatus.setMagQ(msg->mag_q()); - - switch (msg->cfg_source()) { - case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); - break; - case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); - break; - case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); - break; - case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); - break; - default: - hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); - break; - } - - hwStatus.setLowLevCfg(msg->low_lev_cfg()); - hwStatus.setPostStatus(msg->post_status()); - - return capnp::messageToFlatArray(msg_builder); -} diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h deleted file mode 100644 index d21760edc2..0000000000 --- a/system/ubloxd/ublox_msg.h +++ /dev/null @@ -1,131 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/util.h" -#include "system/ubloxd/generated/gps.h" -#include "system/ubloxd/generated/glonass.h" -#include "system/ubloxd/generated/ubx.h" - -using namespace std::string_literals; - -const int SECS_IN_MIN = 60; -const int SECS_IN_HR = 60 * SECS_IN_MIN; -const int SECS_IN_DAY = 24 * SECS_IN_HR; -const int SECS_IN_WEEK = 7 * SECS_IN_DAY; - -// protocol constants -namespace ublox { - const uint8_t PREAMBLE1 = 0xb5; - const uint8_t PREAMBLE2 = 0x62; - - const int UBLOX_HEADER_SIZE = 6; - const int UBLOX_CHECKSUM_SIZE = 2; - const int UBLOX_MAX_MSG_SIZE = 65536; - - struct ubx_mga_ini_time_utc_t { - uint8_t type; - uint8_t version; - uint8_t ref; - int8_t leapSecs; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint8_t second; - uint8_t reserved1; - uint32_t ns; - uint16_t tAccS; - uint16_t reserved2; - uint32_t tAccNs; - } __attribute__((packed)); - - inline std::string ubx_add_checksum(const std::string &msg) { - assert(msg.size() > 2); - - uint8_t ck_a = 0, ck_b = 0; - for (int i = 2; i < msg.size(); i++) { - ck_a = (ck_a + msg[i]) & 0xFF; - ck_b = (ck_b + ck_a) & 0xFF; - } - - std::string r = msg; - r.push_back(ck_a); - r.push_back(ck_b); - return r; - } - - inline std::string build_ubx_mga_ini_time_utc(struct tm time) { - ublox::ubx_mga_ini_time_utc_t payload = { - .type = 0x10, - .version = 0x0, - .ref = 0x0, - .leapSecs = -128, // Unknown - .year = (uint16_t)(1900 + time.tm_year), - .month = (uint8_t)(1 + time.tm_mon), - .day = (uint8_t)time.tm_mday, - .hour = (uint8_t)time.tm_hour, - .minute = (uint8_t)time.tm_min, - .second = (uint8_t)time.tm_sec, - .reserved1 = 0x0, - .ns = 0, - .tAccS = 30, - .reserved2 = 0x0, - .tAccNs = 0, - }; - assert(sizeof(payload) == 24); - - std::string msg = "\xb5\x62\x13\x40\x18\x00"s; - msg += std::string((char*)&payload, sizeof(payload)); - - return ubx_add_checksum(msg); - } -} - -class UbloxMsgParser { - public: - bool add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); - inline void reset() {bytes_in_parse_buf = 0;} - inline int needed_bytes(); - inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);} - - std::pair> gen_msg(); - kj::Array gen_nav_pvt(ubx_t::nav_pvt_t *msg); - kj::Array gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg); - kj::Array gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); - kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); - kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); - kj::Array gen_nav_sat(ubx_t::nav_sat_t *msg); - - private: - inline bool valid_cheksum(); - inline bool valid(); - inline bool valid_so_far(); - - kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); - kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); - - std::unordered_map> gps_subframes; - - float last_log_time = 0.0; - size_t bytes_in_parse_buf = 0; - uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE]; - - // user range accuracy in meters - const std::unordered_map glonass_URA_lookup = - {{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7}, - { 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32}, - {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}}; - - std::unordered_map> glonass_strings; - std::unordered_map> glonass_string_times; - std::unordered_map> glonass_string_superframes; -}; diff --git a/system/ubloxd/ubloxd.cc b/system/ubloxd/ubloxd.cc deleted file mode 100644 index 4e7e91f830..0000000000 --- a/system/ubloxd/ubloxd.cc +++ /dev/null @@ -1,62 +0,0 @@ -#include - -#include - -#include "cereal/messaging/messaging.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "system/ubloxd/ublox_msg.h" - -ExitHandler do_exit; -using namespace ublox; - -int main() { - LOGW("starting ubloxd"); - AlignedBuffer aligned_buf; - UbloxMsgParser parser; - - PubMaster pm({"ubloxGnss", "gpsLocationExternal"}); - - std::unique_ptr context(Context::create()); - std::unique_ptr subscriber(SubSocket::create(context.get(), "ubloxRaw")); - assert(subscriber != NULL); - subscriber->setTimeout(100); - - - while (!do_exit) { - std::unique_ptr msg(subscriber->receive()); - if (!msg) { - continue; - } - - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); - cereal::Event::Reader event = cmsg.getRoot(); - auto ubloxRaw = event.getUbloxRaw(); - float log_time = 1e-9 * event.getLogMonoTime(); - - const uint8_t *data = ubloxRaw.begin(); - size_t len = ubloxRaw.size(); - size_t bytes_consumed = 0; - - while (bytes_consumed < len && !do_exit) { - size_t bytes_consumed_this_time = 0U; - if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { - - try { - auto ublox_msg = parser.gen_msg(); - if (ublox_msg.second.size() > 0) { - auto bytes = ublox_msg.second.asBytes(); - pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size()); - } - } catch (const std::exception& e) { - LOGE("Error parsing ublox message %s", e.what()); - } - - parser.reset(); - } - bytes_consumed += bytes_consumed_this_time; - } - } - - return 0; -} diff --git a/system/ubloxd/ubloxd.py b/system/ubloxd/ubloxd.py new file mode 100755 index 0000000000..84a926dd78 --- /dev/null +++ b/system/ubloxd/ubloxd.py @@ -0,0 +1,519 @@ +#!/usr/bin/env python3 +import math +import capnp +import calendar +import numpy as np +from collections import defaultdict +from dataclasses import dataclass + +from cereal import log +from cereal import messaging +from openpilot.system.ubloxd.generated.ubx import Ubx +from openpilot.system.ubloxd.generated.gps import Gps +from openpilot.system.ubloxd.generated.glonass import Glonass + + +SECS_IN_MIN = 60 +SECS_IN_HR = 60 * SECS_IN_MIN +SECS_IN_DAY = 24 * SECS_IN_HR +SECS_IN_WEEK = 7 * SECS_IN_DAY + + +class UbxFramer: + PREAMBLE1 = 0xB5 + PREAMBLE2 = 0x62 + HEADER_SIZE = 6 + CHECKSUM_SIZE = 2 + + def __init__(self) -> None: + self.buf = bytearray() + self.last_log_time = 0.0 + + def reset(self) -> None: + self.buf.clear() + + @staticmethod + def _checksum_ok(frame: bytes) -> bool: + ck_a = 0 + ck_b = 0 + for b in frame[2:-2]: + ck_a = (ck_a + b) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return ck_a == frame[-2] and ck_b == frame[-1] + + def add_data(self, log_time: float, incoming: bytes) -> list[bytes]: + self.last_log_time = log_time + out: list[bytes] = [] + if not incoming: + return out + self.buf += incoming + + while True: + # find preamble + if len(self.buf) < 2: + break + start = self.buf.find(b"\xB5\x62") + if start < 0: + # no preamble in buffer + self.buf.clear() + break + if start > 0: + # drop garbage before preamble + self.buf = self.buf[start:] + + if len(self.buf) < self.HEADER_SIZE: + break + + length_le = int.from_bytes(self.buf[4:6], 'little', signed=False) + total_len = self.HEADER_SIZE + length_le + self.CHECKSUM_SIZE + if len(self.buf) < total_len: + break + + candidate = bytes(self.buf[:total_len]) + if self._checksum_ok(candidate): + out.append(candidate) + # consume this frame + self.buf = self.buf[total_len:] + else: + # drop first byte and retry + self.buf = self.buf[1:] + + return out + + +def _bit(b: int, shift: int) -> bool: + return (b & (1 << shift)) != 0 + + +@dataclass +class EphemerisCaches: + gps_subframes: defaultdict[int, dict[int, bytes]] + glonass_strings: defaultdict[int, dict[int, bytes]] + glonass_string_times: defaultdict[int, dict[int, float]] + glonass_string_superframes: defaultdict[int, dict[int, int]] + + +class UbloxMsgParser: + gpsPi = 3.1415926535898 + + # user range accuracy in meters + glonass_URA_lookup: dict[int, float] = { + 0: 1, 1: 2, 2: 2.5, 3: 4, 4: 5, 5: 7, + 6: 10, 7: 12, 8: 14, 9: 16, 10: 32, + 11: 64, 12: 128, 13: 256, 14: 512, 15: 1024, + } + + def __init__(self) -> None: + self.framer = UbxFramer() + self.caches = EphemerisCaches( + gps_subframes=defaultdict(dict), + glonass_strings=defaultdict(dict), + glonass_string_times=defaultdict(dict), + glonass_string_superframes=defaultdict(dict), + ) + + # Message generation entry point + def parse_frame(self, frame: bytes) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None: + # Quick header parse + msg_type = int.from_bytes(frame[2:4], 'big') + payload = frame[6:-2] + if msg_type == 0x0107: + body = Ubx.NavPvt.from_bytes(payload) + return self._gen_nav_pvt(body) + if msg_type == 0x0213: + # Manually parse RXM-SFRBX to avoid Kaitai EOF on some frames + if len(payload) < 8: + return None + gnss_id = payload[0] + sv_id = payload[1] + freq_id = payload[3] + num_words = payload[4] + exp = 8 + 4 * num_words + if exp != len(payload): + return None + words: list[int] = [] + off = 8 + for _ in range(num_words): + words.append(int.from_bytes(payload[off:off+4], 'little')) + off += 4 + + class _SfrbxView: + def __init__(self, gid: int, sid: int, fid: int, body: list[int]): + self.gnss_id = Ubx.GnssType(gid) + self.sv_id = sid + self.freq_id = fid + self.body = body + view = _SfrbxView(gnss_id, sv_id, freq_id, words) + return self._gen_rxm_sfrbx(view) + if msg_type == 0x0215: + body = Ubx.RxmRawx.from_bytes(payload) + return self._gen_rxm_rawx(body) + if msg_type == 0x0A09: + body = Ubx.MonHw.from_bytes(payload) + return self._gen_mon_hw(body) + if msg_type == 0x0A0B: + body = Ubx.MonHw2.from_bytes(payload) + return self._gen_mon_hw2(body) + if msg_type == 0x0135: + body = Ubx.NavSat.from_bytes(payload) + return self._gen_nav_sat(body) + return None + + # NAV-PVT -> gpsLocationExternal + def _gen_nav_pvt(self, msg: Ubx.NavPvt) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]: + dat = messaging.new_message('gpsLocationExternal', valid=True) + gps = dat.gpsLocationExternal + gps.source = log.GpsLocationData.SensorSource.ublox + gps.flags = msg.flags + gps.hasFix = (msg.flags % 2) == 1 + gps.latitude = msg.lat * 1e-07 + gps.longitude = msg.lon * 1e-07 + gps.altitude = msg.height * 1e-03 + gps.speed = msg.g_speed * 1e-03 + gps.bearingDeg = msg.head_mot * 1e-5 + gps.horizontalAccuracy = msg.h_acc * 1e-03 + gps.satelliteCount = msg.num_sv + + # build UTC timestamp millis (NAV-PVT is in UTC) + # tolerate invalid or unset date values like C++ timegm + try: + utc_tt = calendar.timegm((msg.year, msg.month, msg.day, msg.hour, msg.min, msg.sec, 0, 0, 0)) + except Exception: + utc_tt = 0 + gps.unixTimestampMillis = int(utc_tt * 1e3 + (msg.nano * 1e-6)) + + # match C++ float32 rounding semantics exactly + gps.vNED = [ + float(np.float32(msg.vel_n) * np.float32(1e-03)), + float(np.float32(msg.vel_e) * np.float32(1e-03)), + float(np.float32(msg.vel_d) * np.float32(1e-03)), + ] + gps.verticalAccuracy = msg.v_acc * 1e-03 + gps.speedAccuracy = msg.s_acc * 1e-03 + gps.bearingAccuracyDeg = msg.head_acc * 1e-05 + return ('gpsLocationExternal', dat) + + # RXM-SFRBX dispatch to GPS or GLONASS ephemeris + def _gen_rxm_sfrbx(self, msg) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None: + if msg.gnss_id == Ubx.GnssType.gps: + return self._parse_gps_ephemeris(msg) + if msg.gnss_id == Ubx.GnssType.glonass: + return self._parse_glonass_ephemeris(msg) + return None + + def _parse_gps_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None: + # body is list of 10 words; convert to 30-byte subframe (strip parity/padding) + body = msg.body + if len(body) != 10: + return None + subframe_data = bytearray() + for word in body: + word >>= 6 + subframe_data.append((word >> 16) & 0xFF) + subframe_data.append((word >> 8) & 0xFF) + subframe_data.append(word & 0xFF) + + sf = Gps.from_bytes(bytes(subframe_data)) + subframe_id = sf.how.subframe_id + if subframe_id < 1 or subframe_id > 3: + return None + self.caches.gps_subframes[msg.sv_id][subframe_id] = bytes(subframe_data) + + if len(self.caches.gps_subframes[msg.sv_id]) != 3: + return None + + dat = messaging.new_message('ubloxGnss', valid=True) + eph = dat.ubloxGnss.init('ephemeris') + eph.svId = msg.sv_id + + iode_s2 = 0 + iode_s3 = 0 + iodc_lsb = 0 + week = 0 + + # Subframe 1 + sf1 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][1]) + s1 = sf1.body + assert isinstance(s1, Gps.Subframe1) + week = s1.week_no + week += 1024 + if week < 1877: + week += 1024 + eph.tgd = s1.t_gd * math.pow(2, -31) + eph.toc = s1.t_oc * math.pow(2, 4) + eph.af2 = s1.af_2 * math.pow(2, -55) + eph.af1 = s1.af_1 * math.pow(2, -43) + eph.af0 = s1.af_0 * math.pow(2, -31) + eph.svHealth = s1.sv_health + eph.towCount = sf1.how.tow_count + iodc_lsb = s1.iodc_lsb + + # Subframe 2 + sf2 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][2]) + s2 = sf2.body + assert isinstance(s2, Gps.Subframe2) + if s2.t_oe == 0 and sf2.how.tow_count * 6 >= (SECS_IN_WEEK - 2 * SECS_IN_HR): + week += 1 + eph.crs = s2.c_rs * math.pow(2, -5) + eph.deltaN = s2.delta_n * math.pow(2, -43) * self.gpsPi + eph.m0 = s2.m_0 * math.pow(2, -31) * self.gpsPi + eph.cuc = s2.c_uc * math.pow(2, -29) + eph.ecc = s2.e * math.pow(2, -33) + eph.cus = s2.c_us * math.pow(2, -29) + eph.a = math.pow(s2.sqrt_a * math.pow(2, -19), 2.0) + eph.toe = s2.t_oe * math.pow(2, 4) + iode_s2 = s2.iode + + # Subframe 3 + sf3 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][3]) + s3 = sf3.body + assert isinstance(s3, Gps.Subframe3) + eph.cic = s3.c_ic * math.pow(2, -29) + eph.omega0 = s3.omega_0 * math.pow(2, -31) * self.gpsPi + eph.cis = s3.c_is * math.pow(2, -29) + eph.i0 = s3.i_0 * math.pow(2, -31) * self.gpsPi + eph.crc = s3.c_rc * math.pow(2, -5) + eph.omega = s3.omega * math.pow(2, -31) * self.gpsPi + eph.omegaDot = s3.omega_dot * math.pow(2, -43) * self.gpsPi + eph.iode = s3.iode + eph.iDot = s3.idot * math.pow(2, -43) * self.gpsPi + iode_s3 = s3.iode + + eph.toeWeek = week + eph.tocWeek = week + + # clear cache for this SV + self.caches.gps_subframes[msg.sv_id].clear() + if not (iodc_lsb == iode_s2 == iode_s3): + return None + return ('ubloxGnss', dat) + + def _parse_glonass_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None: + # words are 4 bytes each; Glonass parser expects 16 bytes (string) + body = msg.body + if len(body) != 4: + return None + string_bytes = bytearray() + for word in body: + for i in (3, 2, 1, 0): + string_bytes.append((word >> (8 * i)) & 0xFF) + + gl = Glonass.from_bytes(bytes(string_bytes)) + string_number = gl.string_number + if string_number < 1 or string_number > 5 or gl.idle_chip: + return None + + # correlate by superframe and timing, similar to C++ logic + freq_id = msg.freq_id + superframe_unknown = False + needs_clear = False + for i in range(1, 6): + if i not in self.caches.glonass_strings[freq_id]: + continue + sf_prev = self.caches.glonass_string_superframes[freq_id].get(i, 0) + if sf_prev == 0 or gl.superframe_number == 0: + superframe_unknown = True + elif sf_prev != gl.superframe_number: + needs_clear = True + if superframe_unknown: + prev_time = self.caches.glonass_string_times[freq_id].get(i, 0.0) + if abs((prev_time - 2.0 * i) - (self.framer.last_log_time - 2.0 * string_number)) > 10: + needs_clear = True + + if needs_clear: + self.caches.glonass_strings[freq_id].clear() + self.caches.glonass_string_superframes[freq_id].clear() + self.caches.glonass_string_times[freq_id].clear() + + self.caches.glonass_strings[freq_id][string_number] = bytes(string_bytes) + self.caches.glonass_string_superframes[freq_id][string_number] = gl.superframe_number + self.caches.glonass_string_times[freq_id][string_number] = self.framer.last_log_time + + if msg.sv_id == 255: + # unknown SV id + return None + if len(self.caches.glonass_strings[freq_id]) != 5: + return None + + dat = messaging.new_message('ubloxGnss', valid=True) + eph = dat.ubloxGnss.init('glonassEphemeris') + eph.svId = msg.sv_id + eph.freqNum = msg.freq_id - 7 + + current_day = 0 + tk = 0 + + # string 1 + try: + s1 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][1]).data + except Exception: + return None + assert isinstance(s1, Glonass.String1) + eph.p1 = int(s1.p1) + tk = int(s1.t_k) + eph.tkDEPRECATED = tk + eph.xVel = float(s1.x_vel) * math.pow(2, -20) + eph.xAccel = float(s1.x_accel) * math.pow(2, -30) + eph.x = float(s1.x) * math.pow(2, -11) + + # string 2 + try: + s2 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][2]).data + except Exception: + return None + assert isinstance(s2, Glonass.String2) + eph.svHealth = int(s2.b_n >> 2) + eph.p2 = int(s2.p2) + eph.tb = int(s2.t_b) + eph.yVel = float(s2.y_vel) * math.pow(2, -20) + eph.yAccel = float(s2.y_accel) * math.pow(2, -30) + eph.y = float(s2.y) * math.pow(2, -11) + + # string 3 + try: + s3 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][3]).data + except Exception: + return None + assert isinstance(s3, Glonass.String3) + eph.p3 = int(s3.p3) + eph.gammaN = float(s3.gamma_n) * math.pow(2, -40) + eph.svHealth = int(eph.svHealth | (1 if s3.l_n else 0)) + eph.zVel = float(s3.z_vel) * math.pow(2, -20) + eph.zAccel = float(s3.z_accel) * math.pow(2, -30) + eph.z = float(s3.z) * math.pow(2, -11) + + # string 4 + try: + s4 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][4]).data + except Exception: + return None + assert isinstance(s4, Glonass.String4) + current_day = int(s4.n_t) + eph.nt = current_day + eph.tauN = float(s4.tau_n) * math.pow(2, -30) + eph.deltaTauN = float(s4.delta_tau_n) * math.pow(2, -30) + eph.age = int(s4.e_n) + eph.p4 = int(s4.p4) + eph.svURA = float(self.glonass_URA_lookup.get(int(s4.f_t), 0.0)) + # consistency check: SV slot number + # if it doesn't match, keep going but note mismatch (no logging here) + eph.svType = int(s4.m) + + # string 5 + try: + s5 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][5]).data + except Exception: + return None + assert isinstance(s5, Glonass.String5) + eph.n4 = int(s5.n_4) + tk_seconds = int(SECS_IN_HR * ((tk >> 7) & 0x1F) + SECS_IN_MIN * ((tk >> 1) & 0x3F) + (tk & 0x1) * 30) + eph.tkSeconds = tk_seconds + + self.caches.glonass_strings[freq_id].clear() + return ('ubloxGnss', dat) + + def _gen_rxm_rawx(self, msg: Ubx.RxmRawx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]: + dat = messaging.new_message('ubloxGnss', valid=True) + mr = dat.ubloxGnss.init('measurementReport') + mr.rcvTow = msg.rcv_tow + mr.gpsWeek = msg.week + mr.leapSeconds = msg.leap_s + + mb = mr.init('measurements', msg.num_meas) + for i, m in enumerate(msg.meas): + mb[i].svId = m.sv_id + mb[i].pseudorange = m.pr_mes + mb[i].carrierCycles = m.cp_mes + mb[i].doppler = m.do_mes + mb[i].gnssId = int(m.gnss_id.value) + mb[i].glonassFrequencyIndex = m.freq_id + mb[i].locktime = m.lock_time + mb[i].cno = m.cno + mb[i].pseudorangeStdev = 0.01 * (math.pow(2, (m.pr_stdev & 15))) + mb[i].carrierPhaseStdev = 0.004 * (m.cp_stdev & 15) + mb[i].dopplerStdev = 0.002 * (math.pow(2, (m.do_stdev & 15))) + + ts = mb[i].init('trackingStatus') + trk = m.trk_stat + ts.pseudorangeValid = _bit(trk, 0) + ts.carrierPhaseValid = _bit(trk, 1) + ts.halfCycleValid = _bit(trk, 2) + ts.halfCycleSubtracted = _bit(trk, 3) + + mr.numMeas = msg.num_meas + rs = mr.init('receiverStatus') + rs.leapSecValid = _bit(msg.rec_stat, 0) + rs.clkReset = _bit(msg.rec_stat, 2) + return ('ubloxGnss', dat) + + def _gen_nav_sat(self, msg: Ubx.NavSat) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]: + dat = messaging.new_message('ubloxGnss', valid=True) + sr = dat.ubloxGnss.init('satReport') + sr.iTow = msg.itow + svs = sr.init('svs', msg.num_svs) + for i, s in enumerate(msg.svs): + svs[i].svId = s.sv_id + svs[i].gnssId = int(s.gnss_id.value) + svs[i].flagsBitfield = s.flags + svs[i].cno = s.cno + svs[i].elevationDeg = s.elev + svs[i].azimuthDeg = s.azim + svs[i].pseudorangeResidual = s.pr_res * 0.1 + return ('ubloxGnss', dat) + + def _gen_mon_hw(self, msg: Ubx.MonHw) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]: + dat = messaging.new_message('ubloxGnss', valid=True) + hw = dat.ubloxGnss.init('hwStatus') + hw.noisePerMS = msg.noise_per_ms + hw.flags = msg.flags + hw.agcCnt = msg.agc_cnt + hw.aStatus = int(msg.a_status.value) + hw.aPower = int(msg.a_power.value) + hw.jamInd = msg.jam_ind + return ('ubloxGnss', dat) + + def _gen_mon_hw2(self, msg: Ubx.MonHw2) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]: + dat = messaging.new_message('ubloxGnss', valid=True) + hw = dat.ubloxGnss.init('hwStatus2') + hw.ofsI = msg.ofs_i + hw.magI = msg.mag_i + hw.ofsQ = msg.ofs_q + hw.magQ = msg.mag_q + # Map Ubx enum to cereal enum {undefined=0, rom=1, otp=2, configpins=3, flash=4} + cfg_map = { + Ubx.MonHw2.ConfigSource.rom: 1, + Ubx.MonHw2.ConfigSource.otp: 2, + Ubx.MonHw2.ConfigSource.config_pins: 3, + Ubx.MonHw2.ConfigSource.flash: 4, + } + hw.cfgSource = cfg_map.get(msg.cfg_source, 0) + hw.lowLevCfg = msg.low_lev_cfg + hw.postStatus = msg.post_status + return ('ubloxGnss', dat) + + +def main(): + parser = UbloxMsgParser() + pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal']) + sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False) + + while True: + msg = messaging.recv_one_or_none(sock) + if msg is None: + continue + + data = bytes(msg.ubloxRaw) + log_time = msg.logMonoTime * 1e-9 + frames = parser.framer.add_data(log_time, data) + for frame in frames: + try: + res = parser.parse_frame(frame) + except Exception: + continue + if not res: + continue + service, dat = res + pm.send(service, dat) + +if __name__ == '__main__': + main() diff --git a/system/ui/lib/networkmanager.py b/system/ui/lib/networkmanager.py new file mode 100644 index 0000000000..07b5d42f4b --- /dev/null +++ b/system/ui/lib/networkmanager.py @@ -0,0 +1,44 @@ +from enum import IntEnum + + +# NetworkManager device states +class NMDeviceState(IntEnum): + UNKNOWN = 0 + DISCONNECTED = 30 + PREPARE = 40 + STATE_CONFIG = 50 + NEED_AUTH = 60 + IP_CONFIG = 70 + ACTIVATED = 100 + DEACTIVATING = 110 + + +# NetworkManager constants +NM = "org.freedesktop.NetworkManager" +NM_PATH = '/org/freedesktop/NetworkManager' +NM_IFACE = 'org.freedesktop.NetworkManager' +NM_ACCESS_POINT_IFACE = 'org.freedesktop.NetworkManager.AccessPoint' +NM_SETTINGS_PATH = '/org/freedesktop/NetworkManager/Settings' +NM_SETTINGS_IFACE = 'org.freedesktop.NetworkManager.Settings' +NM_CONNECTION_IFACE = 'org.freedesktop.NetworkManager.Settings.Connection' +NM_WIRELESS_IFACE = 'org.freedesktop.NetworkManager.Device.Wireless' +NM_PROPERTIES_IFACE = 'org.freedesktop.DBus.Properties' +NM_DEVICE_IFACE = "org.freedesktop.NetworkManager.Device" + +NM_DEVICE_TYPE_WIFI = 2 +NM_DEVICE_TYPE_MODEM = 8 +NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT = 8 +NM_DEVICE_STATE_REASON_NEW_ACTIVATION = 60 + +# https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApFlags +NM_802_11_AP_FLAGS_NONE = 0x0 +NM_802_11_AP_FLAGS_PRIVACY = 0x1 +NM_802_11_AP_FLAGS_WPS = 0x2 + +# https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NM80211ApSecurityFlags +NM_802_11_AP_SEC_PAIR_WEP40 = 0x00000001 +NM_802_11_AP_SEC_PAIR_WEP104 = 0x00000002 +NM_802_11_AP_SEC_GROUP_WEP40 = 0x00000010 +NM_802_11_AP_SEC_GROUP_WEP104 = 0x00000020 +NM_802_11_AP_SEC_KEY_MGMT_PSK = 0x00000100 +NM_802_11_AP_SEC_KEY_MGMT_802_1X = 0x00000200 diff --git a/system/ui/lib/wifi_manager.py b/system/ui/lib/wifi_manager.py index 4cb741bc95..af9ae943ea 100644 --- a/system/ui/lib/wifi_manager.py +++ b/system/ui/lib/wifi_manager.py @@ -1,52 +1,35 @@ -import asyncio -import concurrent.futures -import copy +import atexit import threading import time import uuid from collections.abc import Callable from dataclasses import dataclass from enum import IntEnum -from typing import TypeVar +from typing import Any -from dbus_next.aio import MessageBus -from dbus_next import BusType, Variant, Message -from dbus_next.errors import DBusError -from dbus_next.constants import MessageType +from jeepney import DBusAddress, new_method_call +from jeepney.bus_messages import MatchRule, message_bus +from jeepney.io.blocking import open_dbus_connection as open_dbus_connection_blocking +from jeepney.io.threading import DBusRouter, open_dbus_connection as open_dbus_connection_threading +from jeepney.low_level import MessageType +from jeepney.wrappers import Properties -try: - from openpilot.common.params import Params -except ImportError: - # Params/Cythonized modules are not available in zipapp - Params = None from openpilot.common.swaglog import cloudlog - -T = TypeVar("T") - -# NetworkManager constants -NM = "org.freedesktop.NetworkManager" -NM_PATH = '/org/freedesktop/NetworkManager' -NM_IFACE = 'org.freedesktop.NetworkManager' -NM_SETTINGS_PATH = '/org/freedesktop/NetworkManager/Settings' -NM_SETTINGS_IFACE = 'org.freedesktop.NetworkManager.Settings' -NM_CONNECTION_IFACE = 'org.freedesktop.NetworkManager.Settings.Connection' -NM_WIRELESS_IFACE = 'org.freedesktop.NetworkManager.Device.Wireless' -NM_PROPERTIES_IFACE = 'org.freedesktop.DBus.Properties' -NM_DEVICE_IFACE = "org.freedesktop.NetworkManager.Device" - -NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT = 8 +from openpilot.system.ui.lib.networkmanager import (NM, NM_WIRELESS_IFACE, NM_802_11_AP_SEC_PAIR_WEP40, + NM_802_11_AP_SEC_PAIR_WEP104, NM_802_11_AP_SEC_GROUP_WEP40, + NM_802_11_AP_SEC_GROUP_WEP104, NM_802_11_AP_SEC_KEY_MGMT_PSK, + NM_802_11_AP_SEC_KEY_MGMT_802_1X, NM_802_11_AP_FLAGS_NONE, + NM_802_11_AP_FLAGS_PRIVACY, NM_802_11_AP_FLAGS_WPS, + NM_PATH, NM_IFACE, NM_ACCESS_POINT_IFACE, NM_SETTINGS_PATH, + NM_SETTINGS_IFACE, NM_CONNECTION_IFACE, NM_DEVICE_IFACE, + NM_DEVICE_TYPE_WIFI, NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT, + NM_DEVICE_STATE_REASON_NEW_ACTIVATION, + NMDeviceState) TETHERING_IP_ADDRESS = "192.168.43.1" DEFAULT_TETHERING_PASSWORD = "swagswagcomma" - - -# NetworkManager device states -class NMDeviceState(IntEnum): - DISCONNECTED = 30 - PREPARE = 40 - NEED_AUTH = 60 - IP_CONFIG = 70 - ACTIVATED = 100 +SIGNAL_QUEUE_SIZE = 10 +SCAN_PERIOD_SECONDS = 10 class SecurityType(IntEnum): @@ -57,673 +40,387 @@ class SecurityType(IntEnum): UNSUPPORTED = 4 -@dataclass -class NetworkInfo: +def get_security_type(flags: int, wpa_flags: int, rsn_flags: int) -> SecurityType: + wpa_props = wpa_flags | rsn_flags + + # obtained by looking at flags of networks in the office as reported by an Android phone + supports_wpa = (NM_802_11_AP_SEC_PAIR_WEP40 | NM_802_11_AP_SEC_PAIR_WEP104 | NM_802_11_AP_SEC_GROUP_WEP40 | + NM_802_11_AP_SEC_GROUP_WEP104 | NM_802_11_AP_SEC_KEY_MGMT_PSK) + + if (flags == NM_802_11_AP_FLAGS_NONE) or ((flags & NM_802_11_AP_FLAGS_WPS) and not (wpa_props & supports_wpa)): + return SecurityType.OPEN + elif (flags & NM_802_11_AP_FLAGS_PRIVACY) and (wpa_props & supports_wpa) and not (wpa_props & NM_802_11_AP_SEC_KEY_MGMT_802_1X): + return SecurityType.WPA + else: + cloudlog.warning(f"Unsupported network! flags: {flags}, wpa_flags: {wpa_flags}, rsn_flags: {rsn_flags}") + return SecurityType.UNSUPPORTED + + +@dataclass(frozen=True) +class Network: ssid: str strength: int is_connected: bool security_type: SecurityType - path: str + is_saved: bool + + @classmethod + def from_dbus(cls, ssid: str, aps: list["AccessPoint"], is_saved: bool) -> "Network": + # we only want to show the strongest AP for each Network/SSID + strongest_ap = max(aps, key=lambda ap: ap.strength) + is_connected = any(ap.is_connected for ap in aps) + security_type = get_security_type(strongest_ap.flags, strongest_ap.wpa_flags, strongest_ap.rsn_flags) + + return cls( + ssid=ssid, + strength=strongest_ap.strength, + is_connected=is_connected and is_saved, + security_type=security_type, + is_saved=is_saved, + ) + + +@dataclass(frozen=True) +class AccessPoint: + ssid: str bssid: str - is_saved: bool = False - # saved_path: str + strength: int + is_connected: bool + flags: int + wpa_flags: int + rsn_flags: int + ap_path: str + @classmethod + def from_dbus(cls, ap_props: dict[str, tuple[str, Any]], ap_path: str, active_ap_path: str) -> "AccessPoint": + ssid = bytes(ap_props['Ssid'][1]).decode("utf-8", "replace") + bssid = str(ap_props['HwAddress'][1]) + strength = int(ap_props['Strength'][1]) + flags = int(ap_props['Flags'][1]) + wpa_flags = int(ap_props['WpaFlags'][1]) + rsn_flags = int(ap_props['RsnFlags'][1]) -@dataclass -class WifiManagerCallbacks: - need_auth: Callable[[str], None] | None = None - activated: Callable[[], None] | None = None - forgotten: Callable[[str], None] | None = None - networks_updated: Callable[[list[NetworkInfo]], None] | None = None - connection_failed: Callable[[str, str], None] | None = None # Added for error feedback + return cls( + ssid=ssid, + bssid=bssid, + strength=strength, + is_connected=ap_path == active_ap_path, + flags=flags, + wpa_flags=wpa_flags, + rsn_flags=rsn_flags, + ap_path=ap_path, + ) class WifiManager: - def __init__(self, callbacks): - self.callbacks: WifiManagerCallbacks = callbacks - self.networks: list[NetworkInfo] = [] - self.bus: MessageBus = None - self.device_path: str = "" - self.device_proxy = None - self.saved_connections: dict[str, str] = {} - self.active_ap_path: str = "" - self.scan_task: asyncio.Task | None = None - # Set tethering ssid as "weedle" + first 4 characters of a dongle id - self._tethering_ssid = "weedle" - if Params is not None: - dongle_id = Params().get("DongleId") - if dongle_id: - self._tethering_ssid += "-" + dongle_id[:4] - self.running: bool = True - self._current_connection_ssid: str | None = None + def __init__(self): + self._networks = [] # a network can be comprised of multiple APs + self._active = True # used to not run when not in settings + self._exit = False - async def connect(self) -> None: - """Connect to the DBus system bus.""" + # DBus connections try: - self.bus = await MessageBus(bus_type=BusType.SYSTEM).connect() - while not await self._find_wifi_device(): - await asyncio.sleep(1) + self._router_main = DBusRouter(open_dbus_connection_threading(bus="SYSTEM")) # used by scanner / general method calls + self._conn_monitor = open_dbus_connection_blocking(bus="SYSTEM") # used by state monitor thread + self._nm = DBusAddress(NM_PATH, bus_name=NM, interface=NM_IFACE) + except FileNotFoundError: + cloudlog.exception("Failed to connect to system D-Bus") + self._exit = True - await self._setup_signals(self.device_path) - self.active_ap_path = await self.get_active_access_point() - await self.add_tethering_connection(self._tethering_ssid, DEFAULT_TETHERING_PASSWORD) - self.saved_connections = await self._get_saved_connections() - self.scan_task = asyncio.create_task(self._periodic_scan()) - except DBusError as e: - cloudlog.error(f"Failed to connect to DBus: {e}") - raise - except Exception as e: - cloudlog.error(f"Unexpected error during connect: {e}") - raise + # Store wifi device path + self._wifi_device: str | None = None - async def shutdown(self) -> None: - self.running = False - if self.scan_task: - self.scan_task.cancel() - try: - await self.scan_task - except asyncio.CancelledError: - pass - if self.bus: - self.bus.disconnect() + # State + self._connecting_to_ssid: str = "" + self._last_network_update: float = 0.0 + self._callback_queue: list[Callable] = [] - async def _request_scan(self) -> None: - try: - interface = self.device_proxy.get_interface(NM_WIRELESS_IFACE) - await interface.call_request_scan({}) - except DBusError as e: - cloudlog.warning(f"Scan request failed: {str(e)}") + # Callbacks + self._need_auth: Callable[[str], None] | None = None + self._activated: Callable[[], None] | None = None + self._forgotten: Callable[[], None] | None = None + self._networks_updated: Callable[[list[Network]], None] | None = None + self._disconnected: Callable[[], None] | None = None - async def get_active_access_point(self): - try: - props_iface = self.device_proxy.get_interface(NM_PROPERTIES_IFACE) - ap_path = await props_iface.call_get(NM_WIRELESS_IFACE, 'ActiveAccessPoint') - return ap_path.value - except DBusError as e: - cloudlog.error(f"Error fetching active access point: {str(e)}") - return '' + self._lock = threading.Lock() - async def forget_connection(self, ssid: str) -> bool: - path = self.saved_connections.get(ssid) - if not path: - return False + self._scan_thread = threading.Thread(target=self._network_scanner, daemon=True) + self._scan_thread.start() - try: - nm_iface = await self._get_interface(NM, path, NM_CONNECTION_IFACE) - await nm_iface.call_delete() + self._state_thread = threading.Thread(target=self._monitor_state, daemon=True) + self._state_thread.start() - if self._current_connection_ssid == ssid: - self._current_connection_ssid = None + atexit.register(self.stop) - if ssid in self.saved_connections: - del self.saved_connections[ssid] + def set_callbacks(self, need_auth: Callable[[str], None], + activated: Callable[[], None] | None, + forgotten: Callable[[], None], + networks_updated: Callable[[list[Network]], None], + disconnected: Callable[[], None]): + self._need_auth = need_auth + self._activated = activated + self._forgotten = forgotten + self._networks_updated = networks_updated + self._disconnected = disconnected - for network in self.networks: - if network.ssid == ssid: - network.is_saved = False - network.is_connected = False + def _enqueue_callback(self, cb: Callable, *args): + self._callback_queue.append(lambda: cb(*args)) + + def process_callbacks(self): + # Call from UI thread to run any pending callbacks + to_run, self._callback_queue = self._callback_queue, [] + for cb in to_run: + cb() + + def set_active(self, active: bool): + self._active = active + + # Scan immediately if we haven't scanned in a while + if active and time.monotonic() - self._last_network_update > SCAN_PERIOD_SECONDS / 2: + self._last_network_update = 0.0 + + def _monitor_state(self): + device_path = self._wait_for_wifi_device() + if device_path is None: + return + + rule = MatchRule( + type="signal", + interface=NM_DEVICE_IFACE, + member="StateChanged", + path=device_path, + ) + + # Filter for StateChanged signal + self._conn_monitor.send_and_get_reply(message_bus.AddMatch(rule)) + + with self._conn_monitor.filter(rule, bufsize=SIGNAL_QUEUE_SIZE) as q: + while not self._exit: + if not self._active: + time.sleep(1) + continue + + # Block until a matching signal arrives + try: + msg = self._conn_monitor.recv_until_filtered(q, timeout=1) + except TimeoutError: + continue + + new_state, previous_state, change_reason = msg.body + + # BAD PASSWORD + if new_state == NMDeviceState.NEED_AUTH and change_reason == NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT and len(self._connecting_to_ssid): + self.forget_connection(self._connecting_to_ssid, block=True) + if self._need_auth is not None: + self._enqueue_callback(self._need_auth, self._connecting_to_ssid) + self._connecting_to_ssid = "" + + elif new_state == NMDeviceState.ACTIVATED: + if self._activated is not None: + self._update_networks() + self._enqueue_callback(self._activated) + self._connecting_to_ssid = "" + + elif new_state == NMDeviceState.DISCONNECTED and change_reason != NM_DEVICE_STATE_REASON_NEW_ACTIVATION: + self._connecting_to_ssid = "" + if self._disconnected is not None: + self._enqueue_callback(self._disconnected) + + def _network_scanner(self): + self._wait_for_wifi_device() + + while not self._exit: + if self._active: + if time.monotonic() - self._last_network_update > SCAN_PERIOD_SECONDS: + # Scan for networks every 10 seconds + # TODO: should update when scan is complete (PropertiesChanged), but this is more than good enough for now + self._update_networks() + self._request_scan() + self._last_network_update = time.monotonic() + time.sleep(1 / 2.) + + def _wait_for_wifi_device(self) -> str | None: + with self._lock: + device_path: str | None = None + while not self._exit: + device_path = self._get_wifi_device() + if device_path is not None: break + time.sleep(1) + return device_path - # Notify UI of forgotten connection - if self.callbacks.networks_updated: - self.callbacks.networks_updated(copy.deepcopy(self.networks)) + def _get_wifi_device(self) -> str | None: + if self._wifi_device is not None: + return self._wifi_device - return True - except DBusError as e: - cloudlog.error(f"Failed to delete connection for SSID: {ssid}. Error: {e}") - return False + device_paths = self._router_main.send_and_get_reply(new_method_call(self._nm, 'GetDevices')).body[0] + for device_path in device_paths: + dev_addr = DBusAddress(device_path, bus_name=NM, interface=NM_DEVICE_IFACE) + dev_type = self._router_main.send_and_get_reply(Properties(dev_addr).get('DeviceType')).body[0][1] - async def activate_connection(self, ssid: str) -> bool: - connection_path = self.saved_connections.get(ssid) - if not connection_path: - return False - try: - nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) - await nm_iface.call_activate_connection(connection_path, self.device_path, "/") - return True - except DBusError as e: - cloudlog.error(f"Failed to activate connection {ssid}: {str(e)}") - return False + if dev_type == NM_DEVICE_TYPE_WIFI: + self._wifi_device = device_path + break - async def connect_to_network(self, ssid: str, password: str = None, bssid: str = None, is_hidden: bool = False) -> None: - """Connect to a selected Wi-Fi network.""" - try: - self._current_connection_ssid = ssid + return self._wifi_device - if ssid in self.saved_connections: - # Forget old connection if new password provided - if password: - await self.forget_connection(ssid) - await asyncio.sleep(0.2) # NetworkManager delay - else: - # Just activate existing connection - await self.activate_connection(ssid) - return + def _get_connections(self) -> dict[str, str]: + settings_addr = DBusAddress(NM_SETTINGS_PATH, bus_name=NM, interface=NM_SETTINGS_IFACE) + known_connections = self._router_main.send_and_get_reply(new_method_call(settings_addr, 'ListConnections')).body[0] + + conns: dict[str, str] = {} + for conn_path in known_connections: + conn_addr = DBusAddress(conn_path, bus_name=NM, interface=NM_CONNECTION_IFACE) + reply = self._router_main.send_and_get_reply(new_method_call(conn_addr, "GetSettings")) + + # ignore connections removed during iteration (need auth, etc.) + if reply.header.message_type == MessageType.error: + cloudlog.warning(f"Failed to get connection properties for {conn_path}") + continue + + settings = reply.body[0] + if "802-11-wireless" in settings: + ssid = settings['802-11-wireless']['ssid'][1].decode("utf-8", "replace") + if ssid != "": + conns[ssid] = conn_path + return conns + + def connect_to_network(self, ssid: str, password: str): + def worker(): + # Clear all connections that may already exist to the network we are connecting to + self._connecting_to_ssid = ssid + self.forget_connection(ssid, block=True) + + is_hidden = False connection = { 'connection': { - 'type': Variant('s', '802-11-wireless'), - 'uuid': Variant('s', str(uuid.uuid4())), - 'id': Variant('s', f'openpilot connection {ssid}'), - 'autoconnect-retries': Variant('i', 0), + 'type': ('s', '802-11-wireless'), + 'uuid': ('s', str(uuid.uuid4())), + 'id': ('s', f'openpilot connection {ssid}'), + 'autoconnect-retries': ('i', 0), }, '802-11-wireless': { - 'ssid': Variant('ay', ssid.encode('utf-8')), - 'hidden': Variant('b', is_hidden), - 'mode': Variant('s', 'infrastructure'), + 'ssid': ('ay', ssid.encode("utf-8")), + 'hidden': ('b', is_hidden), + 'mode': ('s', 'infrastructure'), }, 'ipv4': { - 'method': Variant('s', 'auto'), - 'dns-priority': Variant('i', 600), + 'method': ('s', 'auto'), + 'dns-priority': ('i', 600), }, - 'ipv6': {'method': Variant('s', 'ignore')}, + 'ipv6': {'method': ('s', 'ignore')}, } - if bssid: - connection['802-11-wireless']['bssid'] = Variant('ay', bssid.encode('utf-8')) - if password: connection['802-11-wireless-security'] = { - 'key-mgmt': Variant('s', 'wpa-psk'), - 'auth-alg': Variant('s', 'open'), - 'psk': Variant('s', password), + 'key-mgmt': ('s', 'wpa-psk'), + 'auth-alg': ('s', 'open'), + 'psk': ('s', password), } - nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) - await nm_iface.call_add_and_activate_connection(connection, self.device_path, "/") - except Exception as e: - self._current_connection_ssid = None - cloudlog.error(f"Error connecting to network: {e}") - # Notify UI of failure - if self.callbacks.connection_failed: - self.callbacks.connection_failed(ssid, str(e)) + settings_addr = DBusAddress(NM_SETTINGS_PATH, bus_name=NM, interface=NM_SETTINGS_IFACE) + self._router_main.send_and_get_reply(new_method_call(settings_addr, 'AddConnection', 'a{sa{sv}}', (connection,))) + self.activate_connection(ssid, block=True) - def is_saved(self, ssid: str) -> bool: - return ssid in self.saved_connections + threading.Thread(target=worker, daemon=True).start() - async def _find_wifi_device(self) -> bool: - nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) - devices = await nm_iface.get_devices() + def forget_connection(self, ssid: str, block: bool = False): + def worker(): + conn_path = self._get_connections().get(ssid, None) + if conn_path is not None: + conn_addr = DBusAddress(conn_path, bus_name=NM, interface=NM_CONNECTION_IFACE) + self._router_main.send_and_get_reply(new_method_call(conn_addr, 'Delete')) - for device_path in devices: - device = await self.bus.introspect(NM, device_path) - device_proxy = self.bus.get_proxy_object(NM, device_path, device) - device_interface = device_proxy.get_interface(NM_DEVICE_IFACE) - device_type = await device_interface.get_device_type() # type: ignore[attr-defined] - if device_type == 2: # Wi-Fi device - self.device_path = device_path - self.device_proxy = device_proxy - return True + if self._forgotten is not None: + self._update_networks() + self._enqueue_callback(self._forgotten) - return False + if block: + worker() + else: + threading.Thread(target=worker, daemon=True).start() - async def add_tethering_connection(self, ssid: str, password: str = "12345678") -> bool: - """Create a WiFi tethering connection.""" - if len(password) < 8: - print("Tethering password must be at least 8 characters") - return False + def activate_connection(self, ssid: str, block: bool = False): + def worker(): + conn_path = self._get_connections().get(ssid, None) + if conn_path is not None: + if self._wifi_device is None: + cloudlog.warning("No WiFi device found") + return - try: - # First, check if a hotspot connection already exists - settings_iface = await self._get_interface(NM, NM_SETTINGS_PATH, NM_SETTINGS_IFACE) - connection_paths = await settings_iface.call_list_connections() + self._connecting_to_ssid = ssid + self._router_main.send(new_method_call(self._nm, 'ActivateConnection', 'ooo', + (conn_path, self._wifi_device, "/"))) + + if block: + worker() + else: + threading.Thread(target=worker, daemon=True).start() + + def _request_scan(self): + if self._wifi_device is None: + cloudlog.warning("No WiFi device found") + return + + wifi_addr = DBusAddress(self._wifi_device, bus_name=NM, interface=NM_WIRELESS_IFACE) + reply = self._router_main.send_and_get_reply(new_method_call(wifi_addr, 'RequestScan', 'a{sv}', ({},))) + + if reply.header.message_type == MessageType.error: + cloudlog.warning(f"Failed to request scan: {reply}") + + def _update_networks(self): + with self._lock: + if self._wifi_device is None: + cloudlog.warning("No WiFi device found") + return + + # returns '/' if no active AP + wifi_addr = DBusAddress(self._wifi_device, NM, interface=NM_WIRELESS_IFACE) + active_ap_path = self._router_main.send_and_get_reply(Properties(wifi_addr).get('ActiveAccessPoint')).body[0][1] + ap_paths = self._router_main.send_and_get_reply(new_method_call(wifi_addr, 'GetAllAccessPoints')).body[0] + + aps: dict[str, list[AccessPoint]] = {} + + for ap_path in ap_paths: + ap_addr = DBusAddress(ap_path, NM, interface=NM_ACCESS_POINT_IFACE) + ap_props = self._router_main.send_and_get_reply(Properties(ap_addr).get_all()) + + # some APs have been seen dropping off during iteration + if ap_props.header.message_type == MessageType.error: + cloudlog.warning(f"Failed to get AP properties for {ap_path}") + continue - # Look for an existing hotspot connection - for path in connection_paths: try: - settings = await self._get_connection_settings(path) - conn_type = settings.get('connection', {}).get('type', Variant('s', '')).value - wifi_mode = settings.get('802-11-wireless', {}).get('mode', Variant('s', '')).value + ap = AccessPoint.from_dbus(ap_props.body[0], ap_path, active_ap_path) + if ap.ssid == "": + continue - if conn_type == '802-11-wireless' and wifi_mode == 'ap': - # Extract the SSID to check - connection_ssid = self._extract_ssid(settings) - if connection_ssid == ssid: - return True - except DBusError: - continue + if ap.ssid not in aps: + aps[ap.ssid] = [] - connection = { - 'connection': { - 'id': Variant('s', 'Hotspot'), - 'uuid': Variant('s', str(uuid.uuid4())), - 'type': Variant('s', '802-11-wireless'), - 'interface-name': Variant('s', 'wlan0'), - 'autoconnect': Variant('b', False), - }, - '802-11-wireless': { - 'band': Variant('s', 'bg'), - 'mode': Variant('s', 'ap'), - 'ssid': Variant('ay', ssid.encode('utf-8')), - }, - '802-11-wireless-security': { - 'group': Variant('as', ['ccmp']), - 'key-mgmt': Variant('s', 'wpa-psk'), - 'pairwise': Variant('as', ['ccmp']), - 'proto': Variant('as', ['rsn']), - 'psk': Variant('s', password), - }, - 'ipv4': { - 'method': Variant('s', 'shared'), - 'address-data': Variant('aa{sv}', [{'address': Variant('s', TETHERING_IP_ADDRESS), 'prefix': Variant('u', 24)}]), - 'gateway': Variant('s', TETHERING_IP_ADDRESS), - 'never-default': Variant('b', True), - }, - 'ipv6': { - 'method': Variant('s', 'ignore'), - }, - } + aps[ap.ssid].append(ap) + except Exception: + # catch all for parsing errors + cloudlog.exception(f"Failed to parse AP properties for {ap_path}") - settings_iface = await self._get_interface(NM, NM_SETTINGS_PATH, NM_SETTINGS_IFACE) - new_connection = await settings_iface.call_add_connection(connection) - print(f"Added tethering connection with path: {new_connection}") - return True - except DBusError as e: - print(f"Failed to add tethering connection: {e}") - return False - except Exception as e: - print(f"Unexpected error adding tethering connection: {e}") - return False + known_connections = self._get_connections() + networks = [Network.from_dbus(ssid, ap_list, ssid in known_connections) for ssid, ap_list in aps.items()] + networks.sort(key=lambda n: (-n.is_connected, -n.strength, n.ssid.lower())) + self._networks = networks - async def get_tethering_password(self) -> str: - """Get the current tethering password.""" - try: - hotspot_path = self.saved_connections.get(self._tethering_ssid) - if hotspot_path: - conn_iface = await self._get_interface(NM, hotspot_path, NM_CONNECTION_IFACE) - secrets = await conn_iface.call_get_secrets('802-11-wireless-security') - if secrets and '802-11-wireless-security' in secrets: - psk = secrets.get('802-11-wireless-security', {}).get('psk', Variant('s', '')).value - return str(psk) if psk is not None else "" - return "" - except DBusError as e: - print(f"Failed to get tethering password: {e}") - return "" - except Exception as e: - print(f"Unexpected error getting tethering password: {e}") - return "" + if self._networks_updated is not None: + self._enqueue_callback(self._networks_updated, self._networks) - async def set_tethering_password(self, password: str) -> bool: - """Set the tethering password.""" - if len(password) < 8: - cloudlog.error("Tethering password must be at least 8 characters") - return False + def __del__(self): + self.stop() - try: - hotspot_path = self.saved_connections.get(self._tethering_ssid) - if not hotspot_path: - print("No hotspot connection found") - return False + def stop(self): + if not self._exit: + self._exit = True + self._scan_thread.join() + self._state_thread.join() - # Update the connection settings with new password - settings = await self._get_connection_settings(hotspot_path) - if '802-11-wireless-security' not in settings: - settings['802-11-wireless-security'] = {} - settings['802-11-wireless-security']['psk'] = Variant('s', password) - - # Apply changes - conn_iface = await self._get_interface(NM, hotspot_path, NM_CONNECTION_IFACE) - await conn_iface.call_update(settings) - - # Check if connection is active and restart if needed - is_active = False - nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) - active_connections = await nm_iface.get_active_connections() - - for conn_path in active_connections: - props_iface = await self._get_interface(NM, conn_path, NM_PROPERTIES_IFACE) - conn_id_path = await props_iface.call_get('org.freedesktop.NetworkManager.Connection.Active', 'Connection') - if conn_id_path.value == hotspot_path: - is_active = True - await nm_iface.call_deactivate_connection(conn_path) - break - - if is_active: - await nm_iface.call_activate_connection(hotspot_path, self.device_path, "/") - - print("Tethering password updated successfully") - return True - except DBusError as e: - print(f"Failed to set tethering password: {e}") - return False - except Exception as e: - print(f"Unexpected error setting tethering password: {e}") - return False - - async def is_tethering_active(self) -> bool: - """Check if tethering is active for the specified SSID.""" - try: - hotspot_path = self.saved_connections.get(self._tethering_ssid) - if not hotspot_path: - return False - - nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) - active_connections = await nm_iface.get_active_connections() - - for conn_path in active_connections: - props_iface = await self._get_interface(NM, conn_path, NM_PROPERTIES_IFACE) - conn_id_path = await props_iface.call_get('org.freedesktop.NetworkManager.Connection.Active', 'Connection') - - if conn_id_path.value == hotspot_path: - return True - - return False - except Exception: - return False - - async def _periodic_scan(self): - while self.running: - try: - await self._request_scan() - await asyncio.sleep(30) - except asyncio.CancelledError: - break - except DBusError as e: - cloudlog.error(f"Scan failed: {e}") - await asyncio.sleep(5) - - async def _setup_signals(self, device_path: str) -> None: - rules = [ - f"type='signal',interface='{NM_PROPERTIES_IFACE}',member='PropertiesChanged',path='{device_path}'", - f"type='signal',interface='{NM_DEVICE_IFACE}',member='StateChanged',path='{device_path}'", - f"type='signal',interface='{NM_SETTINGS_IFACE}',member='NewConnection',path='{NM_SETTINGS_PATH}'", - f"type='signal',interface='{NM_SETTINGS_IFACE}',member='ConnectionRemoved',path='{NM_SETTINGS_PATH}'", - ] - for rule in rules: - await self._add_match_rule(rule) - - # Set up signal handlers - self.device_proxy.get_interface(NM_PROPERTIES_IFACE).on_properties_changed(self._on_properties_changed) - self.device_proxy.get_interface(NM_DEVICE_IFACE).on_state_changed(self._on_state_changed) - - settings_iface = await self._get_interface(NM, NM_SETTINGS_PATH, NM_SETTINGS_IFACE) - settings_iface.on_new_connection(self._on_new_connection) - settings_iface.on_connection_removed(self._on_connection_removed) - - def _on_properties_changed(self, interface: str, changed: dict, invalidated: list): - if interface == NM_WIRELESS_IFACE and 'LastScan' in changed: - asyncio.create_task(self._refresh_networks()) - elif interface == NM_WIRELESS_IFACE and "ActiveAccessPoint" in changed: - new_ap_path = changed["ActiveAccessPoint"].value - if self.active_ap_path != new_ap_path: - self.active_ap_path = new_ap_path - - def _on_state_changed(self, new_state: int, old_state: int, reason: int): - if new_state == NMDeviceState.ACTIVATED: - if self.callbacks.activated: - self.callbacks.activated() - self._current_connection_ssid = None - asyncio.create_task(self._refresh_networks()) - elif new_state in (NMDeviceState.DISCONNECTED, NMDeviceState.NEED_AUTH): - for network in self.networks: - network.is_connected = False - - # BAD PASSWORD - if new_state == NMDeviceState.NEED_AUTH and reason == NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT and self.callbacks.need_auth: - if self._current_connection_ssid: - asyncio.create_task(self.forget_connection(self._current_connection_ssid)) - self.callbacks.need_auth(self._current_connection_ssid) - else: - # Try to find the network from active_ap_path - for network in self.networks: - if network.path == self.active_ap_path: - asyncio.create_task(self.forget_connection(network.ssid)) - self.callbacks.need_auth(network.ssid) - break - else: - # Couldn't identify the network that needs auth - cloudlog.error("Network needs authentication but couldn't identify which one") - - def _on_new_connection(self, path: str) -> None: - """Callback for NewConnection signal.""" - asyncio.create_task(self._add_saved_connection(path)) - - def _on_connection_removed(self, path: str) -> None: - """Callback for ConnectionRemoved signal.""" - for ssid, p in list(self.saved_connections.items()): - if path == p: - del self.saved_connections[ssid] - - if self.callbacks.forgotten: - self.callbacks.forgotten(ssid) - break - - async def _add_saved_connection(self, path: str) -> None: - """Add a new saved connection to the dictionary.""" - try: - settings = await self._get_connection_settings(path) - if ssid := self._extract_ssid(settings): - self.saved_connections[ssid] = path - except DBusError as e: - cloudlog.error(f"Failed to add connection {path}: {e}") - - def _extract_ssid(self, settings: dict) -> str | None: - """Extract SSID from connection settings.""" - ssid_variant = settings.get('802-11-wireless', {}).get('ssid', Variant('ay', b'')).value - return bytes(ssid_variant).decode('utf-8') if ssid_variant else None - - async def _add_match_rule(self, rule): - """Add a match rule on the bus.""" - reply = await self.bus.call( - Message( - message_type=MessageType.METHOD_CALL, - destination='org.freedesktop.DBus', - interface="org.freedesktop.DBus", - path='/org/freedesktop/DBus', - member='AddMatch', - signature='s', - body=[rule], - ) - ) - - assert reply.message_type == MessageType.METHOD_RETURN - return reply - - async def _refresh_networks(self): - """Get a list of available networks via NetworkManager.""" - wifi_iface = self.device_proxy.get_interface(NM_WIRELESS_IFACE) - access_points = await wifi_iface.get_access_points() - self.active_ap_path = await self.get_active_access_point() - network_dict = {} - for ap_path in access_points: - try: - props_iface = await self._get_interface(NM, ap_path, NM_PROPERTIES_IFACE) - properties = await props_iface.call_get_all('org.freedesktop.NetworkManager.AccessPoint') - ssid_variant = properties['Ssid'].value - ssid = bytes(ssid_variant).decode('utf-8') - if not ssid: - continue - - bssid = properties.get('HwAddress', Variant('s', '')).value - strength = properties['Strength'].value - flags = properties['Flags'].value - wpa_flags = properties['WpaFlags'].value - rsn_flags = properties['RsnFlags'].value - - # May be multiple access points for each SSID. Use first for ssid - # and security type, then update the rest using all APs - if ssid not in network_dict: - network_dict[ssid] = NetworkInfo( - ssid=ssid, - strength=0, - security_type=self._get_security_type(flags, wpa_flags, rsn_flags), - path="", - bssid="", - is_connected=False, - is_saved=ssid in self.saved_connections - ) - - existing_network = network_dict.get(ssid) - if existing_network.strength < strength: - existing_network.strength = strength - existing_network.path = ap_path - existing_network.bssid = bssid - if self.active_ap_path == ap_path: - existing_network.is_connected = self._current_connection_ssid != ssid - - except DBusError as e: - cloudlog.error(f"Error fetching networks: {e}") - except Exception as e: - cloudlog.error({e}) - - self.networks = sorted( - network_dict.values(), - key=lambda network: ( - not network.is_connected, - -network.strength, # Higher signal strength first - network.ssid.lower(), - ), - ) - - if self.callbacks.networks_updated: - self.callbacks.networks_updated(copy.deepcopy(self.networks)) - - async def _get_connection_settings(self, path): - """Fetch connection settings for a specific connection path.""" - try: - settings = await self._get_interface(NM, path, NM_CONNECTION_IFACE) - return await settings.call_get_settings() - except DBusError as e: - cloudlog.error(f"Failed to get settings for {path}: {str(e)}") - return {} - - async def _process_chunk(self, paths_chunk): - """Process a chunk of connection paths.""" - tasks = [self._get_connection_settings(path) for path in paths_chunk] - return await asyncio.gather(*tasks, return_exceptions=True) - - async def _get_saved_connections(self) -> dict[str, str]: - try: - settings_iface = await self._get_interface(NM, NM_SETTINGS_PATH, NM_SETTINGS_IFACE) - connection_paths = await settings_iface.call_list_connections() - saved_ssids: dict[str, str] = {} - batch_size = 20 - for i in range(0, len(connection_paths), batch_size): - chunk = connection_paths[i : i + batch_size] - results = await self._process_chunk(chunk) - for path, config in zip(chunk, results, strict=True): - if isinstance(config, dict) and '802-11-wireless' in config: - if ssid := self._extract_ssid(config): - saved_ssids[ssid] = path - return saved_ssids - except DBusError as e: - cloudlog.error(f"Error fetching saved connections: {str(e)}") - return {} - - async def _get_interface(self, bus_name: str, path: str, name: str): - introspection = await self.bus.introspect(bus_name, path) - proxy = self.bus.get_proxy_object(bus_name, path, introspection) - return proxy.get_interface(name) - - def _get_security_type(self, flags: int, wpa_flags: int, rsn_flags: int) -> SecurityType: - """Determine the security type based on flags.""" - if flags == 0 and not (wpa_flags or rsn_flags): - return SecurityType.OPEN - if rsn_flags & 0x200: # SAE (WPA3 Personal) - # TODO: support WPA3 - return SecurityType.UNSUPPORTED - if rsn_flags: # RSN indicates WPA2 or higher - return SecurityType.WPA2 - if wpa_flags: # WPA flags indicate WPA - return SecurityType.WPA - return SecurityType.UNSUPPORTED - - -class WifiManagerWrapper: - def __init__(self): - self._manager: WifiManager | None = None - self._callbacks: WifiManagerCallbacks = WifiManagerCallbacks() - - self._thread = threading.Thread(target=self._run, daemon=True) - self._loop: asyncio.EventLoop | None = None - self._running = False - - def set_callbacks(self, callbacks: WifiManagerCallbacks): - self._callbacks = callbacks - - def start(self) -> None: - if not self._running: - self._thread.start() - while self._thread is not None and not self._running: - time.sleep(0.1) - - def _run(self): - self._loop = asyncio.new_event_loop() - asyncio.set_event_loop(self._loop) - - try: - self._manager = WifiManager(self._callbacks) - self._running = True - self._loop.run_forever() - except Exception as e: - cloudlog.error(f"Error in WifiManagerWrapper thread: {e}") - finally: - if self._loop.is_running(): - self._loop.stop() - self._running = False - - def shutdown(self) -> None: - if self._running: - if self._manager is not None and self._loop: - shutdown_future = asyncio.run_coroutine_threadsafe(self._manager.shutdown(), self._loop) - shutdown_future.result(timeout=3.0) - - if self._loop and self._loop.is_running(): - self._loop.call_soon_threadsafe(self._loop.stop) - if self._thread and self._thread.is_alive(): - self._thread.join(timeout=2.0) - self._running = False - - def is_saved(self, ssid: str) -> bool: - """Check if a network is saved.""" - return self._run_coroutine_sync(lambda manager: manager.is_saved(ssid), default=False) - - def connect(self): - """Connect to DBus and start Wi-Fi scanning.""" - if not self._manager: - return - self._run_coroutine(self._manager.connect()) - - def forget_connection(self, ssid: str): - """Forget a saved Wi-Fi connection.""" - if not self._manager: - return - self._run_coroutine(self._manager.forget_connection(ssid)) - - def activate_connection(self, ssid: str): - """Activate an existing Wi-Fi connection.""" - if not self._manager: - return - self._run_coroutine(self._manager.activate_connection(ssid)) - - def connect_to_network(self, ssid: str, password: str = None, bssid: str = None, is_hidden: bool = False): - """Connect to a Wi-Fi network.""" - if not self._manager: - return - self._run_coroutine(self._manager.connect_to_network(ssid, password, bssid, is_hidden)) - - def _run_coroutine(self, coro): - """Run a coroutine in the async thread.""" - if not self._running or not self._loop: - cloudlog.error("WifiManager thread is not running") - return - asyncio.run_coroutine_threadsafe(coro, self._loop) - - def _run_coroutine_sync(self, func: Callable[[WifiManager], T], default: T) -> T: - """Run a function synchronously in the async thread.""" - if not self._running or not self._loop or not self._manager: - return default - future = concurrent.futures.Future[T]() - - def wrapper(manager: WifiManager) -> None: - try: - future.set_result(func(manager)) - except Exception as e: - future.set_exception(e) - - try: - self._loop.call_soon_threadsafe(wrapper, self._manager) - return future.result(timeout=1.0) - except Exception as e: - cloudlog.error(f"WifiManagerWrapper property access failed: {e}") - return default + self._router_main.close() + self._router_main.conn.close() + self._conn_monitor.close() diff --git a/system/ui/reset.py b/system/ui/reset.py index b1bf5a6b6a..a5cf1731dc 100755 --- a/system/ui/reset.py +++ b/system/ui/reset.py @@ -13,7 +13,6 @@ from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.button import Button, ButtonStyle from openpilot.system.ui.widgets.label import gui_label, gui_text_box -NVME = "/dev/nvme0n1" USERDATA = "/dev/disk/by-partlabel/userdata" TIMEOUT = 3*60 @@ -49,10 +48,6 @@ class Reset(Widget): if PC: return - # Best effort to wipe NVME - os.system(f"sudo umount {NVME}") - os.system(f"yes | sudo mkfs.ext4 {NVME}") - # Removing data and formatting rm = os.system("sudo rm -rf /data/*") os.system(f"sudo umount {USERDATA}") diff --git a/system/ui/setup.py b/system/ui/setup.py index 800ca7662c..a985e783be 100755 --- a/system/ui/setup.py +++ b/system/ui/setup.py @@ -19,7 +19,7 @@ from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.button import Button, ButtonStyle, ButtonRadio from openpilot.system.ui.widgets.keyboard import Keyboard from openpilot.system.ui.widgets.label import Label, TextAlignment -from openpilot.system.ui.widgets.network import WifiManagerUI, WifiManagerWrapper +from openpilot.system.ui.widgets.network import WifiManagerUI, WifiManager NetworkType = log.DeviceState.NetworkType @@ -72,8 +72,7 @@ class Setup(Widget): self.download_url = "" self.download_progress = 0 self.download_thread = None - self.wifi_manager = WifiManagerWrapper() - self.wifi_ui = WifiManagerUI(self.wifi_manager) + self.wifi_ui = WifiManagerUI(WifiManager()) self.keyboard = Keyboard() self.selected_radio = None self.warning = gui_app.texture("icons/warning.png", 150, 150) diff --git a/system/ui/updater.py b/system/ui/updater.py index 31799d3628..b3cdc82cf5 100755 --- a/system/ui/updater.py +++ b/system/ui/updater.py @@ -7,7 +7,7 @@ from enum import IntEnum from openpilot.system.hardware import HARDWARE from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.wifi_manager import WifiManagerWrapper +from openpilot.system.ui.lib.wifi_manager import WifiManager from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.button import gui_button, ButtonStyle from openpilot.system.ui.widgets.label import gui_text_box, gui_label @@ -43,8 +43,7 @@ class Updater(Widget): self.show_reboot_button = False self.process = None self.update_thread = None - self.wifi_manager = WifiManagerWrapper() - self.wifi_manager_ui = WifiManagerUI(self.wifi_manager) + self.wifi_manager_ui = WifiManagerUI(WifiManager()) def install_update(self): self.current_screen = Screen.PROGRESS diff --git a/system/ui/widgets/__init__.py b/system/ui/widgets/__init__.py index d45f48ac38..cca2cec7ad 100644 --- a/system/ui/widgets/__init__.py +++ b/system/ui/widgets/__init__.py @@ -129,3 +129,10 @@ class Widget(abc.ABC): def _handle_mouse_release(self, mouse_pos: MousePos) -> bool: """Optionally handle mouse release events.""" return False + + def show_event(self): + """Optionally handle show event. Parent must manually call this""" + + def hide_event(self): + """Optionally handle hide event. Parent must manually call this""" + diff --git a/system/ui/widgets/list_view.py b/system/ui/widgets/list_view.py index e871eef0a1..a8d81a8ba2 100644 --- a/system/ui/widgets/list_view.py +++ b/system/ui/widgets/list_view.py @@ -213,6 +213,7 @@ class ListItem(Widget): self.set_rect(rl.Rectangle(0, 0, ITEM_BASE_WIDTH, ITEM_BASE_HEIGHT)) self._font = gui_app.font(FontWeight.NORMAL) + self._icon_texture = gui_app.texture(os.path.join("icons", self.icon), ICON_SIZE, ICON_SIZE) if self.icon else None # Cached properties for performance self._prev_max_width: int = 0 @@ -261,8 +262,7 @@ class ListItem(Widget): if self.title: # Draw icon if present if self.icon: - icon_texture = gui_app.texture(os.path.join("icons", self.icon), ICON_SIZE, ICON_SIZE) - rl.draw_texture(icon_texture, int(content_x), int(self._rect.y + (ITEM_BASE_HEIGHT - icon_texture.width) // 2), rl.WHITE) + rl.draw_texture(self._icon_texture, int(content_x), int(self._rect.y + (ITEM_BASE_HEIGHT - self._icon_texture.width) // 2), rl.WHITE) text_x += ICON_SIZE + ITEM_PADDING # Draw main text diff --git a/system/ui/widgets/network.py b/system/ui/widgets/network.py index 0bb759a919..3e6317a49c 100644 --- a/system/ui/widgets/network.py +++ b/system/ui/widgets/network.py @@ -1,12 +1,11 @@ from enum import IntEnum from functools import partial -from threading import Lock from typing import cast import pyray as rl from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel -from openpilot.system.ui.lib.wifi_manager import NetworkInfo, WifiManagerCallbacks, WifiManagerWrapper, SecurityType +from openpilot.system.ui.lib.wifi_manager import WifiManager, SecurityType, Network from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.button import ButtonStyle, Button from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog @@ -36,52 +35,58 @@ class UIState(IntEnum): class WifiManagerUI(Widget): - def __init__(self, wifi_manager: WifiManagerWrapper): + def __init__(self, wifi_manager: WifiManager): super().__init__() + self.wifi_manager = wifi_manager self.state: UIState = UIState.IDLE - self._state_network: NetworkInfo | None = None # for CONNECTING / NEEDS_AUTH / SHOW_FORGET_CONFIRM / FORGETTING + self._state_network: Network | None = None # for CONNECTING / NEEDS_AUTH / SHOW_FORGET_CONFIRM / FORGETTING self._password_retry: bool = False # for NEEDS_AUTH self.btn_width: int = 200 self.scroll_panel = GuiScrollPanel() self.keyboard = Keyboard(max_text_size=MAX_PASSWORD_LENGTH, min_text_size=MIN_PASSWORD_LENGTH, show_password_toggle=True) + self._load_icons() - self._networks: list[NetworkInfo] = [] + self._networks: list[Network] = [] self._networks_buttons: dict[str, Button] = {} self._forget_networks_buttons: dict[str, Button] = {} - self._lock = Lock() - self.wifi_manager = wifi_manager self._confirm_dialog = ConfirmDialog("", "Forget", "Cancel") - self.wifi_manager.set_callbacks( - WifiManagerCallbacks( - need_auth=self._on_need_auth, - activated=self._on_activated, - forgotten=self._on_forgotten, - networks_updated=self._on_network_updated, - connection_failed=self._on_connection_failed - ) - ) - self.wifi_manager.start() - self.wifi_manager.connect() + self.wifi_manager.set_callbacks(need_auth=self._on_need_auth, + activated=self._on_activated, + forgotten=self._on_forgotten, + networks_updated=self._on_network_updated, + disconnected=self._on_disconnected) + + def show_event(self): + # start/stop scanning when widget is visible + self.wifi_manager.set_active(True) + + def hide_event(self): + self.wifi_manager.set_active(False) + + def _load_icons(self): + for icon in STRENGTH_ICONS + ["icons/checkmark.png", "icons/circled_slash.png", "icons/lock_closed.png"]: + gui_app.texture(icon, ICON_SIZE, ICON_SIZE) def _render(self, rect: rl.Rectangle): - with self._lock: - if not self._networks: - gui_label(rect, "Scanning Wi-Fi networks...", 72, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - return + self.wifi_manager.process_callbacks() - if self.state == UIState.NEEDS_AUTH and self._state_network: - self.keyboard.set_title("Wrong password" if self._password_retry else "Enter password", f"for {self._state_network.ssid}") - self.keyboard.reset() - gui_app.set_modal_overlay(self.keyboard, lambda result: self._on_password_entered(cast(NetworkInfo, self._state_network), result)) - elif self.state == UIState.SHOW_FORGET_CONFIRM and self._state_network: - self._confirm_dialog.set_text(f'Forget Wi-Fi Network "{self._state_network.ssid}"?') - self._confirm_dialog.reset() - gui_app.set_modal_overlay(self._confirm_dialog, callback=lambda result: self.on_forgot_confirm_finished(self._state_network, result)) - else: - self._draw_network_list(rect) + if not self._networks: + gui_label(rect, "Scanning Wi-Fi networks...", 72, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) + return - def _on_password_entered(self, network: NetworkInfo, result: int): + if self.state == UIState.NEEDS_AUTH and self._state_network: + self.keyboard.set_title("Wrong password" if self._password_retry else "Enter password", f"for {self._state_network.ssid}") + self.keyboard.reset() + gui_app.set_modal_overlay(self.keyboard, lambda result: self._on_password_entered(cast(Network, self._state_network), result)) + elif self.state == UIState.SHOW_FORGET_CONFIRM and self._state_network: + self._confirm_dialog.set_text(f'Forget Wi-Fi Network "{self._state_network.ssid}"?') + self._confirm_dialog.reset() + gui_app.set_modal_overlay(self._confirm_dialog, callback=lambda result: self.on_forgot_confirm_finished(self._state_network, result)) + else: + self._draw_network_list(rect) + + def _on_password_entered(self, network: Network, result: int): if result == 1: password = self.keyboard.text self.keyboard.clear() @@ -116,7 +121,7 @@ class WifiManagerUI(Widget): rl.end_scissor_mode() - def _draw_network_item(self, rect, network: NetworkInfo, clicked: bool): + def _draw_network_item(self, rect, network: Network, clicked: bool): spacing = 50 ssid_rect = rl.Rectangle(rect.x, rect.y, rect.width - self.btn_width * 2, ITEM_HEIGHT) signal_icon_rect = rl.Rectangle(rect.x + rect.width - ICON_SIZE, rect.y + (ITEM_HEIGHT - ICON_SIZE) / 2, ICON_SIZE, ICON_SIZE) @@ -169,10 +174,10 @@ class WifiManagerUI(Widget): self.state = UIState.SHOW_FORGET_CONFIRM self._state_network = network - def _draw_status_icon(self, rect, network: NetworkInfo): + def _draw_status_icon(self, rect, network: Network): """Draw the status icon based on network's connection state""" icon_file = None - if network.is_connected: + if network.is_connected and self.state != UIState.CONNECTING: icon_file = "icons/checkmark.png" elif network.security_type == SecurityType.UNSUPPORTED: icon_file = "icons/circled_slash.png" @@ -186,12 +191,12 @@ class WifiManagerUI(Widget): icon_rect = rl.Vector2(rect.x, rect.y + (ICON_SIZE - texture.height) / 2) rl.draw_texture_v(texture, icon_rect, rl.WHITE) - def _draw_signal_strength_icon(self, rect: rl.Rectangle, network: NetworkInfo): + def _draw_signal_strength_icon(self, rect: rl.Rectangle, network: Network): """Draw the Wi-Fi signal strength icon based on network's signal strength""" strength_level = max(0, min(3, round(network.strength / 33.0))) rl.draw_texture_v(gui_app.texture(STRENGTH_ICONS[strength_level], ICON_SIZE, ICON_SIZE), rl.Vector2(rect.x, rect.y), rl.WHITE) - def connect_to_network(self, network: NetworkInfo, password=''): + def connect_to_network(self, network: Network, password=''): self.state = UIState.CONNECTING self._state_network = network if network.is_saved and not password: @@ -199,54 +204,46 @@ class WifiManagerUI(Widget): else: self.wifi_manager.connect_to_network(network.ssid, password) - def forget_network(self, network: NetworkInfo): + def forget_network(self, network: Network): self.state = UIState.FORGETTING self._state_network = network - network.is_saved = False self.wifi_manager.forget_connection(network.ssid) - def _on_network_updated(self, networks: list[NetworkInfo]): - with self._lock: - self._networks = networks - for n in self._networks: - self._networks_buttons[n.ssid] = Button(n.ssid, partial(self._networks_buttons_callback, n), font_size=55, text_alignment=TextAlignment.LEFT, - button_style=ButtonStyle.NO_EFFECT) - self._forget_networks_buttons[n.ssid] = Button("Forget", partial(self._forget_networks_buttons_callback, n), button_style=ButtonStyle.FORGET_WIFI, - font_size=45) + def _on_network_updated(self, networks: list[Network]): + self._networks = networks + for n in self._networks: + self._networks_buttons[n.ssid] = Button(n.ssid, partial(self._networks_buttons_callback, n), font_size=55, text_alignment=TextAlignment.LEFT, + button_style=ButtonStyle.NO_EFFECT) + self._forget_networks_buttons[n.ssid] = Button("Forget", partial(self._forget_networks_buttons_callback, n), button_style=ButtonStyle.FORGET_WIFI, + font_size=45) def _on_need_auth(self, ssid): - with self._lock: - network = next((n for n in self._networks if n.ssid == ssid), None) - if network: - self.state = UIState.NEEDS_AUTH - self._state_network = network - self._password_retry = True + network = next((n for n in self._networks if n.ssid == ssid), None) + if network: + self.state = UIState.NEEDS_AUTH + self._state_network = network + self._password_retry = True def _on_activated(self): - with self._lock: - if self.state == UIState.CONNECTING: - self.state = UIState.IDLE + if self.state == UIState.CONNECTING: + self.state = UIState.IDLE - def _on_forgotten(self, ssid): - with self._lock: - if self.state == UIState.FORGETTING: - self.state = UIState.IDLE + def _on_forgotten(self): + if self.state == UIState.FORGETTING: + self.state = UIState.IDLE - def _on_connection_failed(self, ssid: str, error: str): - with self._lock: - if self.state == UIState.CONNECTING: - self.state = UIState.IDLE + def _on_disconnected(self): + if self.state == UIState.CONNECTING: + self.state = UIState.IDLE def main(): gui_app.init_window("Wi-Fi Manager") - wifi_manager = WifiManagerWrapper() - wifi_ui = WifiManagerUI(wifi_manager) + wifi_ui = WifiManagerUI(WifiManager()) for _ in gui_app.render(): wifi_ui.render(rl.Rectangle(50, 50, gui_app.width - 100, gui_app.height - 100)) - wifi_manager.shutdown() gui_app.close() diff --git a/system/version.py b/system/version.py index b0b6553f04..97a16bb651 100755 --- a/system/version.py +++ b/system/version.py @@ -21,6 +21,9 @@ SP_BRANCH_MIGRATIONS = { ("tici", "dev-c3-new"): "staging-tici", ("tici", "master"): "master-tici", ("tici", "master-dev-c3-new"): "master-tici", + ("tizi", "staging-c3-new"): "staging", + ("tizi", "dev-c3-new"): "dev", + ("tizi", "master-dev-c3-new"): "master-dev", } BUILD_METADATA_FILENAME = "build.json" @@ -131,7 +134,7 @@ class BuildMetadata: @property def development_channel(self) -> bool: - return self.channel.startswith("dev-") or self.channel.endswith("-prebuilt") + return self.channel == "dev" or self.channel.startswith("dev-") or self.channel.endswith("-prebuilt") @property def channel_type(self) -> str: @@ -139,11 +142,11 @@ class BuildMetadata: return "tici" elif self.development_channel: return "development" - elif self.channel.startswith("staging-"): + elif self.tested_channel: return "staging" elif self.master_channel: return "master" - elif self.tested_channel: + elif self.release_channel: return "release" else: return "feature" diff --git a/third_party/SConscript b/third_party/SConscript index 507c17c4a5..3a7497d162 100644 --- a/third_party/SConscript +++ b/third_party/SConscript @@ -1,4 +1,3 @@ Import('env') env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call']) -env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/third_party/kaitai/custom_decoder.h b/third_party/kaitai/custom_decoder.h deleted file mode 100644 index 6da7f5fd23..0000000000 --- a/third_party/kaitai/custom_decoder.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef KAITAI_CUSTOM_DECODER_H -#define KAITAI_CUSTOM_DECODER_H - -#include - -namespace kaitai { - -class custom_decoder { -public: - virtual ~custom_decoder() {}; - virtual std::string decode(std::string src) = 0; -}; - -} - -#endif diff --git a/third_party/kaitai/exceptions.h b/third_party/kaitai/exceptions.h deleted file mode 100644 index 5c09c4672b..0000000000 --- a/third_party/kaitai/exceptions.h +++ /dev/null @@ -1,189 +0,0 @@ -#ifndef KAITAI_EXCEPTIONS_H -#define KAITAI_EXCEPTIONS_H - -#include - -#include -#include - -// We need to use "noexcept" in virtual destructor of our exceptions -// subclasses. Different compilers have different ideas on how to -// achieve that: C++98 compilers prefer `throw()`, C++11 and later -// use `noexcept`. We define KS_NOEXCEPT macro for that. - -#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900) -#define KS_NOEXCEPT noexcept -#else -#define KS_NOEXCEPT throw() -#endif - -namespace kaitai { - -/** - * Common ancestor for all error originating from Kaitai Struct usage. - * Stores KSY source path, pointing to an element supposedly guilty of - * an error. - */ -class kstruct_error: public std::runtime_error { -public: - kstruct_error(const std::string what, const std::string src_path): - std::runtime_error(src_path + ": " + what), - m_src_path(src_path) - { - } - - virtual ~kstruct_error() KS_NOEXCEPT {}; - -protected: - const std::string m_src_path; -}; - -/** - * Error that occurs when default endianness should be decided with - * a switch, but nothing matches (although using endianness expression - * implies that there should be some positive result). - */ -class undecided_endianness_error: public kstruct_error { -public: - undecided_endianness_error(const std::string src_path): - kstruct_error("unable to decide on endianness for a type", src_path) - { - } - - virtual ~undecided_endianness_error() KS_NOEXCEPT {}; -}; - -/** - * Common ancestor for all validation failures. Stores pointer to - * KaitaiStream IO object which was involved in an error. - */ -class validation_failed_error: public kstruct_error { -public: - validation_failed_error(const std::string what, kstream* io, const std::string src_path): - kstruct_error("at pos " + kstream::to_string(static_cast(io->pos())) + ": validation failed: " + what, src_path), - m_io(io) - { - } - -// "at pos #{io.pos}: validation failed: #{msg}" - - virtual ~validation_failed_error() KS_NOEXCEPT {}; - -protected: - kstream* m_io; -}; - -/** - * Signals validation failure: we required "actual" value to be equal to - * "expected", but it turned out that it's not. - */ -template -class validation_not_equal_error: public validation_failed_error { -public: - validation_not_equal_error(const T& expected, const T& actual, kstream* io, const std::string src_path): - validation_failed_error("not equal", io, src_path), - m_expected(expected), - m_actual(actual) - { - } - - // "not equal, expected #{expected.inspect}, but got #{actual.inspect}" - - virtual ~validation_not_equal_error() KS_NOEXCEPT {}; - -protected: - const T& m_expected; - const T& m_actual; -}; - -/** - * Signals validation failure: we required "actual" value to be greater - * than or equal to "min", but it turned out that it's not. - */ -template -class validation_less_than_error: public validation_failed_error { -public: - validation_less_than_error(const T& min, const T& actual, kstream* io, const std::string src_path): - validation_failed_error("not in range", io, src_path), - m_min(min), - m_actual(actual) - { - } - - // "not in range, min #{min.inspect}, but got #{actual.inspect}" - - virtual ~validation_less_than_error() KS_NOEXCEPT {}; - -protected: - const T& m_min; - const T& m_actual; -}; - -/** - * Signals validation failure: we required "actual" value to be less - * than or equal to "max", but it turned out that it's not. - */ -template -class validation_greater_than_error: public validation_failed_error { -public: - validation_greater_than_error(const T& max, const T& actual, kstream* io, const std::string src_path): - validation_failed_error("not in range", io, src_path), - m_max(max), - m_actual(actual) - { - } - - // "not in range, max #{max.inspect}, but got #{actual.inspect}" - - virtual ~validation_greater_than_error() KS_NOEXCEPT {}; - -protected: - const T& m_max; - const T& m_actual; -}; - -/** - * Signals validation failure: we required "actual" value to be from - * the list, but it turned out that it's not. - */ -template -class validation_not_any_of_error: public validation_failed_error { -public: - validation_not_any_of_error(const T& actual, kstream* io, const std::string src_path): - validation_failed_error("not any of the list", io, src_path), - m_actual(actual) - { - } - - // "not any of the list, got #{actual.inspect}" - - virtual ~validation_not_any_of_error() KS_NOEXCEPT {}; - -protected: - const T& m_actual; -}; - -/** - * Signals validation failure: we required "actual" value to match - * the expression, but it turned out that it doesn't. - */ -template -class validation_expr_error: public validation_failed_error { -public: - validation_expr_error(const T& actual, kstream* io, const std::string src_path): - validation_failed_error("not matching the expression", io, src_path), - m_actual(actual) - { - } - - // "not matching the expression, got #{actual.inspect}" - - virtual ~validation_expr_error() KS_NOEXCEPT {}; - -protected: - const T& m_actual; -}; - -} - -#endif diff --git a/third_party/kaitai/kaitaistream.cpp b/third_party/kaitai/kaitaistream.cpp deleted file mode 100644 index d82ddb7e82..0000000000 --- a/third_party/kaitai/kaitaistream.cpp +++ /dev/null @@ -1,689 +0,0 @@ -#include - -#if defined(__APPLE__) -#include -#include -#define bswap_16(x) OSSwapInt16(x) -#define bswap_32(x) OSSwapInt32(x) -#define bswap_64(x) OSSwapInt64(x) -#define __BYTE_ORDER BYTE_ORDER -#define __BIG_ENDIAN BIG_ENDIAN -#define __LITTLE_ENDIAN LITTLE_ENDIAN -#elif defined(_MSC_VER) // !__APPLE__ -#include -#define __LITTLE_ENDIAN 1234 -#define __BIG_ENDIAN 4321 -#define __BYTE_ORDER __LITTLE_ENDIAN -#define bswap_16(x) _byteswap_ushort(x) -#define bswap_32(x) _byteswap_ulong(x) -#define bswap_64(x) _byteswap_uint64(x) -#else // !__APPLE__ or !_MSC_VER -#include -#include -#endif - -#include -#include -#include - -kaitai::kstream::kstream(std::istream* io) { - m_io = io; - init(); -} - -kaitai::kstream::kstream(std::string& data): m_io_str(data) { - m_io = &m_io_str; - init(); -} - -void kaitai::kstream::init() { - exceptions_enable(); - align_to_byte(); -} - -void kaitai::kstream::close() { - // m_io->close(); -} - -void kaitai::kstream::exceptions_enable() const { - m_io->exceptions( - std::istream::eofbit | - std::istream::failbit | - std::istream::badbit - ); -} - -// ======================================================================== -// Stream positioning -// ======================================================================== - -bool kaitai::kstream::is_eof() const { - if (m_bits_left > 0) { - return false; - } - char t; - m_io->exceptions( - std::istream::badbit - ); - m_io->get(t); - if (m_io->eof()) { - m_io->clear(); - exceptions_enable(); - return true; - } else { - m_io->unget(); - exceptions_enable(); - return false; - } -} - -void kaitai::kstream::seek(uint64_t pos) { - m_io->seekg(pos); -} - -uint64_t kaitai::kstream::pos() { - return m_io->tellg(); -} - -uint64_t kaitai::kstream::size() { - std::iostream::pos_type cur_pos = m_io->tellg(); - m_io->seekg(0, std::ios::end); - std::iostream::pos_type len = m_io->tellg(); - m_io->seekg(cur_pos); - return len; -} - -// ======================================================================== -// Integer numbers -// ======================================================================== - -// ------------------------------------------------------------------------ -// Signed -// ------------------------------------------------------------------------ - -int8_t kaitai::kstream::read_s1() { - char t; - m_io->get(t); - return t; -} - -// ........................................................................ -// Big-endian -// ........................................................................ - -int16_t kaitai::kstream::read_s2be() { - int16_t t; - m_io->read(reinterpret_cast(&t), 2); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_16(t); -#endif - return t; -} - -int32_t kaitai::kstream::read_s4be() { - int32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_32(t); -#endif - return t; -} - -int64_t kaitai::kstream::read_s8be() { - int64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_64(t); -#endif - return t; -} - -// ........................................................................ -// Little-endian -// ........................................................................ - -int16_t kaitai::kstream::read_s2le() { - int16_t t; - m_io->read(reinterpret_cast(&t), 2); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_16(t); -#endif - return t; -} - -int32_t kaitai::kstream::read_s4le() { - int32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_32(t); -#endif - return t; -} - -int64_t kaitai::kstream::read_s8le() { - int64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_64(t); -#endif - return t; -} - -// ------------------------------------------------------------------------ -// Unsigned -// ------------------------------------------------------------------------ - -uint8_t kaitai::kstream::read_u1() { - char t; - m_io->get(t); - return t; -} - -// ........................................................................ -// Big-endian -// ........................................................................ - -uint16_t kaitai::kstream::read_u2be() { - uint16_t t; - m_io->read(reinterpret_cast(&t), 2); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_16(t); -#endif - return t; -} - -uint32_t kaitai::kstream::read_u4be() { - uint32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_32(t); -#endif - return t; -} - -uint64_t kaitai::kstream::read_u8be() { - uint64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_64(t); -#endif - return t; -} - -// ........................................................................ -// Little-endian -// ........................................................................ - -uint16_t kaitai::kstream::read_u2le() { - uint16_t t; - m_io->read(reinterpret_cast(&t), 2); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_16(t); -#endif - return t; -} - -uint32_t kaitai::kstream::read_u4le() { - uint32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_32(t); -#endif - return t; -} - -uint64_t kaitai::kstream::read_u8le() { - uint64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_64(t); -#endif - return t; -} - -// ======================================================================== -// Floating point numbers -// ======================================================================== - -// ........................................................................ -// Big-endian -// ........................................................................ - -float kaitai::kstream::read_f4be() { - uint32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_32(t); -#endif - return reinterpret_cast(t); -} - -double kaitai::kstream::read_f8be() { - uint64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __LITTLE_ENDIAN - t = bswap_64(t); -#endif - return reinterpret_cast(t); -} - -// ........................................................................ -// Little-endian -// ........................................................................ - -float kaitai::kstream::read_f4le() { - uint32_t t; - m_io->read(reinterpret_cast(&t), 4); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_32(t); -#endif - return reinterpret_cast(t); -} - -double kaitai::kstream::read_f8le() { - uint64_t t; - m_io->read(reinterpret_cast(&t), 8); -#if __BYTE_ORDER == __BIG_ENDIAN - t = bswap_64(t); -#endif - return reinterpret_cast(t); -} - -// ======================================================================== -// Unaligned bit values -// ======================================================================== - -void kaitai::kstream::align_to_byte() { - m_bits_left = 0; - m_bits = 0; -} - -uint64_t kaitai::kstream::read_bits_int_be(int n) { - int bits_needed = n - m_bits_left; - if (bits_needed > 0) { - // 1 bit => 1 byte - // 8 bits => 1 byte - // 9 bits => 2 bytes - int bytes_needed = ((bits_needed - 1) / 8) + 1; - if (bytes_needed > 8) - throw std::runtime_error("read_bits_int: more than 8 bytes requested"); - char buf[8]; - m_io->read(buf, bytes_needed); - for (int i = 0; i < bytes_needed; i++) { - uint8_t b = buf[i]; - m_bits <<= 8; - m_bits |= b; - m_bits_left += 8; - } - } - - // raw mask with required number of 1s, starting from lowest bit - uint64_t mask = get_mask_ones(n); - // shift mask to align with highest bits available in @bits - int shift_bits = m_bits_left - n; - mask <<= shift_bits; - // derive reading result - uint64_t res = (m_bits & mask) >> shift_bits; - // clear top bits that we've just read => AND with 1s - m_bits_left -= n; - mask = get_mask_ones(m_bits_left); - m_bits &= mask; - - return res; -} - -// Deprecated, use read_bits_int_be() instead. -uint64_t kaitai::kstream::read_bits_int(int n) { - return read_bits_int_be(n); -} - -uint64_t kaitai::kstream::read_bits_int_le(int n) { - int bits_needed = n - m_bits_left; - if (bits_needed > 0) { - // 1 bit => 1 byte - // 8 bits => 1 byte - // 9 bits => 2 bytes - int bytes_needed = ((bits_needed - 1) / 8) + 1; - if (bytes_needed > 8) - throw std::runtime_error("read_bits_int_le: more than 8 bytes requested"); - char buf[8]; - m_io->read(buf, bytes_needed); - for (int i = 0; i < bytes_needed; i++) { - uint8_t b = buf[i]; - m_bits |= (static_cast(b) << m_bits_left); - m_bits_left += 8; - } - } - - // raw mask with required number of 1s, starting from lowest bit - uint64_t mask = get_mask_ones(n); - // derive reading result - uint64_t res = m_bits & mask; - // remove bottom bits that we've just read by shifting - m_bits >>= n; - m_bits_left -= n; - - return res; -} - -uint64_t kaitai::kstream::get_mask_ones(int n) { - if (n == 64) { - return 0xFFFFFFFFFFFFFFFF; - } else { - return ((uint64_t) 1 << n) - 1; - } -} - -// ======================================================================== -// Byte arrays -// ======================================================================== - -std::string kaitai::kstream::read_bytes(std::streamsize len) { - std::vector result(len); - - // NOTE: streamsize type is signed, negative values are only *supposed* to not be used. - // http://en.cppreference.com/w/cpp/io/streamsize - if (len < 0) { - throw std::runtime_error("read_bytes: requested a negative amount"); - } - - if (len > 0) { - m_io->read(&result[0], len); - } - - return std::string(result.begin(), result.end()); -} - -std::string kaitai::kstream::read_bytes_full() { - std::iostream::pos_type p1 = m_io->tellg(); - m_io->seekg(0, std::ios::end); - std::iostream::pos_type p2 = m_io->tellg(); - size_t len = p2 - p1; - - // Note: this requires a std::string to be backed with a - // contiguous buffer. Officially, it's a only requirement since - // C++11 (C++98 and C++03 didn't have this requirement), but all - // major implementations had contiguous buffers anyway. - std::string result(len, ' '); - m_io->seekg(p1); - m_io->read(&result[0], len); - - return result; -} - -std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) { - std::string result; - std::getline(*m_io, result, term); - if (m_io->eof()) { - // encountered EOF - if (eos_error) { - throw std::runtime_error("read_bytes_term: encountered EOF"); - } - } else { - // encountered terminator - if (include) - result.push_back(term); - if (!consume) - m_io->unget(); - } - return result; -} - -std::string kaitai::kstream::ensure_fixed_contents(std::string expected) { - std::string actual = read_bytes(expected.length()); - - if (actual != expected) { - // NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr. - throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data"); - } - - return actual; -} - -std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) { - std::size_t new_len = src.length(); - - while (new_len > 0 && src[new_len - 1] == pad_byte) - new_len--; - - return src.substr(0, new_len); -} - -std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) { - std::size_t new_len = 0; - std::size_t max_len = src.length(); - - while (new_len < max_len && src[new_len] != term) - new_len++; - - if (include && new_len < max_len) - new_len++; - - return src.substr(0, new_len); -} - -// ======================================================================== -// Byte array processing -// ======================================================================== - -std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) { - size_t len = data.length(); - std::string result(len, ' '); - - for (size_t i = 0; i < len; i++) - result[i] = data[i] ^ key; - - return result; -} - -std::string kaitai::kstream::process_xor_many(std::string data, std::string key) { - size_t len = data.length(); - size_t kl = key.length(); - std::string result(len, ' '); - - size_t ki = 0; - for (size_t i = 0; i < len; i++) { - result[i] = data[i] ^ key[ki]; - ki++; - if (ki >= kl) - ki = 0; - } - - return result; -} - -std::string kaitai::kstream::process_rotate_left(std::string data, int amount) { - size_t len = data.length(); - std::string result(len, ' '); - - for (size_t i = 0; i < len; i++) { - uint8_t bits = data[i]; - result[i] = (bits << amount) | (bits >> (8 - amount)); - } - - return result; -} - -#ifdef KS_ZLIB -#include - -std::string kaitai::kstream::process_zlib(std::string data) { - int ret; - - unsigned char *src_ptr = reinterpret_cast(&data[0]); - std::stringstream dst_strm; - - z_stream strm; - strm.zalloc = Z_NULL; - strm.zfree = Z_NULL; - strm.opaque = Z_NULL; - - ret = inflateInit(&strm); - if (ret != Z_OK) - throw std::runtime_error("process_zlib: inflateInit error"); - - strm.next_in = src_ptr; - strm.avail_in = data.length(); - - unsigned char outbuffer[ZLIB_BUF_SIZE]; - std::string outstring; - - // get the decompressed bytes blockwise using repeated calls to inflate - do { - strm.next_out = reinterpret_cast(outbuffer); - strm.avail_out = sizeof(outbuffer); - - ret = inflate(&strm, 0); - - if (outstring.size() < strm.total_out) - outstring.append(reinterpret_cast(outbuffer), strm.total_out - outstring.size()); - } while (ret == Z_OK); - - if (ret != Z_STREAM_END) { // an error occurred that was not EOF - std::ostringstream exc_msg; - exc_msg << "process_zlib: error #" << ret << "): " << strm.msg; - throw std::runtime_error(exc_msg.str()); - } - - if (inflateEnd(&strm) != Z_OK) - throw std::runtime_error("process_zlib: inflateEnd error"); - - return outstring; -} -#endif - -// ======================================================================== -// Misc utility methods -// ======================================================================== - -int kaitai::kstream::mod(int a, int b) { - if (b <= 0) - throw std::invalid_argument("mod: divisor b <= 0"); - int r = a % b; - if (r < 0) - r += b; - return r; -} - -#include -std::string kaitai::kstream::to_string(int val) { - // if int is 32 bits, "-2147483648" is the longest string representation - // => 11 chars + zero => 12 chars - // if int is 64 bits, "-9223372036854775808" is the longest - // => 20 chars + zero => 21 chars - char buf[25]; - int got_len = snprintf(buf, sizeof(buf), "%d", val); - - // should never happen, but check nonetheless - if (got_len > sizeof(buf)) - throw std::invalid_argument("to_string: integer is longer than string buffer"); - - return std::string(buf); -} - -#include -std::string kaitai::kstream::reverse(std::string val) { - std::reverse(val.begin(), val.end()); - - return val; -} - -uint8_t kaitai::kstream::byte_array_min(const std::string val) { - uint8_t min = 0xff; // UINT8_MAX - std::string::const_iterator end = val.end(); - for (std::string::const_iterator it = val.begin(); it != end; ++it) { - uint8_t cur = static_cast(*it); - if (cur < min) { - min = cur; - } - } - return min; -} - -uint8_t kaitai::kstream::byte_array_max(const std::string val) { - uint8_t max = 0; // UINT8_MIN - std::string::const_iterator end = val.end(); - for (std::string::const_iterator it = val.begin(); it != end; ++it) { - uint8_t cur = static_cast(*it); - if (cur > max) { - max = cur; - } - } - return max; -} - -// ======================================================================== -// Other internal methods -// ======================================================================== - -#ifndef KS_STR_DEFAULT_ENCODING -#define KS_STR_DEFAULT_ENCODING "UTF-8" -#endif - -#ifdef KS_STR_ENCODING_ICONV - -#include -#include -#include - -std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { - iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str()); - - if (cd == (iconv_t) -1) { - if (errno == EINVAL) { - throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested"); - } else { - throw std::runtime_error("bytes_to_str: error opening iconv"); - } - } - - size_t src_len = src.length(); - size_t src_left = src_len; - - // Start with a buffer length of double the source length. - size_t dst_len = src_len * 2; - std::string dst(dst_len, ' '); - size_t dst_left = dst_len; - - char *src_ptr = &src[0]; - char *dst_ptr = &dst[0]; - - while (true) { - size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left); - - if (res == (size_t) -1) { - if (errno == E2BIG) { - // dst buffer is not enough to accomodate whole string - // enlarge the buffer and try again - size_t dst_used = dst_len - dst_left; - dst_left += dst_len; - dst_len += dst_len; - dst.resize(dst_len); - - // dst.resize might have allocated destination buffer in another area - // of memory, thus our previous pointer "dst" will be invalid; re-point - // it using "dst_used". - dst_ptr = &dst[dst_used]; - } else { - throw std::runtime_error("bytes_to_str: iconv error"); - } - } else { - // conversion successful - dst.resize(dst_len - dst_left); - break; - } - } - - if (iconv_close(cd) != 0) { - throw std::runtime_error("bytes_to_str: iconv close error"); - } - - return dst; -} -#elif defined(KS_STR_ENCODING_NONE) -std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) { - return src; -} -#else -#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE -#endif diff --git a/third_party/kaitai/kaitaistream.h b/third_party/kaitai/kaitaistream.h deleted file mode 100644 index e7f4c6ce34..0000000000 --- a/third_party/kaitai/kaitaistream.h +++ /dev/null @@ -1,268 +0,0 @@ -#ifndef KAITAI_STREAM_H -#define KAITAI_STREAM_H - -// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal -#define KAITAI_STRUCT_VERSION 9000L - -#include -#include -#include -#include - -namespace kaitai { - -/** - * Kaitai Stream class (kaitai::kstream) is an implementation of - * Kaitai Struct stream API - * for C++/STL. It's implemented as a wrapper over generic STL std::istream. - * - * It provides a wide variety of simple methods to read (parse) binary - * representations of primitive types, such as integer and floating - * point numbers, byte arrays and strings, and also provides stream - * positioning / navigation methods with unified cross-language and - * cross-toolkit semantics. - * - * Typically, end users won't access Kaitai Stream class manually, but would - * describe a binary structure format using .ksy language and then would use - * Kaitai Struct compiler to generate source code in desired target language. - * That code, in turn, would use this class and API to do the actual parsing - * job. - */ -class kstream { -public: - /** - * Constructs new Kaitai Stream object, wrapping a given std::istream. - * \param io istream object to use for this Kaitai Stream - */ - kstream(std::istream* io); - - /** - * Constructs new Kaitai Stream object, wrapping a given in-memory data - * buffer. - * \param data data buffer to use for this Kaitai Stream - */ - kstream(std::string& data); - - void close(); - - /** @name Stream positioning */ - //@{ - /** - * Check if stream pointer is at the end of stream. Note that the semantics - * are different from traditional STL semantics: one does *not* need to do a - * read (which will fail) after the actual end of the stream to trigger EOF - * flag, which can be accessed after that read. It is sufficient to just be - * at the end of the stream for this method to return true. - * \return "true" if we are located at the end of the stream. - */ - bool is_eof() const; - - /** - * Set stream pointer to designated position. - * \param pos new position (offset in bytes from the beginning of the stream) - */ - void seek(uint64_t pos); - - /** - * Get current position of a stream pointer. - * \return pointer position, number of bytes from the beginning of the stream - */ - uint64_t pos(); - - /** - * Get total size of the stream in bytes. - * \return size of the stream in bytes - */ - uint64_t size(); - //@} - - /** @name Integer numbers */ - //@{ - - // ------------------------------------------------------------------------ - // Signed - // ------------------------------------------------------------------------ - - int8_t read_s1(); - - // ........................................................................ - // Big-endian - // ........................................................................ - - int16_t read_s2be(); - int32_t read_s4be(); - int64_t read_s8be(); - - // ........................................................................ - // Little-endian - // ........................................................................ - - int16_t read_s2le(); - int32_t read_s4le(); - int64_t read_s8le(); - - // ------------------------------------------------------------------------ - // Unsigned - // ------------------------------------------------------------------------ - - uint8_t read_u1(); - - // ........................................................................ - // Big-endian - // ........................................................................ - - uint16_t read_u2be(); - uint32_t read_u4be(); - uint64_t read_u8be(); - - // ........................................................................ - // Little-endian - // ........................................................................ - - uint16_t read_u2le(); - uint32_t read_u4le(); - uint64_t read_u8le(); - - //@} - - /** @name Floating point numbers */ - //@{ - - // ........................................................................ - // Big-endian - // ........................................................................ - - float read_f4be(); - double read_f8be(); - - // ........................................................................ - // Little-endian - // ........................................................................ - - float read_f4le(); - double read_f8le(); - - //@} - - /** @name Unaligned bit values */ - //@{ - - void align_to_byte(); - uint64_t read_bits_int_be(int n); - uint64_t read_bits_int(int n); - uint64_t read_bits_int_le(int n); - - //@} - - /** @name Byte arrays */ - //@{ - - std::string read_bytes(std::streamsize len); - std::string read_bytes_full(); - std::string read_bytes_term(char term, bool include, bool consume, bool eos_error); - std::string ensure_fixed_contents(std::string expected); - - static std::string bytes_strip_right(std::string src, char pad_byte); - static std::string bytes_terminate(std::string src, char term, bool include); - static std::string bytes_to_str(std::string src, std::string src_enc); - - //@} - - /** @name Byte array processing */ - //@{ - - /** - * Performs a XOR processing with given data, XORing every byte of input with a single - * given value. - * @param data data to process - * @param key value to XOR with - * @return processed data - */ - static std::string process_xor_one(std::string data, uint8_t key); - - /** - * Performs a XOR processing with given data, XORing every byte of input with a key - * array, repeating key array many times, if necessary (i.e. if data array is longer - * than key array). - * @param data data to process - * @param key array of bytes to XOR with - * @return processed data - */ - static std::string process_xor_many(std::string data, std::string key); - - /** - * Performs a circular left rotation shift for a given buffer by a given amount of bits, - * using groups of 1 bytes each time. Right circular rotation should be performed - * using this procedure with corrected amount. - * @param data source data to process - * @param amount number of bits to shift by - * @return copy of source array with requested shift applied - */ - static std::string process_rotate_left(std::string data, int amount); - -#ifdef KS_ZLIB - /** - * Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers. - * @param data data to unpack - * @return unpacked data - * @throws IOException - */ - static std::string process_zlib(std::string data); -#endif - - //@} - - /** - * Performs modulo operation between two integers: dividend `a` - * and divisor `b`. Divisor `b` is expected to be positive. The - * result is always 0 <= x <= b - 1. - */ - static int mod(int a, int b); - - /** - * Converts given integer `val` to a decimal string representation. - * Should be used in place of std::to_string() (which is available only - * since C++11) in older C++ implementations. - */ - static std::string to_string(int val); - - /** - * Reverses given string `val`, so that the first character becomes the - * last and the last one becomes the first. This should be used to avoid - * the need of local variables at the caller. - */ - static std::string reverse(std::string val); - - /** - * Finds the minimal byte in a byte array, treating bytes as - * unsigned values. - * @param val byte array to scan - * @return minimal byte in byte array as integer - */ - static uint8_t byte_array_min(const std::string val); - - /** - * Finds the maximal byte in a byte array, treating bytes as - * unsigned values. - * @param val byte array to scan - * @return maximal byte in byte array as integer - */ - static uint8_t byte_array_max(const std::string val); - -private: - std::istream* m_io; - std::istringstream m_io_str; - int m_bits_left; - uint64_t m_bits; - - void init(); - void exceptions_enable() const; - - static uint64_t get_mask_ones(int n); - - static const int ZLIB_BUF_SIZE = 128 * 1024; -}; - -} - -#endif diff --git a/third_party/kaitai/kaitaistruct.h b/third_party/kaitai/kaitaistruct.h deleted file mode 100644 index 8172ede6c9..0000000000 --- a/third_party/kaitai/kaitaistruct.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef KAITAI_STRUCT_H -#define KAITAI_STRUCT_H - -#include - -namespace kaitai { - -class kstruct { -public: - kstruct(kstream *_io) { m__io = _io; } - virtual ~kstruct() {} -protected: - kstream *m__io; -public: - kstream *_io() { return m__io; } -}; - -} - -#endif diff --git a/third_party/mapd_pfeiferj/mapd b/third_party/mapd_pfeiferj/mapd index e5e45e2b98..c8b50b0791 100755 Binary files a/third_party/mapd_pfeiferj/mapd and b/third_party/mapd_pfeiferj/mapd differ diff --git a/tools/auto_source.py b/tools/auto_source.py index 401929a9ad..bef6a43e53 100755 --- a/tools/auto_source.py +++ b/tools/auto_source.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import sys -from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.logreader import LogReader, ReadMode def main(): @@ -9,7 +9,7 @@ def main(): sys.exit(1) log_path = sys.argv[1] - lr = LogReader(log_path, sort_by_time=True) + lr = LogReader(log_path, default_mode=ReadMode.AUTO, sort_by_time=True) print("\n".join(lr.logreader_identifiers)) diff --git a/tools/jotpluggler/assets/pause.png b/tools/jotpluggler/assets/pause.png new file mode 100644 index 0000000000..8040099831 --- /dev/null +++ b/tools/jotpluggler/assets/pause.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3ea96d8193eb9067a5efdc5d88a3099730ecafa40efcd09d7402bb3efd723603 +size 2305 diff --git a/tools/jotpluggler/assets/play.png b/tools/jotpluggler/assets/play.png new file mode 100644 index 0000000000..b1556cf0ab --- /dev/null +++ b/tools/jotpluggler/assets/play.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53097ac5403b725ff1841dfa186ea770b4bb3714205824bde36ec3c2a0fb5dba +size 2758 diff --git a/tools/jotpluggler/assets/split_h.png b/tools/jotpluggler/assets/split_h.png new file mode 100644 index 0000000000..4fd88806e1 --- /dev/null +++ b/tools/jotpluggler/assets/split_h.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:54dd035ff898d881509fa686c402a61af8ef5fb408b92414722da01f773b0d33 +size 2900 diff --git a/tools/jotpluggler/assets/split_v.png b/tools/jotpluggler/assets/split_v.png new file mode 100644 index 0000000000..752e62a4ae --- /dev/null +++ b/tools/jotpluggler/assets/split_v.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:adbd4e5df1f58694dca9dde46d1d95b4e7471684e42e6bca9f41ea5d346e67c5 +size 3669 diff --git a/tools/jotpluggler/assets/x.png b/tools/jotpluggler/assets/x.png new file mode 100644 index 0000000000..3b2eabd447 --- /dev/null +++ b/tools/jotpluggler/assets/x.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6d9c90cb0dd906e0b15e1f7f3fd9f0dfad3c3b0b34eeed7a7882768dc5f3961 +size 2053 diff --git a/tools/jotpluggler/data.py b/tools/jotpluggler/data.py new file mode 100644 index 0000000000..100dfe544d --- /dev/null +++ b/tools/jotpluggler/data.py @@ -0,0 +1,352 @@ +import numpy as np +import threading +import multiprocessing +import bisect +from collections import defaultdict +from tqdm import tqdm +from openpilot.common.swaglog import cloudlog +from openpilot.tools.lib.logreader import _LogFileReader, LogReader + + +def flatten_dict(d: dict, sep: str = "/", prefix: str = None) -> dict: + result = {} + stack: list[tuple] = [(d, prefix)] + + while stack: + obj, current_prefix = stack.pop() + + if isinstance(obj, dict): + for key, val in obj.items(): + new_prefix = key if current_prefix is None else f"{current_prefix}{sep}{key}" + if isinstance(val, (dict, list)): + stack.append((val, new_prefix)) + else: + result[new_prefix] = val + elif isinstance(obj, list): + for i, item in enumerate(obj): + new_prefix = f"{current_prefix}{sep}{i}" + if isinstance(item, (dict, list)): + stack.append((item, new_prefix)) + else: + result[new_prefix] = item + else: + if current_prefix is not None: + result[current_prefix] = obj + return result + + +def extract_field_types(schema, prefix, field_types_dict): + stack = [(schema, prefix)] + + while stack: + current_schema, current_prefix = stack.pop() + + for field in current_schema.fields_list: + field_name = field.proto.name + field_path = f"{current_prefix}/{field_name}" + field_proto = field.proto + field_which = field_proto.which() + + field_type = field_proto.slot.type.which() if field_which == 'slot' else field_which + field_types_dict[field_path] = field_type + + if field_which == 'slot': + slot_type = field_proto.slot.type + type_which = slot_type.which() + + if type_which == 'list': + element_type = slot_type.list.elementType.which() + list_path = f"{field_path}/*" + field_types_dict[list_path] = element_type + + if element_type == 'struct': + stack.append((field.schema.elementType, list_path)) + + elif type_which == 'struct': + stack.append((field.schema, field_path)) + + elif field_which == 'group': + stack.append((field.schema, field_path)) + + +def _convert_to_optimal_dtype(values_list, capnp_type): + dtype_mapping = { + 'bool': np.bool_, 'int8': np.int8, 'int16': np.int16, 'int32': np.int32, 'int64': np.int64, + 'uint8': np.uint8, 'uint16': np.uint16, 'uint32': np.uint32, 'uint64': np.uint64, + 'float32': np.float32, 'float64': np.float64, 'text': object, 'data': object, + 'enum': object, 'anyPointer': object, + } + + target_dtype = dtype_mapping.get(capnp_type, object) + return np.array(values_list, dtype=target_dtype) + + +def _match_field_type(field_path, field_types): + if field_path in field_types: + return field_types[field_path] + + path_parts = field_path.split('/') + template_parts = [p if not p.isdigit() else '*' for p in path_parts] + template_path = '/'.join(template_parts) + return field_types.get(template_path) + + +def _get_field_times_values(segment, field_name): + if field_name not in segment: + return None, None + + field_data = segment[field_name] + segment_times = segment['t'] + + if field_data['sparse']: + if len(field_data['t_index']) == 0: + return None, None + return segment_times[field_data['t_index']], field_data['values'] + else: + return segment_times, field_data['values'] + + +def msgs_to_time_series(msgs): + """Extract scalar fields and return (time_series_data, start_time, end_time).""" + collected_data = defaultdict(lambda: {'timestamps': [], 'columns': defaultdict(list), 'sparse_fields': set()}) + field_types = {} + extracted_schemas = set() + min_time = max_time = None + + for msg in msgs: + typ = msg.which() + timestamp = msg.logMonoTime * 1e-9 + if typ != 'initData': + if min_time is None: + min_time = timestamp + max_time = timestamp + + sub_msg = getattr(msg, typ) + if not hasattr(sub_msg, 'to_dict'): + continue + + if hasattr(sub_msg, 'schema') and typ not in extracted_schemas: + extract_field_types(sub_msg.schema, typ, field_types) + extracted_schemas.add(typ) + + try: + msg_dict = sub_msg.to_dict(verbose=True) + except Exception as e: + cloudlog.warning(f"Failed to convert sub_msg.to_dict() for message of type: {typ}: {e}") + continue + + flat_dict = flatten_dict(msg_dict) + flat_dict['_valid'] = msg.valid + field_types[f"{typ}/_valid"] = 'bool' + + type_data = collected_data[typ] + columns, sparse_fields = type_data['columns'], type_data['sparse_fields'] + known_fields = set(columns.keys()) + missing_fields = known_fields - flat_dict.keys() + + for field, value in flat_dict.items(): + if field not in known_fields and type_data['timestamps']: + sparse_fields.add(field) + columns[field].append(value) + if value is None: + sparse_fields.add(field) + + for field in missing_fields: + columns[field].append(None) + sparse_fields.add(field) + + type_data['timestamps'].append(timestamp) + + final_result = {} + for typ, data in collected_data.items(): + if not data['timestamps']: + continue + + typ_result = {'t': np.array(data['timestamps'], dtype=np.float64)} + sparse_fields = data['sparse_fields'] + + for field_name, values in data['columns'].items(): + if len(values) < len(data['timestamps']): + values = [None] * (len(data['timestamps']) - len(values)) + values + sparse_fields.add(field_name) + + capnp_type = _match_field_type(f"{typ}/{field_name}", field_types) + + if field_name in sparse_fields: # extract non-None values and their indices + non_none_indices = [] + non_none_values = [] + for i, value in enumerate(values): + if value is not None: + non_none_indices.append(i) + non_none_values.append(value) + + if non_none_values: # check if indices > uint16 max, currently would require a 1000+ Hz signal since indices are within segments + assert max(non_none_indices) <= 65535, f"Sparse field {typ}/{field_name} has timestamp indices exceeding uint16 max. Max: {max(non_none_indices)}" + + typ_result[field_name] = { + 'values': _convert_to_optimal_dtype(non_none_values, capnp_type), + 'sparse': True, + 't_index': np.array(non_none_indices, dtype=np.uint16), + } + else: # dense representation + typ_result[field_name] = {'values': _convert_to_optimal_dtype(values, capnp_type), 'sparse': False} + + final_result[typ] = typ_result + + return final_result, min_time or 0.0, max_time or 0.0 + + +def _process_segment(segment_identifier: str): + try: + lr = _LogFileReader(segment_identifier, sort_by_time=True) + return msgs_to_time_series(lr) + except Exception as e: + cloudlog.warning(f"Warning: Failed to process segment {segment_identifier}: {e}") + return {}, 0.0, 0.0 + + +class DataManager: + def __init__(self): + self._segments = [] + self._segment_starts = [] + self._start_time = 0.0 + self._duration = 0.0 + self._paths = set() + self._observers = [] + self._loading = False + self._lock = threading.RLock() + + def load_route(self, route: str) -> None: + if self._loading: + return + self._reset() + threading.Thread(target=self._load_async, args=(route,), daemon=True).start() + + def get_timeseries(self, path: str): + with self._lock: + msg_type, field = path.split('/', 1) + times, values = [], [] + + for segment in self._segments: + if msg_type in segment: + field_times, field_values = _get_field_times_values(segment[msg_type], field) + if field_times is not None: + times.append(field_times) + values.append(field_values) + + if not times: + return np.array([]), np.array([]) + + combined_times = np.concatenate(times) - self._start_time + + if len(values) > 1: + first_dtype = values[0].dtype + if all(arr.dtype == first_dtype for arr in values): # check if all arrays have compatible dtypes + combined_values = np.concatenate(values) + else: + combined_values = np.concatenate([arr.astype(object) for arr in values]) + else: + combined_values = values[0] if values else np.array([]) + + return combined_times, combined_values + + def get_value_at(self, path: str, time: float): + with self._lock: + MAX_LOOKBACK = 5.0 # seconds + absolute_time = self._start_time + time + message_type, field = path.split('/', 1) + current_index = bisect.bisect_right(self._segment_starts, absolute_time) - 1 + for index in (current_index, current_index - 1): + if not 0 <= index < len(self._segments): + continue + segment = self._segments[index].get(message_type) + if not segment: + continue + times, values = _get_field_times_values(segment, field) + if times is None or len(times) == 0 or (index != current_index and absolute_time - times[-1] > MAX_LOOKBACK): + continue + position = np.searchsorted(times, absolute_time, 'right') - 1 + if position >= 0 and absolute_time - times[position] <= MAX_LOOKBACK: + return values[position] + return None + + def get_all_paths(self): + with self._lock: + return sorted(self._paths) + + def get_duration(self): + with self._lock: + return self._duration + + def is_plottable(self, path: str): + _, values = self.get_timeseries(path) + if len(values) == 0: + return False + return np.issubdtype(values.dtype, np.number) or np.issubdtype(values.dtype, np.bool_) + + def add_observer(self, callback): + with self._lock: + self._observers.append(callback) + + def remove_observer(self, callback): + with self._lock: + if callback in self._observers: + self._observers.remove(callback) + + def _reset(self): + with self._lock: + self._loading = True + self._segments.clear() + self._segment_starts.clear() + self._paths.clear() + self._start_time = self._duration = 0.0 + observers = self._observers.copy() + + for callback in observers: + callback({'reset': True}) + + def _load_async(self, route: str): + try: + lr = LogReader(route, sort_by_time=True) + if not lr.logreader_identifiers: + cloudlog.warning(f"Warning: No log segments found for route: {route}") + return + + num_processes = max(1, multiprocessing.cpu_count() // 2) + with multiprocessing.Pool(processes=num_processes) as pool, tqdm(total=len(lr.logreader_identifiers), desc="Processing Segments") as pbar: + for segment_result, start_time, end_time in pool.imap(_process_segment, lr.logreader_identifiers): + pbar.update(1) + if segment_result: + self._add_segment(segment_result, start_time, end_time) + except Exception: + cloudlog.exception(f"Error loading route {route}:") + finally: + self._finalize_loading() + + def _add_segment(self, segment_data: dict, start_time: float, end_time: float): + with self._lock: + self._segments.append(segment_data) + self._segment_starts.append(start_time) + + if len(self._segments) == 1: + self._start_time = start_time + self._duration = end_time - self._start_time + + for msg_type, data in segment_data.items(): + for field_name in data.keys(): + if field_name != 't': + self._paths.add(f"{msg_type}/{field_name}") + + observers = self._observers.copy() + + for callback in observers: + callback({'segment_added': True, 'duration': self._duration, 'segment_count': len(self._segments)}) + + def _finalize_loading(self): + with self._lock: + self._loading = False + observers = self._observers.copy() + duration = self._duration + + for callback in observers: + callback({'loading_complete': True, 'duration': duration}) diff --git a/tools/jotpluggler/datatree.py b/tools/jotpluggler/datatree.py new file mode 100644 index 0000000000..3390fed2e1 --- /dev/null +++ b/tools/jotpluggler/datatree.py @@ -0,0 +1,315 @@ +import os +import re +import threading +import numpy as np +import dearpygui.dearpygui as dpg + + +class DataTreeNode: + def __init__(self, name: str, full_path: str = "", parent=None): + self.name = name + self.full_path = full_path + self.parent = parent + self.children: dict[str, DataTreeNode] = {} + self.filtered_children: dict[str, DataTreeNode] = {} + self.created_children: dict[str, DataTreeNode] = {} + self.is_leaf = False + self.is_plottable: bool | None = None + self.ui_created = False + self.children_ui_created = False + self.ui_tag: str | None = None + + +class DataTree: + MAX_NODES_PER_FRAME = 50 + + def __init__(self, data_manager, playback_manager): + self.data_manager = data_manager + self.playback_manager = playback_manager + self.current_search = "" + self.data_tree = DataTreeNode(name="root") + self._build_queue: dict[str, tuple[DataTreeNode, DataTreeNode, str | int]] = {} # full_path -> (node, parent, before_tag) + self._current_created_paths: set[str] = set() + self._current_filtered_paths: set[str] = set() + self._path_to_node: dict[str, DataTreeNode] = {} # full_path -> node + self._expanded_tags: set[str] = set() + self._item_handlers: dict[str, str] = {} # ui_tag -> handler_tag + self._char_width = None + self._queued_search = None + self._new_data = False + self._ui_lock = threading.RLock() + self._handlers_to_delete = [] + self.data_manager.add_observer(self._on_data_loaded) + + def create_ui(self, parent_tag: str): + with dpg.child_window(parent=parent_tag, border=False, width=-1, height=-1): + dpg.add_text("Timeseries List") + dpg.add_separator() + dpg.add_input_text(tag="search_input", width=-1, hint="Search fields...", callback=self.search_data) + dpg.add_separator() + with dpg.child_window(border=False, width=-1, height=-1): + with dpg.group(tag="data_tree_container"): + pass + + def _on_data_loaded(self, data: dict): + with self._ui_lock: + if data.get('segment_added') or data.get('reset'): + self._new_data = True + + def update_frame(self, font): + if self._handlers_to_delete: # we need to do everything in main thread, frame callbacks are flaky + dpg.render_dearpygui_frame() # wait a frame to ensure queued callbacks are done + with self._ui_lock: + for handler in self._handlers_to_delete: + dpg.delete_item(handler) + self._handlers_to_delete.clear() + + with self._ui_lock: + if self._char_width is None: + if size := dpg.get_text_size(" ", font=font): + self._char_width = size[0] + + if self._new_data: + self._process_path_change() + self._new_data = False + return + + if self._queued_search is not None: + self.current_search = self._queued_search + self._process_path_change() + self._queued_search = None + return + + nodes_processed = 0 + while self._build_queue and nodes_processed < self.MAX_NODES_PER_FRAME: + child_node, parent, before_tag = self._build_queue.pop(next(iter(self._build_queue))) + parent_tag = "data_tree_container" if parent.name == "root" else parent.ui_tag + if not child_node.ui_created: + if child_node.is_leaf: + self._create_leaf_ui(child_node, parent_tag, before_tag) + else: + self._create_tree_node_ui(child_node, parent_tag, before_tag) + parent.created_children[child_node.name] = parent.children[child_node.name] + self._current_created_paths.add(child_node.full_path) + nodes_processed += 1 + + def _process_path_change(self): + self._build_queue.clear() + search_term = self.current_search.strip().lower() + all_paths = set(self.data_manager.get_all_paths()) + new_filtered_leafs = {path for path in all_paths if self._should_show_path(path, search_term)} + new_filtered_paths = set(new_filtered_leafs) + for path in new_filtered_leafs: + parts = path.split('/') + for i in range(1, len(parts)): + prefix = '/'.join(parts[:i]) + new_filtered_paths.add(prefix) + created_paths_to_remove = self._current_created_paths - new_filtered_paths + filtered_paths_to_remove = self._current_filtered_paths - new_filtered_leafs + + if created_paths_to_remove or filtered_paths_to_remove: + self._remove_paths_from_tree(created_paths_to_remove, filtered_paths_to_remove) + self._apply_expansion_to_tree(self.data_tree, search_term) + + paths_to_add = new_filtered_leafs - self._current_created_paths + if paths_to_add: + self._add_paths_to_tree(paths_to_add) + self._apply_expansion_to_tree(self.data_tree, search_term) + self._current_filtered_paths = new_filtered_paths + + def _remove_paths_from_tree(self, created_paths_to_remove, filtered_paths_to_remove): + for path in sorted(created_paths_to_remove, reverse=True): + current_node = self._path_to_node[path] + + if len(current_node.created_children) == 0: + self._current_created_paths.remove(current_node.full_path) + if item_handler_tag := self._item_handlers.get(current_node.ui_tag): + dpg.configure_item(item_handler_tag, show=False) + self._handlers_to_delete.append(item_handler_tag) + del self._item_handlers[current_node.ui_tag] + dpg.delete_item(current_node.ui_tag) + current_node.ui_created = False + current_node.ui_tag = None + current_node.children_ui_created = False + del current_node.parent.created_children[current_node.name] + del current_node.parent.filtered_children[current_node.name] + + for path in filtered_paths_to_remove: + parts = path.split('/') + current_node = self._path_to_node[path] + + part_array_index = -1 + while len(current_node.filtered_children) == 0 and part_array_index >= -len(parts): + current_node = current_node.parent + if parts[part_array_index] in current_node.filtered_children: + del current_node.filtered_children[parts[part_array_index]] + part_array_index -= 1 + + def _add_paths_to_tree(self, paths): + parent_nodes_to_recheck = set() + for path in sorted(paths): + parts = path.split('/') + current_node = self.data_tree + current_path_prefix = "" + + for i, part in enumerate(parts): + current_path_prefix = f"{current_path_prefix}/{part}" if current_path_prefix else part + if i < len(parts) - 1: + parent_nodes_to_recheck.add(current_node) # for incremental changes from new data + if part not in current_node.children: + current_node.children[part] = DataTreeNode(name=part, full_path=current_path_prefix, parent=current_node) + self._path_to_node[current_path_prefix] = current_node.children[part] + current_node.filtered_children[part] = current_node.children[part] + current_node = current_node.children[part] + + if not current_node.is_leaf: + current_node.is_leaf = True + + for p_node in parent_nodes_to_recheck: + p_node.children_ui_created = False + self._request_children_build(p_node) + + def _get_node_label_and_expand(self, node: DataTreeNode, search_term: str): + label = f"{node.name} ({len(node.filtered_children)} fields)" + expand = len(search_term) > 0 and any(search_term in path for path in self._get_descendant_paths(node)) + if expand and node.parent and len(node.parent.filtered_children) > 100 and len(node.filtered_children) > 2: + label += " (+)" # symbol for large lists which aren't fully expanded for performance (only affects procLog rn) + expand = False + return label, expand + + def _apply_expansion_to_tree(self, node: DataTreeNode, search_term: str): + if node.ui_created and not node.is_leaf and node.ui_tag and dpg.does_item_exist(node.ui_tag): + label, expand = self._get_node_label_and_expand(node, search_term) + if expand: + self._expanded_tags.add(node.ui_tag) + dpg.set_value(node.ui_tag, expand) + elif node.ui_tag in self._expanded_tags: # not expanded and was expanded + self._expanded_tags.remove(node.ui_tag) + dpg.set_value(node.ui_tag, expand) + dpg.delete_item(node.ui_tag, children_only=True) # delete children (not visible since collapsed) + self._reset_ui_state_recursive(node) + node.children_ui_created = False + dpg.set_item_label(node.ui_tag, label) + for child in node.created_children.values(): + self._apply_expansion_to_tree(child, search_term) + + def _reset_ui_state_recursive(self, node: DataTreeNode): + for child in node.created_children.values(): + if child.ui_tag is not None: + if item_handler_tag := self._item_handlers.get(child.ui_tag): + self._handlers_to_delete.append(item_handler_tag) + dpg.configure_item(item_handler_tag, show=False) + del self._item_handlers[child.ui_tag] + self._reset_ui_state_recursive(child) + child.ui_created = False + child.ui_tag = None + child.children_ui_created = False + self._current_created_paths.remove(child.full_path) + node.created_children.clear() + + def search_data(self): + with self._ui_lock: + self._queued_search = dpg.get_value("search_input") + + def _create_tree_node_ui(self, node: DataTreeNode, parent_tag: str, before: str | int): + node.ui_tag = f"tree_{node.full_path}" + search_term = self.current_search.strip().lower() + label, expand = self._get_node_label_and_expand(node, search_term) + if expand: + self._expanded_tags.add(node.ui_tag) + elif node.ui_tag in self._expanded_tags: + self._expanded_tags.remove(node.ui_tag) + + with dpg.tree_node( + label=label, parent=parent_tag, tag=node.ui_tag, default_open=expand, open_on_arrow=True, open_on_double_click=True, before=before, delay_search=True + ): + with dpg.item_handler_registry() as handler_tag: + dpg.add_item_toggled_open_handler(callback=lambda s, a, u: self._request_children_build(node)) + dpg.add_item_visible_handler(callback=lambda s, a, u: self._request_children_build(node)) + dpg.bind_item_handler_registry(node.ui_tag, handler_tag) + self._item_handlers[node.ui_tag] = handler_tag + node.ui_created = True + + def _create_leaf_ui(self, node: DataTreeNode, parent_tag: str, before: str | int): + node.ui_tag = f"leaf_{node.full_path}" + with dpg.group(parent=parent_tag, tag=node.ui_tag, before=before, delay_search=True): + with dpg.table(header_row=False, policy=dpg.mvTable_SizingStretchProp, delay_search=True): + dpg.add_table_column(init_width_or_weight=0.5) + dpg.add_table_column(init_width_or_weight=0.5) + with dpg.table_row(): + dpg.add_text(node.name) + dpg.add_text("N/A", tag=f"value_{node.full_path}") + + if node.is_plottable is None: + node.is_plottable = self.data_manager.is_plottable(node.full_path) + if node.is_plottable: + with dpg.drag_payload(parent=node.ui_tag, drag_data=node.full_path, payload_type="TIMESERIES_PAYLOAD"): + dpg.add_text(f"Plot: {node.full_path}") + + with dpg.item_handler_registry() as handler_tag: + dpg.add_item_visible_handler(callback=self._on_item_visible, user_data=node.full_path) + dpg.bind_item_handler_registry(node.ui_tag, handler_tag) + self._item_handlers[node.ui_tag] = handler_tag + node.ui_created = True + + def _on_item_visible(self, sender, app_data, user_data): + with self._ui_lock: + path = user_data + value_tag = f"value_{path}" + if not dpg.does_item_exist(value_tag): + return + value_column_width = dpg.get_item_rect_size(f"leaf_{path}")[0] // 2 + value = self.data_manager.get_value_at(path, self.playback_manager.current_time_s) + if value is not None: + formatted_value = self.format_and_truncate(value, value_column_width, self._char_width) + dpg.set_value(value_tag, formatted_value) + else: + dpg.set_value(value_tag, "N/A") + + def _request_children_build(self, node: DataTreeNode): + with self._ui_lock: + if not node.children_ui_created and (node.name == "root" or (node.ui_tag is not None and dpg.get_value(node.ui_tag))): # check root or node expanded + sorted_children = sorted(node.filtered_children.values(), key=self._natural_sort_key) + next_existing: list[int | str] = [0] * len(sorted_children) + current_before_tag: int | str = 0 + + for i in range(len(sorted_children) - 1, -1, -1): # calculate "before_tag" for correct ordering when incrementally building tree + child = sorted_children[i] + next_existing[i] = current_before_tag + if child.ui_created: + candidate_tag = f"leaf_{child.full_path}" if child.is_leaf else f"tree_{child.full_path}" + if dpg.does_item_exist(candidate_tag): + current_before_tag = candidate_tag + + for i, child_node in enumerate(sorted_children): + if not child_node.ui_created: + before_tag = next_existing[i] + self._build_queue[child_node.full_path] = (child_node, node, before_tag) + node.children_ui_created = True + + def _should_show_path(self, path: str, search_term: str) -> bool: + if 'DEPRECATED' in path and not os.environ.get('SHOW_DEPRECATED'): + return False + return not search_term or search_term in path.lower() + + def _natural_sort_key(self, node: DataTreeNode): + node_type_key = node.is_leaf + parts = [int(p) if p.isdigit() else p.lower() for p in re.split(r'(\d+)', node.name) if p] + return (node_type_key, parts) + + def _get_descendant_paths(self, node: DataTreeNode): + for child_name, child_node in node.filtered_children.items(): + child_name_lower = child_name.lower() + if child_node.is_leaf: + yield child_name_lower + else: + for path in self._get_descendant_paths(child_node): + yield f"{child_name_lower}/{path}" + + @staticmethod + def format_and_truncate(value, available_width: float, char_width: float) -> str: + s = f"{value:.5f}" if np.issubdtype(type(value), np.floating) else str(value) + max_chars = int(available_width / char_width) + if len(s) > max_chars: + return s[: max(0, max_chars - 3)] + "..." + return s diff --git a/tools/jotpluggler/layout.py b/tools/jotpluggler/layout.py new file mode 100644 index 0000000000..917c156f9f --- /dev/null +++ b/tools/jotpluggler/layout.py @@ -0,0 +1,272 @@ +import dearpygui.dearpygui as dpg +from openpilot.tools.jotpluggler.data import DataManager +from openpilot.tools.jotpluggler.views import TimeSeriesPanel + +GRIP_SIZE = 4 +MIN_PANE_SIZE = 60 + + +class PlotLayoutManager: + def __init__(self, data_manager: DataManager, playback_manager, worker_manager, scale: float = 1.0): + self.data_manager = data_manager + self.playback_manager = playback_manager + self.worker_manager = worker_manager + self.scale = scale + self.container_tag = "plot_layout_container" + self.active_panels: list = [] + + self.grip_size = int(GRIP_SIZE * self.scale) + self.min_pane_size = int(MIN_PANE_SIZE * self.scale) + + initial_panel = TimeSeriesPanel(data_manager, playback_manager, worker_manager) + self.layout: dict = {"type": "panel", "panel": initial_panel} + + def create_ui(self, parent_tag: str): + if dpg.does_item_exist(self.container_tag): + dpg.delete_item(self.container_tag) + + with dpg.child_window(tag=self.container_tag, parent=parent_tag, border=False, width=-1, height=-1, no_scrollbar=True, no_scroll_with_mouse=True): + container_width, container_height = dpg.get_item_rect_size(self.container_tag) + self._create_ui_recursive(self.layout, self.container_tag, [], container_width, container_height) + + def _create_ui_recursive(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int): + if layout["type"] == "panel": + self._create_panel_ui(layout, parent_tag, path, width, height) + else: + self._create_split_ui(layout, parent_tag, path, width, height) + + def _create_panel_ui(self, layout: dict, parent_tag: str, path: list[int], width: int, height:int): + panel_tag = self._path_to_tag(path, "panel") + panel = layout["panel"] + self.active_panels.append(panel) + text_size = int(13 * self.scale) + bar_height = (text_size+24) if width < int(279 * self.scale + 80) else (text_size+8) # adjust height to allow for scrollbar + + with dpg.child_window(parent=parent_tag, border=True, width=-1, height=-1, no_scrollbar=True): + with dpg.group(horizontal=True): + with dpg.child_window(tag=panel_tag, width=-(text_size + 16), height=bar_height, horizontal_scrollbar=True, no_scroll_with_mouse=True, border=False): + with dpg.group(horizontal=True): + dpg.add_input_text(default_value=panel.title, width=int(100 * self.scale), callback=lambda s, v: setattr(panel, "title", v)) + dpg.add_combo(items=["Time Series"], default_value="Time Series", width=int(100 * self.scale)) + dpg.add_button(label="Clear", callback=lambda: self.clear_panel(panel), width=int(40 * self.scale)) + dpg.add_image_button(texture_tag="split_h_texture", callback=lambda: self.split_panel(path, 0), width=text_size, height=text_size) + dpg.add_image_button(texture_tag="split_v_texture", callback=lambda: self.split_panel(path, 1), width=text_size, height=text_size) + dpg.add_image_button(texture_tag="x_texture", callback=lambda: self.delete_panel(path), width=text_size, height=text_size) + + dpg.add_separator() + + content_tag = self._path_to_tag(path, "content") + with dpg.child_window(tag=content_tag, border=False, height=-1, width=-1, no_scrollbar=True): + panel.create_ui(content_tag) + + def _create_split_ui(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int): + split_tag = self._path_to_tag(path, "split") + orientation, _, pane_sizes = self._get_split_geometry(layout, (width, height)) + + with dpg.group(tag=split_tag, parent=parent_tag, horizontal=orientation == 0): + for i, child_layout in enumerate(layout["children"]): + child_path = path + [i] + container_tag = self._path_to_tag(child_path, "container") + pane_width, pane_height = [(pane_sizes[i], -1), (-1, pane_sizes[i])][orientation] # fill 2nd dim up to the border + with dpg.child_window(tag=container_tag, width=pane_width, height=pane_height, border=False, no_scrollbar=True): + child_width, child_height = [(pane_sizes[i], height), (width, pane_sizes[i])][orientation] + self._create_ui_recursive(child_layout, container_tag, child_path, child_width, child_height) + if i < len(layout["children"]) - 1: + self._create_grip(split_tag, path, i, orientation) + + def clear_panel(self, panel): + panel.clear() + + def delete_panel(self, panel_path: list[int]): + if not panel_path: # Root deletion + old_panel = self.layout["panel"] + old_panel.destroy_ui() + self.active_panels.remove(old_panel) + new_panel = TimeSeriesPanel(self.data_manager, self.playback_manager, self.worker_manager) + self.layout = {"type": "panel", "panel": new_panel} + self._rebuild_ui_at_path([]) + return + + parent, child_index = self._get_parent_and_index(panel_path) + layout_to_delete = parent["children"][child_index] + self._cleanup_ui_recursive(layout_to_delete, panel_path) + + parent["children"].pop(child_index) + parent["proportions"].pop(child_index) + + if len(parent["children"]) == 1: # remove parent and collapse + remaining_child = parent["children"][0] + if len(panel_path) == 1: # parent is at root level - promote remaining child to root + self.layout = remaining_child + self._rebuild_ui_at_path([]) + else: # replace parent with remaining child in grandparent + grandparent_path = panel_path[:-2] + parent_index = panel_path[-2] + self._replace_layout_at_path(grandparent_path + [parent_index], remaining_child) + self._rebuild_ui_at_path(grandparent_path + [parent_index]) + else: # redistribute proportions + equal_prop = 1.0 / len(parent["children"]) + parent["proportions"] = [equal_prop] * len(parent["children"]) + self._rebuild_ui_at_path(panel_path[:-1]) + + def split_panel(self, panel_path: list[int], orientation: int): + current_layout = self._get_layout_at_path(panel_path) + existing_panel = current_layout["panel"] + new_panel = TimeSeriesPanel(self.data_manager, self.playback_manager, self.worker_manager) + parent, child_index = self._get_parent_and_index(panel_path) + + if parent is None: # Root split + self.layout = { + "type": "split", + "orientation": orientation, + "children": [{"type": "panel", "panel": existing_panel}, {"type": "panel", "panel": new_panel}], + "proportions": [0.5, 0.5], + } + self._rebuild_ui_at_path([]) + elif parent["type"] == "split" and parent["orientation"] == orientation: # Same orientation - insert into existing split + parent["children"].insert(child_index + 1, {"type": "panel", "panel": new_panel}) + parent["proportions"] = [1.0 / len(parent["children"])] * len(parent["children"]) + self._rebuild_ui_at_path(panel_path[:-1]) + else: # Different orientation - create new split level + new_split = {"type": "split", "orientation": orientation, "children": [current_layout, {"type": "panel", "panel": new_panel}], "proportions": [0.5, 0.5]} + self._replace_layout_at_path(panel_path, new_split) + self._rebuild_ui_at_path(panel_path) + + def _rebuild_ui_at_path(self, path: list[int]): + layout = self._get_layout_at_path(path) + if path: + container_tag = self._path_to_tag(path, "container") + else: # Root update + container_tag = self.container_tag + + self._cleanup_ui_recursive(layout, path) + dpg.delete_item(container_tag, children_only=True) + width, height = dpg.get_item_rect_size(container_tag) + self._create_ui_recursive(layout, container_tag, path, width, height) + + def _cleanup_ui_recursive(self, layout: dict, path: list[int]): + if layout["type"] == "panel": + panel = layout["panel"] + panel.destroy_ui() + if panel in self.active_panels: + self.active_panels.remove(panel) + else: + for i in range(len(layout["children"]) - 1): + handler_tag = f"{self._path_to_tag(path, f'grip_{i}')}_handler" + if dpg.does_item_exist(handler_tag): + dpg.delete_item(handler_tag) + + for i, child in enumerate(layout["children"]): + self._cleanup_ui_recursive(child, path + [i]) + + def update_all_panels(self): + for panel in self.active_panels: + panel.update() + + def on_viewport_resize(self): + self._resize_splits_recursive(self.layout, []) + + def _resize_splits_recursive(self, layout: dict, path: list[int], width: int | None = None, height: int | None = None): + if layout["type"] == "split": + split_tag = self._path_to_tag(path, "split") + if dpg.does_item_exist(split_tag): + available_sizes = (width, height) if width and height else dpg.get_item_rect_size(dpg.get_item_parent(split_tag)) + orientation, _, pane_sizes = self._get_split_geometry(layout, available_sizes) + size_properties = ("width", "height") + + for i, child_layout in enumerate(layout["children"]): + child_path = path + [i] + container_tag = self._path_to_tag(child_path, "container") + if dpg.does_item_exist(container_tag): + dpg.configure_item(container_tag, **{size_properties[orientation]: pane_sizes[i]}) + child_width, child_height = [(pane_sizes[i], available_sizes[1]), (available_sizes[0], pane_sizes[i])][orientation] + self._resize_splits_recursive(child_layout, child_path, child_width, child_height) + else: # leaf node/panel - adjust bar height to allow for scrollbar + panel_tag = self._path_to_tag(path, "panel") + if width is not None and width < int(279 * self.scale + 80): # scaled widths of the elements in top bar + fixed 8 padding on left and right of each item + dpg.configure_item(panel_tag, height=(int(13*self.scale) + 24)) + else: + dpg.configure_item(panel_tag, height=(int(13*self.scale) + 8)) + + def _get_split_geometry(self, layout: dict, available_size: tuple[int, int]) -> tuple[int, int, list[int]]: + orientation = layout["orientation"] + num_grips = len(layout["children"]) - 1 + usable_size = max(self.min_pane_size, available_size[orientation] - (num_grips * (self.grip_size + 8 * (2-orientation)))) # approximate, scaling is weird + pane_sizes = [max(self.min_pane_size, int(usable_size * prop)) for prop in layout["proportions"]] + return orientation, usable_size, pane_sizes + + def _get_layout_at_path(self, path: list[int]) -> dict: + current = self.layout + for index in path: + current = current["children"][index] + return current + + def _get_parent_and_index(self, path: list[int]) -> tuple: + return (None, -1) if not path else (self._get_layout_at_path(path[:-1]), path[-1]) + + def _replace_layout_at_path(self, path: list[int], new_layout: dict): + if not path: + self.layout = new_layout + else: + parent, index = self._get_parent_and_index(path) + parent["children"][index] = new_layout + + def _path_to_tag(self, path: list[int], prefix: str = "") -> str: + path_str = "_".join(map(str, path)) if path else "root" + return f"{prefix}_{path_str}" if prefix else path_str + + def _create_grip(self, parent_tag: str, path: list[int], grip_index: int, orientation: int): + grip_tag = self._path_to_tag(path, f"grip_{grip_index}") + width, height = [(self.grip_size, -1), (-1, self.grip_size)][orientation] + + with dpg.child_window(tag=grip_tag, parent=parent_tag, width=width, height=height, no_scrollbar=True, border=False): + button_tag = dpg.add_button(label="", width=-1, height=-1) + + with dpg.item_handler_registry(tag=f"{grip_tag}_handler"): + user_data = (path, grip_index, orientation) + dpg.add_item_active_handler(callback=self._on_grip_drag, user_data=user_data) + dpg.add_item_deactivated_handler(callback=self._on_grip_end, user_data=user_data) + dpg.bind_item_handler_registry(button_tag, f"{grip_tag}_handler") + + def _on_grip_drag(self, sender, app_data, user_data): + path, grip_index, orientation = user_data + layout = self._get_layout_at_path(path) + + if "_drag_data" not in layout: + layout["_drag_data"] = {"initial_proportions": layout["proportions"][:], "start_mouse": dpg.get_mouse_pos(local=False)[orientation]} + return + + drag_data = layout["_drag_data"] + split_tag = self._path_to_tag(path, "split") + if not dpg.does_item_exist(split_tag): + return + + _, usable_size, _ = self._get_split_geometry(layout, dpg.get_item_rect_size(split_tag)) + current_coord = dpg.get_mouse_pos(local=False)[orientation] + delta = current_coord - drag_data["start_mouse"] + delta_prop = delta / usable_size + + left_idx = grip_index + right_idx = left_idx + 1 + initial = drag_data["initial_proportions"] + min_prop = self.min_pane_size / usable_size + + new_left = max(min_prop, initial[left_idx] + delta_prop) + new_right = max(min_prop, initial[right_idx] - delta_prop) + + total_available = initial[left_idx] + initial[right_idx] + if new_left + new_right > total_available: + if new_left > new_right: + new_left = total_available - new_right + else: + new_right = total_available - new_left + + layout["proportions"] = initial[:] + layout["proportions"][left_idx] = new_left + layout["proportions"][right_idx] = new_right + + self._resize_splits_recursive(layout, path) + + def _on_grip_end(self, sender, app_data, user_data): + path, _, _ = user_data + self._get_layout_at_path(path).pop("_drag_data", None) diff --git a/tools/jotpluggler/pluggle.py b/tools/jotpluggler/pluggle.py new file mode 100755 index 0000000000..582a44454e --- /dev/null +++ b/tools/jotpluggler/pluggle.py @@ -0,0 +1,252 @@ +#!/usr/bin/env python3 +import argparse +import os +import pyautogui +import subprocess +import dearpygui.dearpygui as dpg +import multiprocessing +import uuid +import signal +from openpilot.common.basedir import BASEDIR +from openpilot.tools.jotpluggler.data import DataManager +from openpilot.tools.jotpluggler.datatree import DataTree +from openpilot.tools.jotpluggler.layout import PlotLayoutManager + +DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19" + + +class WorkerManager: + def __init__(self, max_workers=None): + self.pool = multiprocessing.Pool(max_workers or min(4, multiprocessing.cpu_count()), initializer=WorkerManager.worker_initializer) + self.active_tasks = {} + + def submit_task(self, func, args_list, callback=None, task_id=None): + task_id = task_id or str(uuid.uuid4()) + + if task_id in self.active_tasks: + try: + self.active_tasks[task_id].terminate() + except Exception: + pass + + def handle_success(result): + self.active_tasks.pop(task_id, None) + if callback: + try: + callback(result) + except Exception as e: + print(f"Callback for task {task_id} failed: {e}") + + def handle_error(error): + self.active_tasks.pop(task_id, None) + print(f"Task {task_id} failed: {error}") + + async_result = self.pool.starmap_async(func, args_list, callback=handle_success, error_callback=handle_error) + self.active_tasks[task_id] = async_result + return task_id + + @staticmethod + def worker_initializer(): + signal.signal(signal.SIGINT, signal.SIG_IGN) + + def shutdown(self): + for task in self.active_tasks.values(): + try: + task.terminate() + except Exception: + pass + self.pool.terminate() + self.pool.join() + + +class PlaybackManager: + def __init__(self): + self.is_playing = False + self.current_time_s = 0.0 + self.duration_s = 0.0 + + def set_route_duration(self, duration: float): + self.duration_s = duration + self.seek(min(self.current_time_s, duration)) + + def toggle_play_pause(self): + if not self.is_playing and self.current_time_s >= self.duration_s: + self.seek(0.0) + self.is_playing = not self.is_playing + texture_tag = "pause_texture" if self.is_playing else "play_texture" + dpg.configure_item("play_pause_button", texture_tag=texture_tag) + + def seek(self, time_s: float): + self.current_time_s = max(0.0, min(time_s, self.duration_s)) + + def update_time(self, delta_t: float): + if self.is_playing: + self.current_time_s = min(self.current_time_s + delta_t, self.duration_s) + if self.current_time_s >= self.duration_s: + self.is_playing = False + dpg.configure_item("play_pause_button", texture_tag="play_texture") + return self.current_time_s + + +class MainController: + def __init__(self, scale: float = 1.0): + self.scale = scale + self.data_manager = DataManager() + self.playback_manager = PlaybackManager() + self.worker_manager = WorkerManager() + self._create_global_themes() + self.data_tree = DataTree(self.data_manager, self.playback_manager) + self.plot_layout_manager = PlotLayoutManager(self.data_manager, self.playback_manager, self.worker_manager, scale=self.scale) + self.data_manager.add_observer(self.on_data_loaded) + + def _create_global_themes(self): + with dpg.theme(tag="global_line_theme"): + with dpg.theme_component(dpg.mvLineSeries): + scaled_thickness = max(1.0, self.scale) + dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots) + + with dpg.theme(tag="global_timeline_theme"): + with dpg.theme_component(dpg.mvInfLineSeries): + scaled_thickness = max(1.0, self.scale) + dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots) + dpg.add_theme_color(dpg.mvPlotCol_Line, (255, 0, 0, 128), category=dpg.mvThemeCat_Plots) + + def on_data_loaded(self, data: dict): + duration = data.get('duration', 0.0) + self.playback_manager.set_route_duration(duration) + + if data.get('reset'): + self.playback_manager.current_time_s = 0.0 + self.playback_manager.duration_s = 0.0 + self.playback_manager.is_playing = False + dpg.set_value("load_status", "Loading...") + dpg.set_value("timeline_slider", 0.0) + dpg.configure_item("timeline_slider", max_value=0.0) + dpg.configure_item("play_pause_button", texture_tag="play_texture") + dpg.configure_item("load_button", enabled=True) + elif data.get('loading_complete'): + num_paths = len(self.data_manager.get_all_paths()) + dpg.set_value("load_status", f"Loaded {num_paths} data paths") + dpg.configure_item("load_button", enabled=True) + elif data.get('segment_added'): + segment_count = data.get('segment_count', 0) + dpg.set_value("load_status", f"Loading... {segment_count} segments processed") + + dpg.configure_item("timeline_slider", max_value=duration) + + def setup_ui(self): + with dpg.texture_registry(): + script_dir = os.path.dirname(os.path.realpath(__file__)) + for image in ["play", "pause", "x", "split_h", "split_v"]: + texture = dpg.load_image(os.path.join(script_dir, "assets", f"{image}.png")) + dpg.add_static_texture(width=texture[0], height=texture[1], default_value=texture[3], tag=f"{image}_texture") + + with dpg.window(tag="Primary Window"): + with dpg.group(horizontal=True): + # Left panel - Data tree + with dpg.child_window(label="Sidebar", width=300 * self.scale, tag="sidebar_window", border=True, resizable_x=True): + with dpg.group(horizontal=True): + dpg.add_input_text(tag="route_input", width=-75 * self.scale, hint="Enter route name...") + dpg.add_button(label="Load", callback=self.load_route, tag="load_button", width=-1) + dpg.add_text("Ready to load route", tag="load_status") + dpg.add_separator() + self.data_tree.create_ui("sidebar_window") + + # Right panel - Plots and timeline + with dpg.group(tag="right_panel"): + with dpg.child_window(label="Plot Window", border=True, height=-(32 + 13 * self.scale), tag="main_plot_area"): + self.plot_layout_manager.create_ui("main_plot_area") + + with dpg.child_window(label="Timeline", border=True): + with dpg.table(header_row=False, borders_innerH=False, borders_innerV=False, borders_outerH=False, borders_outerV=False): + btn_size = int(13 * self.scale) + dpg.add_table_column(width_fixed=True, init_width_or_weight=(btn_size + 8)) # Play button + dpg.add_table_column(width_stretch=True) # Timeline slider + dpg.add_table_column(width_fixed=True, init_width_or_weight=int(50 * self.scale)) # FPS counter + with dpg.table_row(): + dpg.add_image_button(texture_tag="play_texture", tag="play_pause_button", callback=self.toggle_play_pause, width=btn_size, height=btn_size) + dpg.add_slider_float(tag="timeline_slider", default_value=0.0, label="", width=-1, callback=self.timeline_drag) + dpg.add_text("", tag="fps_counter") + with dpg.item_handler_registry(tag="plot_resize_handler"): + dpg.add_item_resize_handler(callback=self.on_plot_resize) + dpg.bind_item_handler_registry("right_panel", "plot_resize_handler") + + dpg.set_primary_window("Primary Window", True) + + def on_plot_resize(self, sender, app_data, user_data): + self.plot_layout_manager.on_viewport_resize() + + def load_route(self): + route_name = dpg.get_value("route_input").strip() + if route_name: + dpg.set_value("load_status", "Loading route...") + dpg.configure_item("load_button", enabled=False) + self.data_manager.load_route(route_name) + + def toggle_play_pause(self, sender): + self.playback_manager.toggle_play_pause() + + def timeline_drag(self, sender, app_data): + self.playback_manager.seek(app_data) + + def update_frame(self, font): + self.data_tree.update_frame(font) + + new_time = self.playback_manager.update_time(dpg.get_delta_time()) + if not dpg.is_item_active("timeline_slider"): + dpg.set_value("timeline_slider", new_time) + + self.plot_layout_manager.update_all_panels() + + dpg.set_value("fps_counter", f"{dpg.get_frame_rate():.1f} FPS") + + def shutdown(self): + self.worker_manager.shutdown() + + +def main(route_to_load=None): + dpg.create_context() + + # TODO: find better way of calculating display scaling + try: + w, h = next(tuple(map(int, l.split()[0].split('x'))) for l in subprocess.check_output(['xrandr']).decode().split('\n') if '*' in l) # actual resolution + scale = pyautogui.size()[0] / w # scaled resolution + except Exception: + scale = 1 + + with dpg.font_registry(): + default_font = dpg.add_font(os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf"), int(13 * scale)) + dpg.bind_font(default_font) + + viewport_width, viewport_height = int(1200 * scale), int(800 * scale) + mouse_x, mouse_y = pyautogui.position() # TODO: find better way of creating the window where the user is (default dpg behavior annoying on multiple displays) + dpg.create_viewport( + title='JotPluggler', width=viewport_width, height=viewport_height, x_pos=mouse_x - viewport_width // 2, y_pos=mouse_y - viewport_height // 2 + ) + dpg.setup_dearpygui() + + controller = MainController(scale=scale) + controller.setup_ui() + + if route_to_load: + dpg.set_value("route_input", route_to_load) + controller.load_route() + + dpg.show_viewport() + + # Main loop + try: + while dpg.is_dearpygui_running(): + controller.update_frame(default_font) + dpg.render_dearpygui_frame() + finally: + controller.shutdown() + dpg.destroy_context() + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="A tool for visualizing openpilot logs.") + parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one") + parser.add_argument("route", nargs='?', default=None, help="Optional route name to load on startup.") + args = parser.parse_args() + route = DEMO_ROUTE if args.demo else args.route + main(route_to_load=route) diff --git a/tools/jotpluggler/views.py b/tools/jotpluggler/views.py new file mode 100644 index 0000000000..4af9a102ac --- /dev/null +++ b/tools/jotpluggler/views.py @@ -0,0 +1,195 @@ +import uuid +import threading +import numpy as np +from collections import deque +import dearpygui.dearpygui as dpg +from abc import ABC, abstractmethod + + +class ViewPanel(ABC): + """Abstract base class for all view panels that can be displayed in a plot container""" + + def __init__(self, panel_id: str = None): + self.panel_id = panel_id or str(uuid.uuid4()) + self.title = "Untitled Panel" + + @abstractmethod + def clear(self): + pass + + @abstractmethod + def create_ui(self, parent_tag: str): + pass + + @abstractmethod + def destroy_ui(self): + pass + + @abstractmethod + def get_panel_type(self) -> str: + pass + + @abstractmethod + def update(self): + pass + + +class TimeSeriesPanel(ViewPanel): + def __init__(self, data_manager, playback_manager, worker_manager, panel_id: str | None = None): + super().__init__(panel_id) + self.data_manager = data_manager + self.playback_manager = playback_manager + self.worker_manager = worker_manager + self.title = "Time Series Plot" + self.plot_tag = f"plot_{self.panel_id}" + self.x_axis_tag = f"{self.plot_tag}_x_axis" + self.y_axis_tag = f"{self.plot_tag}_y_axis" + self.timeline_indicator_tag = f"{self.plot_tag}_timeline" + self._ui_created = False + self._series_data: dict[str, tuple[np.ndarray, np.ndarray]] = {} + self._last_plot_duration = 0 + self._update_lock = threading.RLock() + self._results_deque: deque[tuple[str, list, list]] = deque() + self._new_data = False + + def create_ui(self, parent_tag: str): + self.data_manager.add_observer(self.on_data_loaded) + with dpg.plot(height=-1, width=-1, tag=self.plot_tag, parent=parent_tag, drop_callback=self._on_series_drop, payload_type="TIMESERIES_PAYLOAD"): + dpg.add_plot_legend() + dpg.add_plot_axis(dpg.mvXAxis, no_label=True, tag=self.x_axis_tag) + dpg.add_plot_axis(dpg.mvYAxis, no_label=True, tag=self.y_axis_tag) + timeline_series_tag = dpg.add_inf_line_series(x=[0], label="Timeline", parent=self.y_axis_tag, tag=self.timeline_indicator_tag) + dpg.bind_item_theme(timeline_series_tag, "global_timeline_theme") + + for series_path in list(self._series_data.keys()): + self.add_series(series_path) + self._ui_created = True + + def update(self): + with self._update_lock: + if not self._ui_created: + return + + if self._new_data: # handle new data in main thread + self._new_data = False + for series_path in list(self._series_data.keys()): + self.add_series(series_path, update=True) + + while self._results_deque: # handle downsampled results in main thread + results = self._results_deque.popleft() + for series_path, downsampled_time, downsampled_values in results: + series_tag = f"series_{self.panel_id}_{series_path}" + if dpg.does_item_exist(series_tag): + dpg.set_value(series_tag, (downsampled_time, downsampled_values.astype(float))) + + # update timeline + current_time_s = self.playback_manager.current_time_s + dpg.set_value(self.timeline_indicator_tag, [[current_time_s], [0]]) + + # update timeseries legend label + for series_path, (time_array, value_array) in self._series_data.items(): + position = np.searchsorted(time_array, current_time_s, side='right') - 1 + if position >= 0 and (current_time_s - time_array[position]) <= 1.0: + value = value_array[position] + formatted_value = f"{value:.5f}" if np.issubdtype(type(value), np.floating) else str(value) + series_tag = f"series_{self.panel_id}_{series_path}" + if dpg.does_item_exist(series_tag): + dpg.configure_item(series_tag, label=f"{series_path}: {formatted_value}") + + # downsample if plot zoom changed significantly + plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0] + if plot_duration > self._last_plot_duration * 2 or plot_duration < self._last_plot_duration * 0.5: + self._downsample_all_series(plot_duration) + + def _downsample_all_series(self, plot_duration): + plot_width = dpg.get_item_rect_size(self.plot_tag)[0] + if plot_width <= 0 or plot_duration <= 0: + return + + self._last_plot_duration = plot_duration + target_points_per_second = plot_width / plot_duration + work_items = [] + for series_path, (time_array, value_array) in self._series_data.items(): + if len(time_array) == 0: + continue + series_duration = time_array[-1] - time_array[0] if len(time_array) > 1 else 1 + points_per_second = len(time_array) / series_duration + if points_per_second > target_points_per_second * 2: + target_points = max(int(target_points_per_second * series_duration), plot_width) + work_items.append((series_path, time_array, value_array, target_points)) + elif dpg.does_item_exist(f"series_{self.panel_id}_{series_path}"): + dpg.set_value(f"series_{self.panel_id}_{series_path}", (time_array, value_array.astype(float))) + + if work_items: + self.worker_manager.submit_task( + TimeSeriesPanel._downsample_worker, work_items, callback=lambda results: self._results_deque.append(results), task_id=f"downsample_{self.panel_id}" + ) + + def add_series(self, series_path: str, update: bool = False): + with self._update_lock: + if update or series_path not in self._series_data: + self._series_data[series_path] = self.data_manager.get_timeseries(series_path) + + time_array, value_array = self._series_data[series_path] + series_tag = f"series_{self.panel_id}_{series_path}" + if dpg.does_item_exist(series_tag): + dpg.set_value(series_tag, (time_array, value_array.astype(float))) + else: + line_series_tag = dpg.add_line_series(x=time_array, y=value_array.astype(float), label=series_path, parent=self.y_axis_tag, tag=series_tag) + dpg.bind_item_theme(line_series_tag, "global_line_theme") + dpg.fit_axis_data(self.x_axis_tag) + dpg.fit_axis_data(self.y_axis_tag) + plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0] + self._downsample_all_series(plot_duration) + + def destroy_ui(self): + with self._update_lock: + self.data_manager.remove_observer(self.on_data_loaded) + if dpg.does_item_exist(self.plot_tag): + dpg.delete_item(self.plot_tag) + self._ui_created = False + + def get_panel_type(self) -> str: + return "timeseries" + + def clear(self): + with self._update_lock: + for series_path in list(self._series_data.keys()): + self.remove_series(series_path) + + def remove_series(self, series_path: str): + with self._update_lock: + if series_path in self._series_data: + if dpg.does_item_exist(f"series_{self.panel_id}_{series_path}"): + dpg.delete_item(f"series_{self.panel_id}_{series_path}") + del self._series_data[series_path] + + def on_data_loaded(self, data: dict): + self._new_data = True + + def _on_series_drop(self, sender, app_data, user_data): + self.add_series(app_data) + + @staticmethod + def _downsample_worker(series_path, time_array, value_array, target_points): + if len(time_array) <= target_points: + return series_path, time_array, value_array + + step = len(time_array) / target_points + indices = [] + + for i in range(target_points): + start_idx = int(i * step) + end_idx = int((i + 1) * step) + if start_idx == end_idx: + indices.append(start_idx) + else: + bucket_values = value_array[start_idx:end_idx] + min_idx = start_idx + np.argmin(bucket_values) + max_idx = start_idx + np.argmax(bucket_values) + if min_idx != max_idx: + indices.extend([min(min_idx, max_idx), max(min_idx, max_idx)]) + else: + indices.append(min_idx) + indices = sorted(set(indices)) + return series_path, time_array[indices], value_array[indices] diff --git a/tools/op.sh b/tools/op.sh index e83b46b2d2..54ff8e97e9 100755 --- a/tools/op.sh +++ b/tools/op.sh @@ -365,6 +365,7 @@ function op_switch() { fi BRANCH="$1" + git config --replace-all remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*" git fetch "$REMOTE" "$BRANCH" git checkout -f FETCH_HEAD git checkout -B "$BRANCH" --track "$REMOTE"/"$BRANCH" diff --git a/tools/plotjuggler/layouts/torque-controller.xml b/tools/plotjuggler/layouts/torque-controller.xml index 606df03611..8e9a1a8526 100644 --- a/tools/plotjuggler/layouts/torque-controller.xml +++ b/tools/plotjuggler/layouts/torque-controller.xml @@ -1,77 +1,45 @@ - + - + - - + + - - + + - - + + - - + + - - + + - + - + - - + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + @@ -79,115 +47,134 @@ - + - - + + - - + + - - + + - - + + - - + + - - + + - + - - - - + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - + + - - + + - - - - + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -199,25 +186,34 @@ + + + + + + + + + + + + + - + - return (0) - /carState/canValid + return value * 3.6 + /carState/vEgo - + - return (value * v1 ^ 2) - (v2 * 9.81) - /controlsState/curvature - - /carState/vEgo - /liveParameters/roll - + return value * 2.23694 + /carState/vEgo @@ -228,15 +224,24 @@ /liveParameters/roll - + - return value * 2.23694 - /carState/vEgo + return (value * v1 ^ 2) - (v2 * 9.81) + /controlsState/curvature + + /carState/vEgo + /liveParameters/roll + - + - return value * 3.6 - /carState/vEgo + return value + 0.2 + /carParams/steerActuatorDelay + + + + return (0) + /carState/canValid diff --git a/tools/replay/SConscript b/tools/replay/SConscript index 18849407cf..136c4119f6 100644 --- a/tools/replay/SConscript +++ b/tools/replay/SConscript @@ -13,6 +13,8 @@ else: replay_lib_src = ["replay.cc", "consoleui.cc", "camera.cc", "filereader.cc", "logreader.cc", "framereader.cc", "route.cc", "util.cc", "seg_mgr.cc", "timeline.cc", "api.cc"] +if arch != "Darwin": + replay_lib_src.append("qcom_decoder.cc") replay_lib = replay_env.Library("replay", replay_lib_src, LIBS=base_libs, FRAMEWORKS=base_frameworks) Export('replay_lib') replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'ncurses'] + base_libs diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc index ed88626be3..e9cd090446 100644 --- a/tools/replay/framereader.cc +++ b/tools/replay/framereader.cc @@ -8,6 +8,7 @@ #include "common/util.h" #include "third_party/libyuv/include/libyuv.h" #include "tools/replay/util.h" +#include "system/hardware/hw.h" #ifdef __APPLE__ #define HW_DEVICE_TYPE AV_HWDEVICE_TYPE_VIDEOTOOLBOX @@ -37,7 +38,16 @@ struct DecoderManager { return it->second.get(); } - auto decoder = std::make_unique(); + std::unique_ptr decoder; + #ifndef __APPLE__ + if (Hardware::TICI() && hw_decoder) { + decoder = std::make_unique(); + } else + #endif + { + decoder = std::make_unique(); + } + if (!decoder->open(codecpar, hw_decoder)) { decoder.reset(nullptr); } @@ -114,19 +124,19 @@ bool FrameReader::get(int idx, VisionBuf *buf) { // class VideoDecoder -VideoDecoder::VideoDecoder() { +FFmpegVideoDecoder::FFmpegVideoDecoder() { av_frame_ = av_frame_alloc(); hw_frame_ = av_frame_alloc(); } -VideoDecoder::~VideoDecoder() { +FFmpegVideoDecoder::~FFmpegVideoDecoder() { if (hw_device_ctx) av_buffer_unref(&hw_device_ctx); if (decoder_ctx) avcodec_free_context(&decoder_ctx); av_frame_free(&av_frame_); av_frame_free(&hw_frame_); } -bool VideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { +bool FFmpegVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { const AVCodec *decoder = avcodec_find_decoder(codecpar->codec_id); if (!decoder) return false; @@ -149,7 +159,7 @@ bool VideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { return true; } -bool VideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) { +bool FFmpegVideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) { const AVCodecHWConfig *config = nullptr; for (int i = 0; (config = avcodec_get_hw_config(decoder_ctx->codec, i)) != nullptr; i++) { if (config->methods & AV_CODEC_HW_CONFIG_METHOD_HW_DEVICE_CTX && config->device_type == hw_device_type) { @@ -175,7 +185,7 @@ bool VideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) { return true; } -bool VideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { +bool FFmpegVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { int current_idx = idx; if (idx != reader->prev_idx + 1) { // seeking to the nearest key frame @@ -219,7 +229,7 @@ bool VideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { return false; } -AVFrame *VideoDecoder::decodeFrame(AVPacket *pkt) { +AVFrame *FFmpegVideoDecoder::decodeFrame(AVPacket *pkt) { int ret = avcodec_send_packet(decoder_ctx, pkt); if (ret < 0) { rError("Error sending a packet for decoding: %d", ret); @@ -239,7 +249,7 @@ AVFrame *VideoDecoder::decodeFrame(AVPacket *pkt) { return (av_frame_->format == hw_pix_fmt) ? hw_frame_ : av_frame_; } -bool VideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) { +bool FFmpegVideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) { if (hw_pix_fmt == HW_PIX_FMT) { for (int i = 0; i < height/2; i++) { memcpy(buf->y + (i*2 + 0)*buf->stride, f->data[0] + (i*2 + 0)*f->linesize[0], width); @@ -256,3 +266,47 @@ bool VideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) { } return true; } + +#ifndef __APPLE__ +bool QcomVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { + if (codecpar->codec_id != AV_CODEC_ID_HEVC) { + rError("Hardware decoder only supports HEVC codec"); + return false; + } + width = codecpar->width; + height = codecpar->height; + msm_vidc.init(VIDEO_DEVICE, width, height, V4L2_PIX_FMT_HEVC); + return true; +} + +bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { + int from_idx = idx; + if (idx != reader->prev_idx + 1) { + // seeking to the nearest key frame + for (int i = idx; i >= 0; --i) { + if (reader->packets_info[i].flags & AV_PKT_FLAG_KEY) { + from_idx = i; + break; + } + } + + auto pos = reader->packets_info[from_idx].pos; + int ret = avformat_seek_file(reader->input_ctx, 0, pos, pos, pos, AVSEEK_FLAG_BYTE); + if (ret < 0) { + rError("Failed to seek to byte position %lld: %d", pos, AVERROR(ret)); + return false; + } + } + reader->prev_idx = idx; + bool result = false; + AVPacket pkt; + msm_vidc.avctx = reader->input_ctx; + for (int i = from_idx; i <= idx; ++i) { + if (av_read_frame(reader->input_ctx, &pkt) == 0) { + result = msm_vidc.decodeFrame(&pkt, buf) && (i == idx); + av_packet_unref(&pkt); + } + } + return result; +} +#endif diff --git a/tools/replay/framereader.h b/tools/replay/framereader.h index a15847e311..d8e86fce0f 100644 --- a/tools/replay/framereader.h +++ b/tools/replay/framereader.h @@ -7,6 +7,10 @@ #include "tools/replay/filereader.h" #include "tools/replay/util.h" +#ifndef __APPLE__ +#include "tools/replay/qcom_decoder.h" +#endif + extern "C" { #include #include @@ -40,11 +44,18 @@ public: class VideoDecoder { public: - VideoDecoder(); - ~VideoDecoder(); - bool open(AVCodecParameters *codecpar, bool hw_decoder); - bool decode(FrameReader *reader, int idx, VisionBuf *buf); + virtual ~VideoDecoder() = default; + virtual bool open(AVCodecParameters *codecpar, bool hw_decoder) = 0; + virtual bool decode(FrameReader *reader, int idx, VisionBuf *buf) = 0; int width = 0, height = 0; +}; + +class FFmpegVideoDecoder : public VideoDecoder { +public: + FFmpegVideoDecoder(); + ~FFmpegVideoDecoder() override; + bool open(AVCodecParameters *codecpar, bool hw_decoder) override; + bool decode(FrameReader *reader, int idx, VisionBuf *buf) override; private: bool initHardwareDecoder(AVHWDeviceType hw_device_type); @@ -56,3 +67,16 @@ private: AVPixelFormat hw_pix_fmt = AV_PIX_FMT_NONE; AVBufferRef *hw_device_ctx = nullptr; }; + +#ifndef __APPLE__ +class QcomVideoDecoder : public VideoDecoder { +public: + QcomVideoDecoder() {}; + ~QcomVideoDecoder() override {}; + bool open(AVCodecParameters *codecpar, bool hw_decoder) override; + bool decode(FrameReader *reader, int idx, VisionBuf *buf) override; + +private: + MsmVidc msm_vidc = MsmVidc(); +}; +#endif diff --git a/tools/replay/qcom_decoder.cc b/tools/replay/qcom_decoder.cc new file mode 100644 index 0000000000..eb5409daa3 --- /dev/null +++ b/tools/replay/qcom_decoder.cc @@ -0,0 +1,346 @@ +#include "qcom_decoder.h" + +#include +#include "third_party/linux/include/v4l2-controls.h" +#include + + +#include "common/swaglog.h" +#include "common/util.h" + +// echo "0xFFFF" > /sys/kernel/debug/msm_vidc/debug_level + +static void copyBuffer(VisionBuf *src_buf, VisionBuf *dst_buf) { + // Copy Y plane + memcpy(dst_buf->y, src_buf->y, src_buf->height * src_buf->stride); + // Copy UV plane + memcpy(dst_buf->uv, src_buf->uv, src_buf->height / 2 * src_buf->stride); +} + +static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) { + struct v4l2_requestbuffers reqbuf = { + .count = count, + .type = buf_type, + .memory = V4L2_MEMORY_USERPTR + }; + util::safe_ioctl(fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed"); +} + +MsmVidc::~MsmVidc() { + if (fd > 0) { + close(fd); + } +} + +bool MsmVidc::init(const char* dev, size_t width, size_t height, uint64_t codec) { + LOG("Initializing msm_vidc device %s", dev); + this->w = width; + this->h = height; + this->fd = open(dev, O_RDWR, 0); + if (fd < 0) { + LOGE("failed to open video device %s", dev); + return false; + } + subscribeEvents(); + v4l2_buf_type out_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + setPlaneFormat(out_type, V4L2_PIX_FMT_HEVC); // Also allocates the output buffer + setFPS(FPS); + request_buffers(fd, out_type, OUTPUT_BUFFER_COUNT); + util::safe_ioctl(fd, VIDIOC_STREAMON, &out_type, "VIDIOC_STREAMON OUTPUT failed"); + restartCapture(); + setupPolling(); + + this->initialized = true; + return true; +} + +VisionBuf* MsmVidc::decodeFrame(AVPacket *pkt, VisionBuf *buf) { + assert(initialized && (pkt != nullptr) && (buf != nullptr)); + + this->frame_ready = false; + this->current_output_buf = buf; + bool sent_packet = false; + + while (!this->frame_ready) { + if (!sent_packet) { + int buf_index = getBufferUnlocked(); + if (buf_index >= 0) { + assert(buf_index < out_buf_cnt); + sendPacket(buf_index, pkt); + sent_packet = true; + } + } + + if (poll(pfd, nfds, -1) < 0) { + LOGE("poll() error: %d", errno); + return nullptr; + } + + if (VisionBuf* result = processEvents()) { + return result; + } + } + + return buf; +} + +VisionBuf* MsmVidc::processEvents() { + for (int idx = 0; idx < nfds; idx++) { + short revents = pfd[idx].revents; + if (!revents) continue; + + if (idx == ev[EV_VIDEO]) { + if (revents & (POLLIN | POLLRDNORM)) { + VisionBuf *result = handleCapture(); + if (result == this->current_output_buf) { + this->frame_ready = true; + } + } + if (revents & (POLLOUT | POLLWRNORM)) { + handleOutput(); + } + if (revents & POLLPRI) { + handleEvent(); + } + } else { + LOGE("Unexpected event on fd %d", pfd[idx].fd); + } + } + return nullptr; +} + +VisionBuf* MsmVidc::handleCapture() { + struct v4l2_buffer buf = {0}; + struct v4l2_plane planes[1] = {0}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + buf.memory = V4L2_MEMORY_USERPTR; + buf.m.planes = planes; + buf.length = 1; + util::safe_ioctl(this->fd, VIDIOC_DQBUF, &buf, "VIDIOC_DQBUF CAPTURE failed"); + + if (this->reconfigure_pending || buf.m.planes[0].bytesused == 0) { + return nullptr; + } + + copyBuffer(&cap_bufs[buf.index], this->current_output_buf); + queueCaptureBuffer(buf.index); + return this->current_output_buf; +} + +bool MsmVidc::subscribeEvents() { + for (uint32_t event : subscriptions) { + struct v4l2_event_subscription sub = { .type = event}; + util::safe_ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "VIDIOC_SUBSCRIBE_EVENT failed"); + } + return true; +} + +bool MsmVidc::setPlaneFormat(enum v4l2_buf_type type, uint32_t fourcc) { + struct v4l2_format fmt = {.type = type}; + struct v4l2_pix_format_mplane *pix = &fmt.fmt.pix_mp; + *pix = { + .width = (__u32)this->w, + .height = (__u32)this->h, + .pixelformat = fourcc + }; + util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt, "VIDIOC_S_FMT failed"); + if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) { + this->out_buf_size = pix->plane_fmt[0].sizeimage; + int ion_size = this->out_buf_size * OUTPUT_BUFFER_COUNT; // Output (input) buffers are ION buffer. + this->out_buf.allocate(ion_size); // mmap rw + for (int i = 0; i < OUTPUT_BUFFER_COUNT; i++) { + this->out_buf_off[i] = i * this->out_buf_size; + this->out_buf_addr[i] = (char *)this->out_buf.addr + this->out_buf_off[i]; + this->out_buf_flag[i] = false; + } + LOGD("Set output buffer size to %d, count %d, addr %p", this->out_buf_size, OUTPUT_BUFFER_COUNT, this->out_buf.addr); + } else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) { + request_buffers(this->fd, type, CAPTURE_BUFFER_COUNT); + util::safe_ioctl(fd, VIDIOC_G_FMT, &fmt, "VIDIOC_G_FMT failed"); + const __u32 y_size = pix->plane_fmt[0].sizeimage; + const __u32 y_stride = pix->plane_fmt[0].bytesperline; + for (int i = 0; i < CAPTURE_BUFFER_COUNT; i++) { + size_t uv_offset = (size_t)y_stride * pix->height; + size_t required = uv_offset + (y_stride * pix->height / 2); // enough for Y + UV. For linear NV12, UV plane starts at y_stride * height. + size_t alloc_size = std::max(y_size, required); + this->cap_bufs[i].allocate(alloc_size); + this->cap_bufs[i].init_yuv(pix->width, pix->height, y_stride, uv_offset); + } + LOGD("Set capture buffer size to %d, count %d, addr %p, extradata size %d", + pix->plane_fmt[0].sizeimage, CAPTURE_BUFFER_COUNT, this->cap_bufs[0].addr, pix->plane_fmt[1].sizeimage); + } + return true; +} + +bool MsmVidc::setFPS(uint32_t fps) { + struct v4l2_streamparm streamparam = { + .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, + .parm.output.timeperframe = {1, fps} + }; + util::safe_ioctl(fd, VIDIOC_S_PARM, &streamparam, "VIDIOC_S_PARM failed"); + return true; +} + +bool MsmVidc::restartCapture() { + // stop if already initialized + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + if (this->initialized) { + LOGD("Restarting capture, flushing buffers..."); + util::safe_ioctl(this->fd, VIDIOC_STREAMOFF, &type, "VIDIOC_STREAMOFF CAPTURE failed"); + struct v4l2_requestbuffers reqbuf = {.type = type, .memory = V4L2_MEMORY_USERPTR}; + util::safe_ioctl(this->fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed"); + for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) { + this->cap_bufs[i].free(); + this->cap_buf_flag[i] = false; // mark as not queued + cap_bufs[i].~VisionBuf(); + new (&cap_bufs[i]) VisionBuf(); + } + } + // setup, start and queue capture buffers + setDBP(); + setPlaneFormat(type, V4L2_PIX_FMT_NV12); + util::safe_ioctl(this->fd, VIDIOC_STREAMON, &type, "VIDIOC_STREAMON CAPTURE failed"); + for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) { + queueCaptureBuffer(i); + } + + return true; +} + +bool MsmVidc::queueCaptureBuffer(int i) { + struct v4l2_buffer buf = {0}; + struct v4l2_plane planes[1] = {0}; + + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + buf.memory = V4L2_MEMORY_USERPTR; + buf.index = i; + buf.m.planes = planes; + buf.length = 1; + // decoded frame plane + planes[0].m.userptr = (unsigned long)this->cap_bufs[i].addr; // no security + planes[0].length = this->cap_bufs[i].len; + planes[0].reserved[0] = this->cap_bufs[i].fd; // ION fd + planes[0].reserved[1] = 0; + planes[0].bytesused = this->cap_bufs[i].len; + planes[0].data_offset = 0; + util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed"); + this->cap_buf_flag[i] = true; // mark as queued + return true; +} + +bool MsmVidc::queueOutputBuffer(int i, size_t size) { + struct v4l2_buffer buf = {0}; + struct v4l2_plane planes[1] = {0}; + + buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + buf.memory = V4L2_MEMORY_USERPTR; + buf.index = i; + buf.m.planes = planes; + buf.length = 1; + // decoded frame plane + planes[0].m.userptr = (unsigned long)this->out_buf_off[i]; // check this + planes[0].length = this->out_buf_size; + planes[0].reserved[0] = this->out_buf.fd; // ION fd + planes[0].reserved[1] = 0; + planes[0].bytesused = size; + planes[0].data_offset = 0; + assert((this->out_buf_off[i] & 0xfff) == 0); // must be 4 KiB aligned + assert(this->out_buf_size % 4096 == 0); // ditto for size + + util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed"); + this->out_buf_flag[i] = true; // mark as queued + return true; +} + +bool MsmVidc::setDBP() { + struct v4l2_ext_control control[2] = {0}; + struct v4l2_ext_controls controls = {0}; + control[0].id = V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE; + control[0].value = 1; // V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_SECONDARY + control[1].id = V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT; + control[1].value = 0; // V4L2_MPEG_VIDC_VIDEO_DPB_COLOR_FMT_NONE + controls.count = 2; + controls.ctrl_class = V4L2_CTRL_CLASS_MPEG; + controls.controls = control; + util::safe_ioctl(fd, VIDIOC_S_EXT_CTRLS, &controls, "VIDIOC_S_EXT_CTRLS failed"); + return true; +} + +bool MsmVidc::setupPolling() { + // Initialize poll array + pfd[EV_VIDEO] = {fd, POLLIN | POLLOUT | POLLWRNORM | POLLRDNORM | POLLPRI, 0}; + ev[EV_VIDEO] = EV_VIDEO; + nfds = 1; + return true; +} + +bool MsmVidc::sendPacket(int buf_index, AVPacket *pkt) { + assert(buf_index >= 0 && buf_index < out_buf_cnt); + assert(pkt != nullptr && pkt->data != nullptr && pkt->size > 0); + // Prepare output buffer + memset(this->out_buf_addr[buf_index], 0, this->out_buf_size); + uint8_t * data = (uint8_t *)this->out_buf_addr[buf_index]; + memcpy(data, pkt->data, pkt->size); + queueOutputBuffer(buf_index, pkt->size); + return true; +} + +int MsmVidc::getBufferUnlocked() { + for (int i = 0; i < this->out_buf_cnt; i++) { + if (!out_buf_flag[i]) { + return i; + } + } + return -1; +} + + +bool MsmVidc::handleOutput() { + struct v4l2_buffer buf = {0}; + struct v4l2_plane planes[1]; + buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + buf.memory = V4L2_MEMORY_USERPTR; + buf.m.planes = planes; + buf.length = 1; + util::safe_ioctl(this->fd, VIDIOC_DQBUF, &buf, "VIDIOC_DQBUF OUTPUT failed"); + this->out_buf_flag[buf.index] = false; // mark as not queued + return true; +} + +bool MsmVidc::handleEvent() { + // dequeue event + struct v4l2_event event = {0}; + util::safe_ioctl(this->fd, VIDIOC_DQEVENT, &event, "VIDIOC_DQEVENT failed"); + switch (event.type) { + case V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT: { + unsigned int *ptr = (unsigned int *)event.u.data; + unsigned int height = ptr[0]; + unsigned int width = ptr[1]; + this->w = width; + this->h = height; + LOGD("Port Reconfig received insufficient, new size %ux%u, flushing capture bufs...", width, height); // This is normal + struct v4l2_decoder_cmd dec; + dec.flags = V4L2_QCOM_CMD_FLUSH_CAPTURE; + dec.cmd = V4L2_QCOM_CMD_FLUSH; + util::safe_ioctl(this->fd, VIDIOC_DECODER_CMD, &dec, "VIDIOC_DECODER_CMD FLUSH_CAPTURE failed"); + this->reconfigure_pending = true; + LOGD("Waiting for flush done event to reconfigure capture queue"); + break; + } + + case V4L2_EVENT_MSM_VIDC_FLUSH_DONE: { + unsigned int *ptr = (unsigned int *)event.u.data; + unsigned int flags = ptr[0]; + if (flags & V4L2_QCOM_CMD_FLUSH_CAPTURE) { + if (this->reconfigure_pending) { + this->restartCapture(); + this->reconfigure_pending = false; + } + } + break; + } + default: + break; + } + return true; +} \ No newline at end of file diff --git a/tools/replay/qcom_decoder.h b/tools/replay/qcom_decoder.h new file mode 100644 index 0000000000..1d7522cc70 --- /dev/null +++ b/tools/replay/qcom_decoder.h @@ -0,0 +1,88 @@ +#pragma once + +#include +#include + +#include "msgq/visionipc/visionbuf.h" + +extern "C" { + #include + #include +} + +#define V4L2_EVENT_MSM_VIDC_START (V4L2_EVENT_PRIVATE_START + 0x00001000) +#define V4L2_EVENT_MSM_VIDC_FLUSH_DONE (V4L2_EVENT_MSM_VIDC_START + 1) +#define V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT (V4L2_EVENT_MSM_VIDC_START + 3) +#define V4L2_CID_MPEG_MSM_VIDC_BASE 0x00992000 +#define V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT (V4L2_CID_MPEG_MSM_VIDC_BASE + 44) +#define V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE (V4L2_CID_MPEG_MSM_VIDC_BASE + 22) +#define V4L2_QCOM_CMD_FLUSH_CAPTURE (1 << 1) +#define V4L2_QCOM_CMD_FLUSH (4) + +#define VIDEO_DEVICE "/dev/video32" +#define OUTPUT_BUFFER_COUNT 8 +#define CAPTURE_BUFFER_COUNT 8 +#define FPS 20 + + +class MsmVidc { +public: + MsmVidc() = default; + ~MsmVidc(); + + bool init(const char* dev, size_t width, size_t height, uint64_t codec); + VisionBuf* decodeFrame(AVPacket* pkt, VisionBuf* buf); + + AVFormatContext* avctx = nullptr; + int fd = 0; + +private: + bool initialized = false; + bool reconfigure_pending = false; + bool frame_ready = false; + + VisionBuf* current_output_buf = nullptr; + VisionBuf out_buf; // Single input buffer + VisionBuf cap_bufs[CAPTURE_BUFFER_COUNT]; // Capture (output) buffers + + size_t w = 1928, h = 1208; + size_t cap_height = 0, cap_width = 0; + + int cap_buf_size = 0; + int out_buf_size = 0; + + size_t cap_plane_off[CAPTURE_BUFFER_COUNT] = {0}; + size_t cap_plane_stride[CAPTURE_BUFFER_COUNT] = {0}; + bool cap_buf_flag[CAPTURE_BUFFER_COUNT] = {false}; + + size_t out_buf_off[OUTPUT_BUFFER_COUNT] = {0}; + void* out_buf_addr[OUTPUT_BUFFER_COUNT] = {0}; + bool out_buf_flag[OUTPUT_BUFFER_COUNT] = {false}; + const int out_buf_cnt = OUTPUT_BUFFER_COUNT; + + const int subscriptions[2] = { + V4L2_EVENT_MSM_VIDC_FLUSH_DONE, + V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT + }; + + enum { EV_VIDEO, EV_COUNT }; + struct pollfd pfd[EV_COUNT] = {0}; + int ev[EV_COUNT] = {-1}; + int nfds = 0; + + VisionBuf* processEvents(); + bool setupOutput(); + bool subscribeEvents(); + bool setPlaneFormat(v4l2_buf_type type, uint32_t fourcc); + bool setFPS(uint32_t fps); + bool restartCapture(); + bool queueCaptureBuffer(int i); + bool queueOutputBuffer(int i, size_t size); + bool setDBP(); + bool setupPolling(); + bool sendPacket(int buf_index, AVPacket* pkt); + int getBufferUnlocked(); + VisionBuf* handleCapture(); + bool handleOutput(); + bool handleEvent(); +}; diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 2681b26904..68ff3050db 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -11,7 +11,7 @@ from openpilot.tools.sim.lib.common import SimulatorState class SimulatedCar: """Simulates a honda civic 2022 (panda state + can messages) to OpenPilot""" - packer = CANPacker("honda_civic_ex_2022_can_generated") + packer = CANPacker("honda_bosch_radarless_generated") def __init__(self): self.pm = messaging.PubMaster(['can', 'pandaStates']) @@ -23,7 +23,7 @@ class SimulatedCar: @staticmethod def get_car_can_parser(): - dbc_f = 'honda_civic_ex_2022_can_generated' + dbc_f = 'honda_bosch_radarless_generated' checks = [] return CANParser(dbc_f, checks, 0) diff --git a/uv.lock b/uv.lock index 47d09f3a57..b871cc8da4 100644 --- a/uv.lock +++ b/uv.lock @@ -451,6 +451,21 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/d2/fc/c0a3f4c4eaa5a22fbef91713474666e13d0ea2a69c84532579490a9f2cc8/dbus_next-0.2.3-py3-none-any.whl", hash = "sha256:58948f9aff9db08316734c0be2a120f6dc502124d9642f55e90ac82ffb16a18b", size = 57885, upload-time = "2021-07-25T22:11:25.466Z" }, ] +[[package]] +name = "dearpygui" +version = "2.1.0" +source = { registry = "https://pypi.org/simple" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/92/fe/66293fc40254a29f060efd3398f2b1001ed79263ae1837db9ec42caa8f1d/dearpygui-2.1.0-cp311-cp311-macosx_10_6_x86_64.whl", hash = "sha256:03e5dc0b3dd2f7965e50bbe41f3316a814408064b582586de994d93afedb125c", size = 2100924, upload-time = "2025-07-07T14:20:00.602Z" }, + { url = "https://files.pythonhosted.org/packages/c4/4d/9fa1c3156ba7bbf4dc89e2e322998752fccfdc3575923a98dd6a4da48911/dearpygui-2.1.0-cp311-cp311-macosx_13_0_arm64.whl", hash = "sha256:b5b37710c3fa135c48e2347f39ecd1f415146e86db5d404707a0bf72d16bd304", size = 1874441, upload-time = "2025-07-07T14:20:09.165Z" }, + { url = "https://files.pythonhosted.org/packages/5a/3c/af5673b50699e1734296a0b5bcef39bb6989175b001ad1f9b0e7888ad90d/dearpygui-2.1.0-cp311-cp311-manylinux1_x86_64.whl", hash = "sha256:b0cfd7ac7eaa090fc22d6aa60fc4b527fc631cee10c348e4d8df92bb39af03d2", size = 2636574, upload-time = "2025-07-07T14:20:14.951Z" }, + { url = "https://files.pythonhosted.org/packages/7f/db/ed4db0bb3d88e7a8c405472641419086bef9632c4b8b0489dc0c43519c0d/dearpygui-2.1.0-cp311-cp311-win_amd64.whl", hash = "sha256:a9af54f96d3ef30c5db9d12cdf3266f005507396fb0da2e12e6b22b662161070", size = 1810266, upload-time = "2025-07-07T14:19:51.565Z" }, + { url = "https://files.pythonhosted.org/packages/55/9d/20a55786cc9d9266395544463d5db3be3528f7d5244bc52ba760de5dcc2d/dearpygui-2.1.0-cp312-cp312-macosx_10_6_x86_64.whl", hash = "sha256:1270ceb9cdb8ecc047c42477ccaa075b7864b314a5d09191f9280a24c8aa90a0", size = 2101499, upload-time = "2025-07-07T14:20:01.701Z" }, + { url = "https://files.pythonhosted.org/packages/a7/b2/39d820796b7ac4d0ebf93306c1f031bf3516b159408286f1fb495c6babeb/dearpygui-2.1.0-cp312-cp312-macosx_13_0_arm64.whl", hash = "sha256:ce9969eb62057b9d4c88a8baaed13b5fbe4058caa9faf5b19fec89da75aece3d", size = 1874385, upload-time = "2025-07-07T14:20:11.226Z" }, + { url = "https://files.pythonhosted.org/packages/fc/26/c29998ffeb5eb8d638f307851e51a81c8bd4aeaf89ad660fc67ea4d1ac1a/dearpygui-2.1.0-cp312-cp312-manylinux1_x86_64.whl", hash = "sha256:a3ca8cf788db63ef7e2e8d6f277631b607d548b37606f080ca1b42b1f0a9b183", size = 2635863, upload-time = "2025-07-07T14:20:17.186Z" }, + { url = "https://files.pythonhosted.org/packages/28/9c/3ab33927f1d8c839c5b7033a33d44fc9f0aeb00c264fc9772cb7555a03c4/dearpygui-2.1.0-cp312-cp312-win_amd64.whl", hash = "sha256:43f0e4db9402f44fc3683a1f5c703564819de18cc15a042de7f1ed1c8cb5d148", size = 1810460, upload-time = "2025-07-07T14:19:53.13Z" }, +] + [[package]] name = "dictdiffer" version = "0.9.0" @@ -510,27 +525,27 @@ wheels = [ [[package]] name = "fonttools" -version = "4.59.1" +version = "4.59.2" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/11/7f/29c9c3fe4246f6ad96fee52b88d0dc3a863c7563b0afc959e36d78b965dc/fonttools-4.59.1.tar.gz", hash = "sha256:74995b402ad09822a4c8002438e54940d9f1ecda898d2bb057729d7da983e4cb", size = 3534394, upload-time = "2025-08-14T16:28:14.266Z" } +sdist = { url = "https://files.pythonhosted.org/packages/0d/a5/fba25f9fbdab96e26dedcaeeba125e5f05a09043bf888e0305326e55685b/fonttools-4.59.2.tar.gz", hash = "sha256:e72c0749b06113f50bcb80332364c6be83a9582d6e3db3fe0b280f996dc2ef22", size = 3540889, upload-time = "2025-08-27T16:40:30.97Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/34/62/9667599561f623d4a523cc9eb4f66f3b94b6155464110fa9aebbf90bbec7/fonttools-4.59.1-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:4909cce2e35706f3d18c54d3dcce0414ba5e0fb436a454dffec459c61653b513", size = 2778815, upload-time = "2025-08-14T16:26:28.484Z" }, - { url = "https://files.pythonhosted.org/packages/8f/78/cc25bcb2ce86033a9df243418d175e58f1956a35047c685ef553acae67d6/fonttools-4.59.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:efbec204fa9f877641747f2d9612b2b656071390d7a7ef07a9dbf0ecf9c7195c", size = 2341631, upload-time = "2025-08-14T16:26:30.396Z" }, - { url = "https://files.pythonhosted.org/packages/a4/cc/fcbb606dd6871f457ac32f281c20bcd6cc77d9fce77b5a4e2b2afab1f500/fonttools-4.59.1-cp311-cp311-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:39dfd42cc2dc647b2c5469bc7a5b234d9a49e72565b96dd14ae6f11c2c59ef15", size = 5022222, upload-time = "2025-08-14T16:26:32.447Z" }, - { url = "https://files.pythonhosted.org/packages/61/96/c0b1cf2b74d08eb616a80dbf5564351fe4686147291a25f7dce8ace51eb3/fonttools-4.59.1-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:b11bc177a0d428b37890825d7d025040d591aa833f85f8d8878ed183354f47df", size = 4966512, upload-time = "2025-08-14T16:26:34.621Z" }, - { url = "https://files.pythonhosted.org/packages/a4/26/51ce2e3e0835ffc2562b1b11d1fb9dafd0aca89c9041b64a9e903790a761/fonttools-4.59.1-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:5b9b4c35b3be45e5bc774d3fc9608bbf4f9a8d371103b858c80edbeed31dd5aa", size = 5001645, upload-time = "2025-08-14T16:26:36.876Z" }, - { url = "https://files.pythonhosted.org/packages/36/11/ef0b23f4266349b6d5ccbd1a07b7adc998d5bce925792aa5d1ec33f593e3/fonttools-4.59.1-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:01158376b8a418a0bae9625c476cebfcfcb5e6761e9d243b219cd58341e7afbb", size = 5113777, upload-time = "2025-08-14T16:26:39.002Z" }, - { url = "https://files.pythonhosted.org/packages/d0/da/b398fe61ef433da0a0472cdb5d4399124f7581ffe1a31b6242c91477d802/fonttools-4.59.1-cp311-cp311-win32.whl", hash = "sha256:cf7c5089d37787387123f1cb8f1793a47c5e1e3d1e4e7bfbc1cc96e0f925eabe", size = 2215076, upload-time = "2025-08-14T16:26:41.196Z" }, - { url = "https://files.pythonhosted.org/packages/94/bd/e2624d06ab94e41c7c77727b2941f1baed7edb647e63503953e6888020c9/fonttools-4.59.1-cp311-cp311-win_amd64.whl", hash = "sha256:c866eef7a0ba320486ade6c32bfc12813d1a5db8567e6904fb56d3d40acc5116", size = 2262779, upload-time = "2025-08-14T16:26:43.483Z" }, - { url = "https://files.pythonhosted.org/packages/ac/fe/6e069cc4cb8881d164a9bd956e9df555bc62d3eb36f6282e43440200009c/fonttools-4.59.1-cp312-cp312-macosx_10_13_universal2.whl", hash = "sha256:43ab814bbba5f02a93a152ee61a04182bb5809bd2bc3609f7822e12c53ae2c91", size = 2769172, upload-time = "2025-08-14T16:26:45.729Z" }, - { url = "https://files.pythonhosted.org/packages/b9/98/ec4e03f748fefa0dd72d9d95235aff6fef16601267f4a2340f0e16b9330f/fonttools-4.59.1-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:4f04c3ffbfa0baafcbc550657cf83657034eb63304d27b05cff1653b448ccff6", size = 2337281, upload-time = "2025-08-14T16:26:47.921Z" }, - { url = "https://files.pythonhosted.org/packages/8b/b1/890360a7e3d04a30ba50b267aca2783f4c1364363797e892e78a4f036076/fonttools-4.59.1-cp312-cp312-manylinux1_x86_64.manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_5_x86_64.whl", hash = "sha256:d601b153e51a5a6221f0d4ec077b6bfc6ac35bfe6c19aeaa233d8990b2b71726", size = 4909215, upload-time = "2025-08-14T16:26:49.682Z" }, - { url = "https://files.pythonhosted.org/packages/8a/ec/2490599550d6c9c97a44c1e36ef4de52d6acf742359eaa385735e30c05c4/fonttools-4.59.1-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:c735e385e30278c54f43a0d056736942023c9043f84ee1021eff9fd616d17693", size = 4951958, upload-time = "2025-08-14T16:26:51.616Z" }, - { url = "https://files.pythonhosted.org/packages/d1/40/bd053f6f7634234a9b9805ff8ae4f32df4f2168bee23cafd1271ba9915a9/fonttools-4.59.1-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:1017413cdc8555dce7ee23720da490282ab7ec1cf022af90a241f33f9a49afc4", size = 4894738, upload-time = "2025-08-14T16:26:53.836Z" }, - { url = "https://files.pythonhosted.org/packages/ac/a1/3cd12a010d288325a7cfcf298a84825f0f9c29b01dee1baba64edfe89257/fonttools-4.59.1-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:5c6d8d773470a5107052874341ed3c487c16ecd179976d81afed89dea5cd7406", size = 5045983, upload-time = "2025-08-14T16:26:56.153Z" }, - { url = "https://files.pythonhosted.org/packages/a2/af/8a2c3f6619cc43cf87951405337cc8460d08a4e717bb05eaa94b335d11dc/fonttools-4.59.1-cp312-cp312-win32.whl", hash = "sha256:2a2d0d33307f6ad3a2086a95dd607c202ea8852fa9fb52af9b48811154d1428a", size = 2203407, upload-time = "2025-08-14T16:26:58.165Z" }, - { url = "https://files.pythonhosted.org/packages/8e/f2/a19b874ddbd3ebcf11d7e25188ef9ac3f68b9219c62263acb34aca8cde05/fonttools-4.59.1-cp312-cp312-win_amd64.whl", hash = "sha256:0b9e4fa7eaf046ed6ac470f6033d52c052481ff7a6e0a92373d14f556f298dc0", size = 2251561, upload-time = "2025-08-14T16:27:00.646Z" }, - { url = "https://files.pythonhosted.org/packages/0f/64/9d606e66d498917cd7a2ff24f558010d42d6fd4576d9dd57f0bd98333f5a/fonttools-4.59.1-py3-none-any.whl", hash = "sha256:647db657073672a8330608970a984d51573557f328030566521bc03415535042", size = 1130094, upload-time = "2025-08-14T16:28:12.048Z" }, + { url = "https://files.pythonhosted.org/packages/f8/53/742fcd750ae0bdc74de4c0ff923111199cc2f90a4ee87aaddad505b6f477/fonttools-4.59.2-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:511946e8d7ea5c0d6c7a53c4cb3ee48eda9ab9797cd9bf5d95829a398400354f", size = 2774961, upload-time = "2025-08-27T16:38:47.536Z" }, + { url = "https://files.pythonhosted.org/packages/57/2a/976f5f9fa3b4dd911dc58d07358467bec20e813d933bc5d3db1a955dd456/fonttools-4.59.2-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:8e5e2682cf7be766d84f462ba8828d01e00c8751a8e8e7ce12d7784ccb69a30d", size = 2344690, upload-time = "2025-08-27T16:38:49.723Z" }, + { url = "https://files.pythonhosted.org/packages/c1/8f/b7eefc274fcf370911e292e95565c8253b0b87c82a53919ab3c795a4f50e/fonttools-4.59.2-cp311-cp311-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:5729e12a982dba3eeae650de48b06f3b9ddb51e9aee2fcaf195b7d09a96250e2", size = 5026910, upload-time = "2025-08-27T16:38:51.904Z" }, + { url = "https://files.pythonhosted.org/packages/69/95/864726eaa8f9d4e053d0c462e64d5830ec7c599cbdf1db9e40f25ca3972e/fonttools-4.59.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:c52694eae5d652361d59ecdb5a2246bff7cff13b6367a12da8499e9df56d148d", size = 4971031, upload-time = "2025-08-27T16:38:53.676Z" }, + { url = "https://files.pythonhosted.org/packages/24/4c/b8c4735ebdea20696277c70c79e0de615dbe477834e5a7c2569aa1db4033/fonttools-4.59.2-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:f1f1bbc23ba1312bd8959896f46f667753b90216852d2a8cfa2d07e0cb234144", size = 5006112, upload-time = "2025-08-27T16:38:55.69Z" }, + { url = "https://files.pythonhosted.org/packages/3b/23/f9ea29c292aa2fc1ea381b2e5621ac436d5e3e0a5dee24ffe5404e58eae8/fonttools-4.59.2-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:1a1bfe5378962825dabe741720885e8b9ae9745ec7ecc4a5ec1f1ce59a6062bf", size = 5117671, upload-time = "2025-08-27T16:38:58.984Z" }, + { url = "https://files.pythonhosted.org/packages/ba/07/cfea304c555bf06e86071ff2a3916bc90f7c07ec85b23bab758d4908c33d/fonttools-4.59.2-cp311-cp311-win32.whl", hash = "sha256:e937790f3c2c18a1cbc7da101550a84319eb48023a715914477d2e7faeaba570", size = 2218157, upload-time = "2025-08-27T16:39:00.75Z" }, + { url = "https://files.pythonhosted.org/packages/d7/de/35d839aa69db737a3f9f3a45000ca24721834d40118652a5775d5eca8ebb/fonttools-4.59.2-cp311-cp311-win_amd64.whl", hash = "sha256:9836394e2f4ce5f9c0a7690ee93bd90aa1adc6b054f1a57b562c5d242c903104", size = 2265846, upload-time = "2025-08-27T16:39:02.453Z" }, + { url = "https://files.pythonhosted.org/packages/ba/3d/1f45db2df51e7bfa55492e8f23f383d372200be3a0ded4bf56a92753dd1f/fonttools-4.59.2-cp312-cp312-macosx_10_13_universal2.whl", hash = "sha256:82906d002c349cad647a7634b004825a7335f8159d0d035ae89253b4abf6f3ea", size = 2769711, upload-time = "2025-08-27T16:39:04.423Z" }, + { url = "https://files.pythonhosted.org/packages/29/df/cd236ab32a8abfd11558f296e064424258db5edefd1279ffdbcfd4fd8b76/fonttools-4.59.2-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:a10c1bd7644dc58f8862d8ba0cf9fb7fef0af01ea184ba6ce3f50ab7dfe74d5a", size = 2340225, upload-time = "2025-08-27T16:39:06.143Z" }, + { url = "https://files.pythonhosted.org/packages/98/12/b6f9f964fe6d4b4dd4406bcbd3328821c3de1f909ffc3ffa558fe72af48c/fonttools-4.59.2-cp312-cp312-manylinux1_x86_64.manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_5_x86_64.whl", hash = "sha256:738f31f23e0339785fd67652a94bc69ea49e413dfdb14dcb8c8ff383d249464e", size = 4912766, upload-time = "2025-08-27T16:39:08.138Z" }, + { url = "https://files.pythonhosted.org/packages/73/78/82bde2f2d2c306ef3909b927363170b83df96171f74e0ccb47ad344563cd/fonttools-4.59.2-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:0ec99f9bdfee9cdb4a9172f9e8fd578cce5feb231f598909e0aecf5418da4f25", size = 4955178, upload-time = "2025-08-27T16:39:10.094Z" }, + { url = "https://files.pythonhosted.org/packages/92/77/7de766afe2d31dda8ee46d7e479f35c7d48747e558961489a2d6e3a02bd4/fonttools-4.59.2-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:0476ea74161322e08c7a982f83558a2b81b491509984523a1a540baf8611cc31", size = 4897898, upload-time = "2025-08-27T16:39:12.087Z" }, + { url = "https://files.pythonhosted.org/packages/c5/77/ce0e0b905d62a06415fda9f2b2e109a24a5db54a59502b769e9e297d2242/fonttools-4.59.2-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:95922a922daa1f77cc72611747c156cfb38030ead72436a2c551d30ecef519b9", size = 5049144, upload-time = "2025-08-27T16:39:13.84Z" }, + { url = "https://files.pythonhosted.org/packages/d9/ea/870d93aefd23fff2e07cbeebdc332527868422a433c64062c09d4d5e7fe6/fonttools-4.59.2-cp312-cp312-win32.whl", hash = "sha256:39ad9612c6a622726a6a130e8ab15794558591f999673f1ee7d2f3d30f6a3e1c", size = 2206473, upload-time = "2025-08-27T16:39:15.854Z" }, + { url = "https://files.pythonhosted.org/packages/61/c4/e44bad000c4a4bb2e9ca11491d266e857df98ab6d7428441b173f0fe2517/fonttools-4.59.2-cp312-cp312-win_amd64.whl", hash = "sha256:980fd7388e461b19a881d35013fec32c713ffea1fc37aef2f77d11f332dfd7da", size = 2254706, upload-time = "2025-08-27T16:39:17.893Z" }, + { url = "https://files.pythonhosted.org/packages/65/a4/d2f7be3c86708912c02571db0b550121caab8cd88a3c0aacb9cfa15ea66e/fonttools-4.59.2-py3-none-any.whl", hash = "sha256:8bd0f759020e87bb5d323e6283914d9bf4ae35a7307dafb2cbd1e379e720ad37", size = 1132315, upload-time = "2025-08-27T16:40:28.984Z" }, ] [[package]] @@ -622,10 +637,10 @@ name = "gymnasium" version = "1.2.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "cloudpickle" }, - { name = "farama-notifications" }, - { name = "numpy" }, - { name = "typing-extensions" }, + { name = "cloudpickle", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "farama-notifications", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "numpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "typing-extensions", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/fd/17/c2a0e15c2cd5a8e788389b280996db927b923410de676ec5c7b2695e9261/gymnasium-1.2.0.tar.gz", hash = "sha256:344e87561012558f603880baf264ebc97f8a5c997a957b0c9f910281145534b0", size = 821142, upload-time = "2025-06-27T08:21:20.262Z" } wheels = [ @@ -690,6 +705,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/15/aa/0aca39a37d3c7eb941ba736ede56d689e7be91cab5d9ca846bde3999eba6/isodate-0.7.2-py3-none-any.whl", hash = "sha256:28009937d8031054830160fce6d409ed342816b543597cece116d966c6d99e15", size = 22320, upload-time = "2024-10-08T23:04:09.501Z" }, ] +[[package]] +name = "jeepney" +version = "0.9.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/7b/6f/357efd7602486741aa73ffc0617fb310a29b588ed0fd69c2399acbb85b0c/jeepney-0.9.0.tar.gz", hash = "sha256:cf0e9e845622b81e4a28df94c40345400256ec608d0e55bb8a3feaa9163f5732", size = 106758, upload-time = "2025-02-27T18:51:01.684Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/b2/a3/e137168c9c44d18eff0376253da9f1e9234d0239e0ee230d2fee6cea8e55/jeepney-0.9.0-py3-none-any.whl", hash = "sha256:97e5714520c16fc0a45695e5365a2e11b81ea79bba796e26f9f1d178cb182683", size = 49010, upload-time = "2025-02-27T18:51:00.104Z" }, +] + [[package]] name = "jinja2" version = "3.1.6" @@ -711,6 +735,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/94/9e/820c4b086ad01ba7d77369fb8b11470a01fac9b4977f02e18659cf378b6b/json_rpc-1.15.0-py2.py3-none-any.whl", hash = "sha256:4a4668bbbe7116feb4abbd0f54e64a4adcf4b8f648f19ffa0848ad0f6606a9bf", size = 39450, upload-time = "2023-06-11T09:45:47.136Z" }, ] +[[package]] +name = "kaitaistruct" +version = "0.10" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/54/04/dd60b9cb65d580ef6cb6eaee975ad1bdd22d46a3f51b07a1e0606710ea88/kaitaistruct-0.10.tar.gz", hash = "sha256:a044dee29173d6afbacf27bcac39daf89b654dd418cfa009ab82d9178a9ae52a", size = 7061, upload-time = "2022-07-09T00:34:06.729Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/4e/bf/88ad23efc08708bda9a2647169828e3553bb2093a473801db61f75356395/kaitaistruct-0.10-py2.py3-none-any.whl", hash = "sha256:a97350919adbf37fda881f75e9365e2fb88d04832b7a4e57106ec70119efb235", size = 7013, upload-time = "2022-07-09T00:34:03.905Z" }, +] + [[package]] name = "kiwisolver" version = "1.4.9" @@ -846,7 +879,7 @@ wheels = [ [[package]] name = "matplotlib" -version = "3.10.5" +version = "3.10.6" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "contourpy" }, @@ -859,25 +892,25 @@ dependencies = [ { name = "pyparsing" }, { name = "python-dateutil" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/43/91/f2939bb60b7ebf12478b030e0d7f340247390f402b3b189616aad790c366/matplotlib-3.10.5.tar.gz", hash = "sha256:352ed6ccfb7998a00881692f38b4ca083c691d3e275b4145423704c34c909076", size = 34804044, upload-time = "2025-07-31T18:09:33.805Z" } +sdist = { url = "https://files.pythonhosted.org/packages/a0/59/c3e6453a9676ffba145309a73c462bb407f4400de7de3f2b41af70720a3c/matplotlib-3.10.6.tar.gz", hash = "sha256:ec01b645840dd1996df21ee37f208cd8ba57644779fa20464010638013d3203c", size = 34804264, upload-time = "2025-08-30T00:14:25.137Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/aa/c7/1f2db90a1d43710478bb1e9b57b162852f79234d28e4f48a28cc415aa583/matplotlib-3.10.5-cp311-cp311-macosx_10_12_x86_64.whl", hash = "sha256:dcfc39c452c6a9f9028d3e44d2d721484f665304857188124b505b2c95e1eecf", size = 8239216, upload-time = "2025-07-31T18:07:51.947Z" }, - { url = "https://files.pythonhosted.org/packages/82/6d/ca6844c77a4f89b1c9e4d481c412e1d1dbabf2aae2cbc5aa2da4a1d6683e/matplotlib-3.10.5-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:903352681b59f3efbf4546985142a9686ea1d616bb054b09a537a06e4b892ccf", size = 8102130, upload-time = "2025-07-31T18:07:53.65Z" }, - { url = "https://files.pythonhosted.org/packages/1d/1e/5e187a30cc673a3e384f3723e5f3c416033c1d8d5da414f82e4e731128ea/matplotlib-3.10.5-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:080c3676a56b8ee1c762bcf8fca3fe709daa1ee23e6ef06ad9f3fc17332f2d2a", size = 8666471, upload-time = "2025-07-31T18:07:55.304Z" }, - { url = "https://files.pythonhosted.org/packages/03/c0/95540d584d7d645324db99a845ac194e915ef75011a0d5e19e1b5cee7e69/matplotlib-3.10.5-cp311-cp311-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:4b4984d5064a35b6f66d2c11d668565f4389b1119cc64db7a4c1725bc11adffc", size = 9500518, upload-time = "2025-07-31T18:07:57.199Z" }, - { url = "https://files.pythonhosted.org/packages/ba/2e/e019352099ea58b4169adb9c6e1a2ad0c568c6377c2b677ee1f06de2adc7/matplotlib-3.10.5-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:3967424121d3a46705c9fa9bdb0931de3228f13f73d7bb03c999c88343a89d89", size = 9552372, upload-time = "2025-07-31T18:07:59.41Z" }, - { url = "https://files.pythonhosted.org/packages/b7/81/3200b792a5e8b354f31f4101ad7834743ad07b6d620259f2059317b25e4d/matplotlib-3.10.5-cp311-cp311-win_amd64.whl", hash = "sha256:33775bbeb75528555a15ac29396940128ef5613cf9a2d31fb1bfd18b3c0c0903", size = 8100634, upload-time = "2025-07-31T18:08:01.801Z" }, - { url = "https://files.pythonhosted.org/packages/52/46/a944f6f0c1f5476a0adfa501969d229ce5ae60cf9a663be0e70361381f89/matplotlib-3.10.5-cp311-cp311-win_arm64.whl", hash = "sha256:c61333a8e5e6240e73769d5826b9a31d8b22df76c0778f8480baf1b4b01c9420", size = 7978880, upload-time = "2025-07-31T18:08:03.407Z" }, - { url = "https://files.pythonhosted.org/packages/66/1e/c6f6bcd882d589410b475ca1fc22e34e34c82adff519caf18f3e6dd9d682/matplotlib-3.10.5-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:00b6feadc28a08bd3c65b2894f56cf3c94fc8f7adcbc6ab4516ae1e8ed8f62e2", size = 8253056, upload-time = "2025-07-31T18:08:05.385Z" }, - { url = "https://files.pythonhosted.org/packages/53/e6/d6f7d1b59413f233793dda14419776f5f443bcccb2dfc84b09f09fe05dbe/matplotlib-3.10.5-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:ee98a5c5344dc7f48dc261b6ba5d9900c008fc12beb3fa6ebda81273602cc389", size = 8110131, upload-time = "2025-07-31T18:08:07.293Z" }, - { url = "https://files.pythonhosted.org/packages/66/2b/bed8a45e74957549197a2ac2e1259671cd80b55ed9e1fe2b5c94d88a9202/matplotlib-3.10.5-cp312-cp312-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:a17e57e33de901d221a07af32c08870ed4528db0b6059dce7d7e65c1122d4bea", size = 8669603, upload-time = "2025-07-31T18:08:09.064Z" }, - { url = "https://files.pythonhosted.org/packages/7e/a7/315e9435b10d057f5e52dfc603cd353167ae28bb1a4e033d41540c0067a4/matplotlib-3.10.5-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:97b9d6443419085950ee4a5b1ee08c363e5c43d7176e55513479e53669e88468", size = 9508127, upload-time = "2025-07-31T18:08:10.845Z" }, - { url = "https://files.pythonhosted.org/packages/7f/d9/edcbb1f02ca99165365d2768d517898c22c6040187e2ae2ce7294437c413/matplotlib-3.10.5-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:ceefe5d40807d29a66ae916c6a3915d60ef9f028ce1927b84e727be91d884369", size = 9566926, upload-time = "2025-07-31T18:08:13.186Z" }, - { url = "https://files.pythonhosted.org/packages/3b/d9/6dd924ad5616c97b7308e6320cf392c466237a82a2040381163b7500510a/matplotlib-3.10.5-cp312-cp312-win_amd64.whl", hash = "sha256:c04cba0f93d40e45b3c187c6c52c17f24535b27d545f757a2fffebc06c12b98b", size = 8107599, upload-time = "2025-07-31T18:08:15.116Z" }, - { url = "https://files.pythonhosted.org/packages/0e/f3/522dc319a50f7b0279fbe74f86f7a3506ce414bc23172098e8d2bdf21894/matplotlib-3.10.5-cp312-cp312-win_arm64.whl", hash = "sha256:a41bcb6e2c8e79dc99c5511ae6f7787d2fb52efd3d805fff06d5d4f667db16b2", size = 7978173, upload-time = "2025-07-31T18:08:21.518Z" }, - { url = "https://files.pythonhosted.org/packages/dc/d6/e921be4e1a5f7aca5194e1f016cb67ec294548e530013251f630713e456d/matplotlib-3.10.5-pp311-pypy311_pp73-macosx_10_15_x86_64.whl", hash = "sha256:160e125da27a749481eaddc0627962990f6029811dbeae23881833a011a0907f", size = 8233224, upload-time = "2025-07-31T18:09:27.512Z" }, - { url = "https://files.pythonhosted.org/packages/ec/74/a2b9b04824b9c349c8f1b2d21d5af43fa7010039427f2b133a034cb09e59/matplotlib-3.10.5-pp311-pypy311_pp73-macosx_11_0_arm64.whl", hash = "sha256:ac3d50760394d78a3c9be6b28318fe22b494c4fcf6407e8fd4794b538251899b", size = 8098539, upload-time = "2025-07-31T18:09:29.629Z" }, - { url = "https://files.pythonhosted.org/packages/fc/66/cd29ebc7f6c0d2a15d216fb572573e8fc38bd5d6dec3bd9d7d904c0949f7/matplotlib-3.10.5-pp311-pypy311_pp73-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:6c49465bf689c4d59d174d0c7795fb42a21d4244d11d70e52b8011987367ac61", size = 8672192, upload-time = "2025-07-31T18:09:31.407Z" }, + { url = "https://files.pythonhosted.org/packages/80/d6/5d3665aa44c49005aaacaa68ddea6fcb27345961cd538a98bb0177934ede/matplotlib-3.10.6-cp311-cp311-macosx_10_12_x86_64.whl", hash = "sha256:905b60d1cb0ee604ce65b297b61cf8be9f4e6cfecf95a3fe1c388b5266bc8f4f", size = 8257527, upload-time = "2025-08-30T00:12:45.31Z" }, + { url = "https://files.pythonhosted.org/packages/8c/af/30ddefe19ca67eebd70047dabf50f899eaff6f3c5e6a1a7edaecaf63f794/matplotlib-3.10.6-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:7bac38d816637343e53d7185d0c66677ff30ffb131044a81898b5792c956ba76", size = 8119583, upload-time = "2025-08-30T00:12:47.236Z" }, + { url = "https://files.pythonhosted.org/packages/d3/29/4a8650a3dcae97fa4f375d46efcb25920d67b512186f8a6788b896062a81/matplotlib-3.10.6-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:942a8de2b5bfff1de31d95722f702e2966b8a7e31f4e68f7cd963c7cd8861cf6", size = 8692682, upload-time = "2025-08-30T00:12:48.781Z" }, + { url = "https://files.pythonhosted.org/packages/aa/d3/b793b9cb061cfd5d42ff0f69d1822f8d5dbc94e004618e48a97a8373179a/matplotlib-3.10.6-cp311-cp311-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:a3276c85370bc0dfca051ec65c5817d1e0f8f5ce1b7787528ec8ed2d524bbc2f", size = 9521065, upload-time = "2025-08-30T00:12:50.602Z" }, + { url = "https://files.pythonhosted.org/packages/f7/c5/53de5629f223c1c66668d46ac2621961970d21916a4bc3862b174eb2a88f/matplotlib-3.10.6-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:9df5851b219225731f564e4b9e7f2ac1e13c9e6481f941b5631a0f8e2d9387ce", size = 9576888, upload-time = "2025-08-30T00:12:52.92Z" }, + { url = "https://files.pythonhosted.org/packages/fc/8e/0a18d6d7d2d0a2e66585032a760d13662e5250c784d53ad50434e9560991/matplotlib-3.10.6-cp311-cp311-win_amd64.whl", hash = "sha256:abb5d9478625dd9c9eb51a06d39aae71eda749ae9b3138afb23eb38824026c7e", size = 8115158, upload-time = "2025-08-30T00:12:54.863Z" }, + { url = "https://files.pythonhosted.org/packages/07/b3/1a5107bb66c261e23b9338070702597a2d374e5aa7004b7adfc754fbed02/matplotlib-3.10.6-cp311-cp311-win_arm64.whl", hash = "sha256:886f989ccfae63659183173bb3fced7fd65e9eb793c3cc21c273add368536951", size = 7992444, upload-time = "2025-08-30T00:12:57.067Z" }, + { url = "https://files.pythonhosted.org/packages/ea/1a/7042f7430055d567cc3257ac409fcf608599ab27459457f13772c2d9778b/matplotlib-3.10.6-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:31ca662df6a80bd426f871105fdd69db7543e28e73a9f2afe80de7e531eb2347", size = 8272404, upload-time = "2025-08-30T00:12:59.112Z" }, + { url = "https://files.pythonhosted.org/packages/a9/5d/1d5f33f5b43f4f9e69e6a5fe1fb9090936ae7bc8e2ff6158e7a76542633b/matplotlib-3.10.6-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:1678bb61d897bb4ac4757b5ecfb02bfb3fddf7f808000fb81e09c510712fda75", size = 8128262, upload-time = "2025-08-30T00:13:01.141Z" }, + { url = "https://files.pythonhosted.org/packages/67/c3/135fdbbbf84e0979712df58e5e22b4f257b3f5e52a3c4aacf1b8abec0d09/matplotlib-3.10.6-cp312-cp312-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:56cd2d20842f58c03d2d6e6c1f1cf5548ad6f66b91e1e48f814e4fb5abd1cb95", size = 8697008, upload-time = "2025-08-30T00:13:03.24Z" }, + { url = "https://files.pythonhosted.org/packages/9c/be/c443ea428fb2488a3ea7608714b1bd85a82738c45da21b447dc49e2f8e5d/matplotlib-3.10.6-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:662df55604a2f9a45435566d6e2660e41efe83cd94f4288dfbf1e6d1eae4b0bb", size = 9530166, upload-time = "2025-08-30T00:13:05.951Z" }, + { url = "https://files.pythonhosted.org/packages/a9/35/48441422b044d74034aea2a3e0d1a49023f12150ebc58f16600132b9bbaf/matplotlib-3.10.6-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:08f141d55148cd1fc870c3387d70ca4df16dee10e909b3b038782bd4bda6ea07", size = 9593105, upload-time = "2025-08-30T00:13:08.356Z" }, + { url = "https://files.pythonhosted.org/packages/45/c3/994ef20eb4154ab84cc08d033834555319e4af970165e6c8894050af0b3c/matplotlib-3.10.6-cp312-cp312-win_amd64.whl", hash = "sha256:590f5925c2d650b5c9d813c5b3b5fc53f2929c3f8ef463e4ecfa7e052044fb2b", size = 8122784, upload-time = "2025-08-30T00:13:10.367Z" }, + { url = "https://files.pythonhosted.org/packages/57/b8/5c85d9ae0e40f04e71bedb053aada5d6bab1f9b5399a0937afb5d6b02d98/matplotlib-3.10.6-cp312-cp312-win_arm64.whl", hash = "sha256:f44c8d264a71609c79a78d50349e724f5d5fc3684ead7c2a473665ee63d868aa", size = 7992823, upload-time = "2025-08-30T00:13:12.24Z" }, + { url = "https://files.pythonhosted.org/packages/12/bb/02c35a51484aae5f49bd29f091286e7af5f3f677a9736c58a92b3c78baeb/matplotlib-3.10.6-pp311-pypy311_pp73-macosx_10_15_x86_64.whl", hash = "sha256:f2d684c3204fa62421bbf770ddfebc6b50130f9cad65531eeba19236d73bb488", size = 8252296, upload-time = "2025-08-30T00:14:19.49Z" }, + { url = "https://files.pythonhosted.org/packages/7d/85/41701e3092005aee9a2445f5ee3904d9dbd4a7df7a45905ffef29b7ef098/matplotlib-3.10.6-pp311-pypy311_pp73-macosx_11_0_arm64.whl", hash = "sha256:6f4a69196e663a41d12a728fab8751177215357906436804217d6d9cf0d4d6cf", size = 8116749, upload-time = "2025-08-30T00:14:21.344Z" }, + { url = "https://files.pythonhosted.org/packages/16/53/8d8fa0ea32a8c8239e04d022f6c059ee5e1b77517769feccd50f1df43d6d/matplotlib-3.10.6-pp311-pypy311_pp73-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:4d6ca6ef03dfd269f4ead566ec6f3fb9becf8dab146fb999022ed85ee9f6b3eb", size = 8693933, upload-time = "2025-08-30T00:14:22.942Z" }, ] [[package]] @@ -894,22 +927,22 @@ name = "metadrive-simulator" version = "0.4.2.4" source = { url = "https://github.com/commaai/metadrive/releases/download/MetaDrive-minimal-0.4.2.4/metadrive_simulator-0.4.2.4-py3-none-any.whl" } dependencies = [ - { name = "filelock" }, - { name = "gymnasium" }, - { name = "lxml" }, - { name = "matplotlib" }, - { name = "numpy" }, - { name = "opencv-python-headless" }, - { name = "panda3d" }, - { name = "panda3d-gltf" }, - { name = "pillow" }, - { name = "progressbar" }, - { name = "psutil" }, - { name = "pygments" }, - { name = "requests" }, - { name = "shapely" }, - { name = "tqdm" }, - { name = "yapf" }, + { name = "filelock", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "gymnasium", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "lxml", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "matplotlib", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "numpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "opencv-python-headless", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "panda3d", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "panda3d-gltf", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "pillow", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "progressbar", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "psutil", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "pygments", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "requests", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "shapely", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "tqdm", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "yapf", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] wheels = [ { url = "https://github.com/commaai/metadrive/releases/download/MetaDrive-minimal-0.4.2.4/metadrive_simulator-0.4.2.4-py3-none-any.whl", hash = "sha256:d0afaf3b005e35e14b929d5491d2d5b64562d0c1cd5093ba969fb63908670dd4" }, @@ -981,6 +1014,27 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/9f/d4/029f984e8d3f3b6b726bd33cafc473b75e9e44c0f7e80a5b29abc466bdea/mkdocs_get_deps-0.2.0-py3-none-any.whl", hash = "sha256:2bf11d0b133e77a0dd036abeeb06dec8775e46efa526dc70667d8863eefc6134", size = 9521, upload-time = "2023-11-20T17:51:08.587Z" }, ] +[[package]] +name = "ml-dtypes" +version = "0.5.3" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/78/a7/aad060393123cfb383956dca68402aff3db1e1caffd5764887ed5153f41b/ml_dtypes-0.5.3.tar.gz", hash = "sha256:95ce33057ba4d05df50b1f3cfefab22e351868a843b3b15a46c65836283670c9", size = 692316, upload-time = "2025-07-29T18:39:19.454Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/af/f1/720cb1409b5d0c05cff9040c0e9fba73fa4c67897d33babf905d5d46a070/ml_dtypes-0.5.3-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:4a177b882667c69422402df6ed5c3428ce07ac2c1f844d8a1314944651439458", size = 667412, upload-time = "2025-07-29T18:38:25.275Z" }, + { url = "https://files.pythonhosted.org/packages/6a/d5/05861ede5d299f6599f86e6bc1291714e2116d96df003cfe23cc54bcc568/ml_dtypes-0.5.3-cp311-cp311-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:9849ce7267444c0a717c80c6900997de4f36e2815ce34ac560a3edb2d9a64cd2", size = 4964606, upload-time = "2025-07-29T18:38:27.045Z" }, + { url = "https://files.pythonhosted.org/packages/db/dc/72992b68de367741bfab8df3b3fe7c29f982b7279d341aa5bf3e7ef737ea/ml_dtypes-0.5.3-cp311-cp311-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:c3f5ae0309d9f888fd825c2e9d0241102fadaca81d888f26f845bc8c13c1e4ee", size = 4938435, upload-time = "2025-07-29T18:38:29.193Z" }, + { url = "https://files.pythonhosted.org/packages/81/1c/d27a930bca31fb07d975a2d7eaf3404f9388114463b9f15032813c98f893/ml_dtypes-0.5.3-cp311-cp311-win_amd64.whl", hash = "sha256:58e39349d820b5702bb6f94ea0cb2dc8ec62ee81c0267d9622067d8333596a46", size = 206334, upload-time = "2025-07-29T18:38:30.687Z" }, + { url = "https://files.pythonhosted.org/packages/1a/d8/6922499effa616012cb8dc445280f66d100a7ff39b35c864cfca019b3f89/ml_dtypes-0.5.3-cp311-cp311-win_arm64.whl", hash = "sha256:66c2756ae6cfd7f5224e355c893cfd617fa2f747b8bbd8996152cbdebad9a184", size = 157584, upload-time = "2025-07-29T18:38:32.187Z" }, + { url = "https://files.pythonhosted.org/packages/0d/eb/bc07c88a6ab002b4635e44585d80fa0b350603f11a2097c9d1bfacc03357/ml_dtypes-0.5.3-cp312-cp312-macosx_10_13_universal2.whl", hash = "sha256:156418abeeda48ea4797db6776db3c5bdab9ac7be197c1233771e0880c304057", size = 663864, upload-time = "2025-07-29T18:38:33.777Z" }, + { url = "https://files.pythonhosted.org/packages/cf/89/11af9b0f21b99e6386b6581ab40fb38d03225f9de5f55cf52097047e2826/ml_dtypes-0.5.3-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:1db60c154989af253f6c4a34e8a540c2c9dce4d770784d426945e09908fbb177", size = 4951313, upload-time = "2025-07-29T18:38:36.45Z" }, + { url = "https://files.pythonhosted.org/packages/d8/a9/b98b86426c24900b0c754aad006dce2863df7ce0bb2bcc2c02f9cc7e8489/ml_dtypes-0.5.3-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:1b255acada256d1fa8c35ed07b5f6d18bc21d1556f842fbc2d5718aea2cd9e55", size = 4928805, upload-time = "2025-07-29T18:38:38.29Z" }, + { url = "https://files.pythonhosted.org/packages/50/c1/85e6be4fc09c6175f36fb05a45917837f30af9a5146a5151cb3a3f0f9e09/ml_dtypes-0.5.3-cp312-cp312-win_amd64.whl", hash = "sha256:da65e5fd3eea434ccb8984c3624bc234ddcc0d9f4c81864af611aaebcc08a50e", size = 208182, upload-time = "2025-07-29T18:38:39.72Z" }, + { url = "https://files.pythonhosted.org/packages/9e/17/cf5326d6867be057f232d0610de1458f70a8ce7b6290e4b4a277ea62b4cd/ml_dtypes-0.5.3-cp312-cp312-win_arm64.whl", hash = "sha256:8bb9cd1ce63096567f5f42851f5843b5a0ea11511e50039a7649619abfb4ba6d", size = 161560, upload-time = "2025-07-29T18:38:41.072Z" }, +] + [[package]] name = "mouseinfo" version = "0.1.3" @@ -1155,27 +1209,28 @@ wheels = [ [[package]] name = "onnx" -version = "1.18.0" +version = "1.19.0" source = { registry = "https://pypi.org/simple" } dependencies = [ + { name = "ml-dtypes" }, { name = "numpy" }, { name = "protobuf" }, { name = "typing-extensions" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/3d/60/e56e8ec44ed34006e6d4a73c92a04d9eea6163cc12440e35045aec069175/onnx-1.18.0.tar.gz", hash = "sha256:3d8dbf9e996629131ba3aa1afd1d8239b660d1f830c6688dd7e03157cccd6b9c", size = 12563009, upload-time = "2025-05-12T22:03:09.626Z" } +sdist = { url = "https://files.pythonhosted.org/packages/5b/bf/b0a63ee9f3759dcd177b28c6f2cb22f2aecc6d9b3efecaabc298883caa5f/onnx-1.19.0.tar.gz", hash = "sha256:aa3f70b60f54a29015e41639298ace06adf1dd6b023b9b30f1bca91bb0db9473", size = 11949859, upload-time = "2025-08-27T02:34:27.107Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/ed/3a/a336dac4db1eddba2bf577191e5b7d3e4c26fcee5ec518a5a5b11d13540d/onnx-1.18.0-cp311-cp311-macosx_12_0_universal2.whl", hash = "sha256:735e06d8d0cf250dc498f54038831401063c655a8d6e5975b2527a4e7d24be3e", size = 18281831, upload-time = "2025-05-12T22:02:06.429Z" }, - { url = "https://files.pythonhosted.org/packages/02/3a/56475a111120d1e5d11939acbcbb17c92198c8e64a205cd68e00bdfd8a1f/onnx-1.18.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:73160799472e1a86083f786fecdf864cf43d55325492a9b5a1cfa64d8a523ecc", size = 17424359, upload-time = "2025-05-12T22:02:09.866Z" }, - { url = "https://files.pythonhosted.org/packages/cf/03/5eb5e9ef446ed9e78c4627faf3c1bc25e0f707116dd00e9811de232a8df5/onnx-1.18.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6acafb3823238bbe8f4340c7ac32fb218689442e074d797bee1c5c9a02fdae75", size = 17586006, upload-time = "2025-05-12T22:02:13.217Z" }, - { url = "https://files.pythonhosted.org/packages/b0/4e/70943125729ce453271a6e46bb847b4a612496f64db6cbc6cb1f49f41ce1/onnx-1.18.0-cp311-cp311-win32.whl", hash = "sha256:4c8c4bbda760c654e65eaffddb1a7de71ec02e60092d33f9000521f897c99be9", size = 15734988, upload-time = "2025-05-12T22:02:16.561Z" }, - { url = "https://files.pythonhosted.org/packages/44/b0/435fd764011911e8f599e3361f0f33425b1004662c1ea33a0ad22e43db2d/onnx-1.18.0-cp311-cp311-win_amd64.whl", hash = "sha256:a5810194f0f6be2e58c8d6dedc6119510df7a14280dd07ed5f0f0a85bd74816a", size = 15849576, upload-time = "2025-05-12T22:02:19.569Z" }, - { url = "https://files.pythonhosted.org/packages/6c/f0/9e31f4b4626d60f1c034f71b411810bc9fafe31f4e7dd3598effd1b50e05/onnx-1.18.0-cp311-cp311-win_arm64.whl", hash = "sha256:aa1b7483fac6cdec26922174fc4433f8f5c2f239b1133c5625063bb3b35957d0", size = 15822961, upload-time = "2025-05-12T22:02:22.735Z" }, - { url = "https://files.pythonhosted.org/packages/a7/fe/16228aca685392a7114625b89aae98b2dc4058a47f0f467a376745efe8d0/onnx-1.18.0-cp312-cp312-macosx_12_0_universal2.whl", hash = "sha256:521bac578448667cbb37c50bf05b53c301243ede8233029555239930996a625b", size = 18285770, upload-time = "2025-05-12T22:02:26.116Z" }, - { url = "https://files.pythonhosted.org/packages/1e/77/ba50a903a9b5e6f9be0fa50f59eb2fca4a26ee653375408fbc72c3acbf9f/onnx-1.18.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e4da451bf1c5ae381f32d430004a89f0405bc57a8471b0bddb6325a5b334aa40", size = 17421291, upload-time = "2025-05-12T22:02:29.645Z" }, - { url = "https://files.pythonhosted.org/packages/11/23/25ec2ba723ac62b99e8fed6d7b59094dadb15e38d4c007331cc9ae3dfa5f/onnx-1.18.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:99afac90b4cdb1471432203c3c1f74e16549c526df27056d39f41a9a47cfb4af", size = 17584084, upload-time = "2025-05-12T22:02:32.789Z" }, - { url = "https://files.pythonhosted.org/packages/6a/4d/2c253a36070fb43f340ff1d2c450df6a9ef50b938adcd105693fee43c4ee/onnx-1.18.0-cp312-cp312-win32.whl", hash = "sha256:ee159b41a3ae58d9c7341cf432fc74b96aaf50bd7bb1160029f657b40dc69715", size = 15734892, upload-time = "2025-05-12T22:02:35.527Z" }, - { url = "https://files.pythonhosted.org/packages/e8/92/048ba8fafe6b2b9a268ec2fb80def7e66c0b32ab2cae74de886981f05a27/onnx-1.18.0-cp312-cp312-win_amd64.whl", hash = "sha256:102c04edc76b16e9dfeda5a64c1fccd7d3d2913b1544750c01d38f1ac3c04e05", size = 15850336, upload-time = "2025-05-12T22:02:38.545Z" }, - { url = "https://files.pythonhosted.org/packages/a1/66/bbc4ffedd44165dcc407a51ea4c592802a5391ce3dc94aa5045350f64635/onnx-1.18.0-cp312-cp312-win_arm64.whl", hash = "sha256:911b37d724a5d97396f3c2ef9ea25361c55cbc9aa18d75b12a52b620b67145af", size = 15823802, upload-time = "2025-05-12T22:02:42.037Z" }, + { url = "https://files.pythonhosted.org/packages/db/5c/b959b17608cfb6ccf6359b39fe56a5b0b7d965b3d6e6a3c0add90812c36e/onnx-1.19.0-cp311-cp311-macosx_12_0_universal2.whl", hash = "sha256:206f00c47b85b5c7af79671e3307147407991a17994c26974565aadc9e96e4e4", size = 18312580, upload-time = "2025-08-27T02:33:03.081Z" }, + { url = "https://files.pythonhosted.org/packages/2c/ee/ac052bbbc832abe0debb784c2c57f9582444fb5f51d63c2967fd04432444/onnx-1.19.0-cp311-cp311-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:4d7bee94abaac28988b50da675ae99ef8dd3ce16210d591fbd0b214a5930beb3", size = 18029165, upload-time = "2025-08-27T02:33:05.771Z" }, + { url = "https://files.pythonhosted.org/packages/5c/c9/8687ba0948d46fd61b04e3952af9237883bbf8f16d716e7ed27e688d73b8/onnx-1.19.0-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:7730b96b68c0c354bbc7857961bb4909b9aaa171360a8e3708d0a4c749aaadeb", size = 18202125, upload-time = "2025-08-27T02:33:09.325Z" }, + { url = "https://files.pythonhosted.org/packages/e2/16/6249c013e81bd689f46f96c7236d7677f1af5dd9ef22746716b48f10e506/onnx-1.19.0-cp311-cp311-win32.whl", hash = "sha256:7cb7a3ad8059d1a0dfdc5e0a98f71837d82002e441f112825403b137227c2c97", size = 16332738, upload-time = "2025-08-27T02:33:12.448Z" }, + { url = "https://files.pythonhosted.org/packages/6a/28/34a1e2166e418c6a78e5c82e66f409d9da9317832f11c647f7d4e23846a6/onnx-1.19.0-cp311-cp311-win_amd64.whl", hash = "sha256:d75452a9be868bd30c3ef6aa5991df89bbfe53d0d90b2325c5e730fbd91fff85", size = 16452303, upload-time = "2025-08-27T02:33:15.176Z" }, + { url = "https://files.pythonhosted.org/packages/e6/b7/639664626e5ba8027860c4d2a639ee02b37e9c322215c921e9222513c3aa/onnx-1.19.0-cp311-cp311-win_arm64.whl", hash = "sha256:23c7959370d7b3236f821e609b0af7763cff7672a758e6c1fc877bac099e786b", size = 16425340, upload-time = "2025-08-27T02:33:17.78Z" }, + { url = "https://files.pythonhosted.org/packages/0d/94/f56f6ca5e2f921b28c0f0476705eab56486b279f04e1d568ed64c14e7764/onnx-1.19.0-cp312-cp312-macosx_12_0_universal2.whl", hash = "sha256:61d94e6498ca636756f8f4ee2135708434601b2892b7c09536befb19bc8ca007", size = 18322331, upload-time = "2025-08-27T02:33:20.373Z" }, + { url = "https://files.pythonhosted.org/packages/c8/00/8cc3f3c40b54b28f96923380f57c9176872e475face726f7d7a78bd74098/onnx-1.19.0-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:224473354462f005bae985c72028aaa5c85ab11de1b71d55b06fdadd64a667dd", size = 18027513, upload-time = "2025-08-27T02:33:23.44Z" }, + { url = "https://files.pythonhosted.org/packages/61/90/17c4d2566fd0117a5e412688c9525f8950d467f477fbd574e6b32bc9cb8d/onnx-1.19.0-cp312-cp312-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:1ae475c85c89bc4d1f16571006fd21a3e7c0e258dd2c091f6e8aafb083d1ed9b", size = 18202278, upload-time = "2025-08-27T02:33:26.103Z" }, + { url = "https://files.pythonhosted.org/packages/bc/6e/a9383d9cf6db4ac761a129b081e9fa5d0cd89aad43cf1e3fc6285b915c7d/onnx-1.19.0-cp312-cp312-win32.whl", hash = "sha256:323f6a96383a9cdb3960396cffea0a922593d221f3929b17312781e9f9b7fb9f", size = 16333080, upload-time = "2025-08-27T02:33:28.559Z" }, + { url = "https://files.pythonhosted.org/packages/a7/2e/3ff480a8c1fa7939662bdc973e41914add2d4a1f2b8572a3c39c2e4982e5/onnx-1.19.0-cp312-cp312-win_amd64.whl", hash = "sha256:50220f3499a499b1a15e19451a678a58e22ad21b34edf2c844c6ef1d9febddc2", size = 16453927, upload-time = "2025-08-27T02:33:31.177Z" }, + { url = "https://files.pythonhosted.org/packages/57/37/ad500945b1b5c154fe9d7b826b30816ebd629d10211ea82071b5bcc30aa4/onnx-1.19.0-cp312-cp312-win_arm64.whl", hash = "sha256:efb768299580b786e21abe504e1652ae6189f0beed02ab087cd841cb4bb37e43", size = 16426022, upload-time = "2025-08-27T02:33:33.515Z" }, ] [[package]] @@ -1206,9 +1261,11 @@ dependencies = [ { name = "cffi" }, { name = "crcmod" }, { name = "cython" }, + { name = "dearpygui" }, { name = "future-fstrings" }, { name = "inputs" }, { name = "json-rpc" }, + { name = "kaitaistruct" }, { name = "libusb1" }, { name = "numpy" }, { name = "onnx" }, @@ -1243,6 +1300,7 @@ dev = [ { name = "azure-storage-blob" }, { name = "dbus-next" }, { name = "dictdiffer" }, + { name = "jeepney" }, { name = "matplotlib" }, { name = "opencv-python-headless" }, { name = "parameterized" }, @@ -1295,12 +1353,15 @@ requires-dist = [ { name = "crcmod" }, { name = "cython" }, { name = "dbus-next", marker = "extra == 'dev'" }, + { name = "dearpygui", specifier = ">=2.1.0" }, { name = "dictdiffer", marker = "extra == 'dev'" }, { name = "future-fstrings" }, { name = "hypothesis", marker = "extra == 'testing'", specifier = "==6.47.*" }, { name = "inputs" }, + { name = "jeepney", marker = "extra == 'dev'" }, { name = "jinja2", marker = "extra == 'docs'" }, { name = "json-rpc" }, + { name = "kaitaistruct" }, { name = "libusb1" }, { name = "matplotlib", marker = "extra == 'dev'" }, { name = "metadrive-simulator", marker = "platform_machine != 'aarch64' and extra == 'tools'", url = "https://github.com/commaai/metadrive/releases/download/MetaDrive-minimal-0.4.2.4/metadrive_simulator-0.4.2.4-py3-none-any.whl" }, @@ -1389,8 +1450,8 @@ name = "panda3d-gltf" version = "0.13" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "panda3d" }, - { name = "panda3d-simplepbr" }, + { name = "panda3d", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "panda3d-simplepbr", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/07/7f/9f18fc3fa843a080acb891af6bcc12262e7bdf1d194a530f7042bebfc81f/panda3d-gltf-0.13.tar.gz", hash = "sha256:d06d373bdd91cf530909b669f43080e599463bbf6d3ef00c3558bad6c6b19675", size = 25573, upload-time = "2021-05-21T05:46:32.738Z" } wheels = [ @@ -1402,8 +1463,8 @@ name = "panda3d-simplepbr" version = "0.13.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "panda3d" }, - { name = "typing-extensions" }, + { name = "panda3d", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "typing-extensions", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/0d/be/c4d1ded04c22b357277cf6e6a44c1ab4abb285a700bd1991460460e05b99/panda3d_simplepbr-0.13.1.tar.gz", hash = "sha256:c83766d7c8f47499f365a07fe1dff078fc8b3054c2689bdc8dceabddfe7f1a35", size = 6216055, upload-time = "2025-03-30T16:57:41.087Z" } wheels = [ @@ -1467,11 +1528,11 @@ wheels = [ [[package]] name = "platformdirs" -version = "4.3.8" +version = "4.4.0" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/fe/8b/3c73abc9c759ecd3f1f7ceff6685840859e8070c4d947c93fae71f6a0bf2/platformdirs-4.3.8.tar.gz", hash = "sha256:3d512d96e16bcb959a814c9f348431070822a6496326a4be0911c40b5a74c2bc", size = 21362, upload-time = "2025-05-07T22:47:42.121Z" } +sdist = { url = "https://files.pythonhosted.org/packages/23/e8/21db9c9987b0e728855bd57bff6984f67952bea55d6f75e055c46b5383e8/platformdirs-4.4.0.tar.gz", hash = "sha256:ca753cf4d81dc309bc67b0ea38fd15dc97bc30ce419a7f58d13eb3bf14c4febf", size = 21634, upload-time = "2025-08-26T14:32:04.268Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/fe/39/979e8e21520d4e47a0bbe349e2713c0aac6f3d853d0e5b34d76206c439aa/platformdirs-4.3.8-py3-none-any.whl", hash = "sha256:ff7059bb7eb1179e2685604f4aaf157cfd9535242bd23742eadc3c13542139b4", size = 18567, upload-time = "2025-05-07T22:47:40.376Z" }, + { url = "https://files.pythonhosted.org/packages/40/4b/2028861e724d3bd36227adfa20d3fd24c3fc6d52032f4a93c133be5d17ce/platformdirs-4.4.0-py3-none-any.whl", hash = "sha256:abd01743f24e5287cd7a5db3752faf1a2d65353f38ec26d98e25a6db65958c85", size = 18654, upload-time = "2025-08-26T14:32:02.735Z" }, ] [[package]] @@ -4140,9 +4201,9 @@ name = "pyopencl" version = "2025.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy" }, - { name = "platformdirs" }, - { name = "pytools" }, + { name = "numpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "platformdirs", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "pytools", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/28/88/0ac460d3e2def08b2ad6345db6a13613815f616bbbd60c6f4bdf774f4c41/pyopencl-2025.1.tar.gz", hash = "sha256:0116736d7f7920f87b8db4b66a03f27b1d930d2e37ddd14518407cc22dd24779", size = 422510, upload-time = "2025-01-22T00:16:58.421Z" } wheels = [ @@ -4318,7 +4379,7 @@ wheels = [ [[package]] name = "pytest-xdist" -version = "3.7.1.dev24+g2b4372bd6" +version = "3.7.1.dev24+g2b4372b" source = { git = "https://github.com/sshane/pytest-xdist?rev=2b4372bd62699fb412c4fe2f95bf9f01bd2018da#2b4372bd62699fb412c4fe2f95bf9f01bd2018da" } dependencies = [ { name = "execnet" }, @@ -4360,9 +4421,9 @@ name = "pytools" version = "2024.1.10" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "platformdirs" }, - { name = "siphash24" }, - { name = "typing-extensions" }, + { name = "platformdirs", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "siphash24", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "typing-extensions", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ee/0f/56e109c0307f831b5d598ad73976aaaa84b4d0e98da29a642e797eaa940c/pytools-2024.1.10.tar.gz", hash = "sha256:9af6f4b045212c49be32bb31fe19606c478ee4b09631886d05a32459f4ce0a12", size = 81741, upload-time = "2024-07-17T18:47:38.287Z" } wheels = [ @@ -4594,28 +4655,28 @@ wheels = [ [[package]] name = "ruff" -version = "0.12.10" +version = "0.12.11" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/3b/eb/8c073deb376e46ae767f4961390d17545e8535921d2f65101720ed8bd434/ruff-0.12.10.tar.gz", hash = "sha256:189ab65149d11ea69a2d775343adf5f49bb2426fc4780f65ee33b423ad2e47f9", size = 5310076, upload-time = "2025-08-21T18:23:22.595Z" } +sdist = { url = "https://files.pythonhosted.org/packages/de/55/16ab6a7d88d93001e1ae4c34cbdcfb376652d761799459ff27c1dc20f6fa/ruff-0.12.11.tar.gz", hash = "sha256:c6b09ae8426a65bbee5425b9d0b82796dbb07cb1af045743c79bfb163001165d", size = 5347103, upload-time = "2025-08-28T13:59:08.87Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/24/e7/560d049d15585d6c201f9eeacd2fd130def3741323e5ccf123786e0e3c95/ruff-0.12.10-py3-none-linux_armv6l.whl", hash = "sha256:8b593cb0fb55cc8692dac7b06deb29afda78c721c7ccfed22db941201b7b8f7b", size = 11935161, upload-time = "2025-08-21T18:22:26.965Z" }, - { url = "https://files.pythonhosted.org/packages/d1/b0/ad2464922a1113c365d12b8f80ed70fcfb39764288ac77c995156080488d/ruff-0.12.10-py3-none-macosx_10_12_x86_64.whl", hash = "sha256:ebb7333a45d56efc7c110a46a69a1b32365d5c5161e7244aaf3aa20ce62399c1", size = 12660884, upload-time = "2025-08-21T18:22:30.925Z" }, - { url = "https://files.pythonhosted.org/packages/d7/f1/97f509b4108d7bae16c48389f54f005b62ce86712120fd8b2d8e88a7cb49/ruff-0.12.10-py3-none-macosx_11_0_arm64.whl", hash = "sha256:d59e58586829f8e4a9920788f6efba97a13d1fa320b047814e8afede381c6839", size = 11872754, upload-time = "2025-08-21T18:22:34.035Z" }, - { url = "https://files.pythonhosted.org/packages/12/ad/44f606d243f744a75adc432275217296095101f83f966842063d78eee2d3/ruff-0.12.10-py3-none-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:822d9677b560f1fdeab69b89d1f444bf5459da4aa04e06e766cf0121771ab844", size = 12092276, upload-time = "2025-08-21T18:22:36.764Z" }, - { url = "https://files.pythonhosted.org/packages/06/1f/ed6c265e199568010197909b25c896d66e4ef2c5e1c3808caf461f6f3579/ruff-0.12.10-py3-none-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:37b4a64f4062a50c75019c61c7017ff598cb444984b638511f48539d3a1c98db", size = 11734700, upload-time = "2025-08-21T18:22:39.822Z" }, - { url = "https://files.pythonhosted.org/packages/63/c5/b21cde720f54a1d1db71538c0bc9b73dee4b563a7dd7d2e404914904d7f5/ruff-0.12.10-py3-none-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:2c6f4064c69d2542029b2a61d39920c85240c39837599d7f2e32e80d36401d6e", size = 13468783, upload-time = "2025-08-21T18:22:42.559Z" }, - { url = "https://files.pythonhosted.org/packages/02/9e/39369e6ac7f2a1848f22fb0b00b690492f20811a1ac5c1fd1d2798329263/ruff-0.12.10-py3-none-manylinux_2_17_ppc64.manylinux2014_ppc64.whl", hash = "sha256:059e863ea3a9ade41407ad71c1de2badfbe01539117f38f763ba42a1206f7559", size = 14436642, upload-time = "2025-08-21T18:22:45.612Z" }, - { url = "https://files.pythonhosted.org/packages/e3/03/5da8cad4b0d5242a936eb203b58318016db44f5c5d351b07e3f5e211bb89/ruff-0.12.10-py3-none-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:1bef6161e297c68908b7218fa6e0e93e99a286e5ed9653d4be71e687dff101cf", size = 13859107, upload-time = "2025-08-21T18:22:48.886Z" }, - { url = "https://files.pythonhosted.org/packages/19/19/dd7273b69bf7f93a070c9cec9494a94048325ad18fdcf50114f07e6bf417/ruff-0.12.10-py3-none-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:4f1345fbf8fb0531cd722285b5f15af49b2932742fc96b633e883da8d841896b", size = 12886521, upload-time = "2025-08-21T18:22:51.567Z" }, - { url = "https://files.pythonhosted.org/packages/c0/1d/b4207ec35e7babaee62c462769e77457e26eb853fbdc877af29417033333/ruff-0.12.10-py3-none-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1f68433c4fbc63efbfa3ba5db31727db229fa4e61000f452c540474b03de52a9", size = 13097528, upload-time = "2025-08-21T18:22:54.609Z" }, - { url = "https://files.pythonhosted.org/packages/ff/00/58f7b873b21114456e880b75176af3490d7a2836033779ca42f50de3b47a/ruff-0.12.10-py3-none-manylinux_2_31_riscv64.whl", hash = "sha256:141ce3d88803c625257b8a6debf4a0473eb6eed9643a6189b68838b43e78165a", size = 13080443, upload-time = "2025-08-21T18:22:57.413Z" }, - { url = "https://files.pythonhosted.org/packages/12/8c/9e6660007fb10189ccb78a02b41691288038e51e4788bf49b0a60f740604/ruff-0.12.10-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:f3fc21178cd44c98142ae7590f42ddcb587b8e09a3b849cbc84edb62ee95de60", size = 11896759, upload-time = "2025-08-21T18:23:00.473Z" }, - { url = "https://files.pythonhosted.org/packages/67/4c/6d092bb99ea9ea6ebda817a0e7ad886f42a58b4501a7e27cd97371d0ba54/ruff-0.12.10-py3-none-musllinux_1_2_armv7l.whl", hash = "sha256:7d1a4e0bdfafcd2e3e235ecf50bf0176f74dd37902f241588ae1f6c827a36c56", size = 11701463, upload-time = "2025-08-21T18:23:03.211Z" }, - { url = "https://files.pythonhosted.org/packages/59/80/d982c55e91df981f3ab62559371380616c57ffd0172d96850280c2b04fa8/ruff-0.12.10-py3-none-musllinux_1_2_i686.whl", hash = "sha256:e67d96827854f50b9e3e8327b031647e7bcc090dbe7bb11101a81a3a2cbf1cc9", size = 12691603, upload-time = "2025-08-21T18:23:06.935Z" }, - { url = "https://files.pythonhosted.org/packages/ad/37/63a9c788bbe0b0850611669ec6b8589838faf2f4f959647f2d3e320383ae/ruff-0.12.10-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:ae479e1a18b439c59138f066ae79cc0f3ee250712a873d00dbafadaad9481e5b", size = 13164356, upload-time = "2025-08-21T18:23:10.225Z" }, - { url = "https://files.pythonhosted.org/packages/47/d4/1aaa7fb201a74181989970ebccd12f88c0fc074777027e2a21de5a90657e/ruff-0.12.10-py3-none-win32.whl", hash = "sha256:9de785e95dc2f09846c5e6e1d3a3d32ecd0b283a979898ad427a9be7be22b266", size = 11896089, upload-time = "2025-08-21T18:23:14.232Z" }, - { url = "https://files.pythonhosted.org/packages/ad/14/2ad38fd4037daab9e023456a4a40ed0154e9971f8d6aed41bdea390aabd9/ruff-0.12.10-py3-none-win_amd64.whl", hash = "sha256:7837eca8787f076f67aba2ca559cefd9c5cbc3a9852fd66186f4201b87c1563e", size = 13004616, upload-time = "2025-08-21T18:23:17.422Z" }, - { url = "https://files.pythonhosted.org/packages/24/3c/21cf283d67af33a8e6ed242396863af195a8a6134ec581524fd22b9811b6/ruff-0.12.10-py3-none-win_arm64.whl", hash = "sha256:cc138cc06ed9d4bfa9d667a65af7172b47840e1a98b02ce7011c391e54635ffc", size = 12074225, upload-time = "2025-08-21T18:23:20.137Z" }, + { url = "https://files.pythonhosted.org/packages/d6/a2/3b3573e474de39a7a475f3fbaf36a25600bfeb238e1a90392799163b64a0/ruff-0.12.11-py3-none-linux_armv6l.whl", hash = "sha256:93fce71e1cac3a8bf9200e63a38ac5c078f3b6baebffb74ba5274fb2ab276065", size = 11979885, upload-time = "2025-08-28T13:58:26.654Z" }, + { url = "https://files.pythonhosted.org/packages/76/e4/235ad6d1785a2012d3ded2350fd9bc5c5af8c6f56820e696b0118dfe7d24/ruff-0.12.11-py3-none-macosx_10_12_x86_64.whl", hash = "sha256:b8e33ac7b28c772440afa80cebb972ffd823621ded90404f29e5ab6d1e2d4b93", size = 12742364, upload-time = "2025-08-28T13:58:30.256Z" }, + { url = "https://files.pythonhosted.org/packages/2c/0d/15b72c5fe6b1e402a543aa9d8960e0a7e19dfb079f5b0b424db48b7febab/ruff-0.12.11-py3-none-macosx_11_0_arm64.whl", hash = "sha256:d69fb9d4937aa19adb2e9f058bc4fbfe986c2040acb1a4a9747734834eaa0bfd", size = 11920111, upload-time = "2025-08-28T13:58:33.677Z" }, + { url = "https://files.pythonhosted.org/packages/3e/c0/f66339d7893798ad3e17fa5a1e587d6fd9806f7c1c062b63f8b09dda6702/ruff-0.12.11-py3-none-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:411954eca8464595077a93e580e2918d0a01a19317af0a72132283e28ae21bee", size = 12160060, upload-time = "2025-08-28T13:58:35.74Z" }, + { url = "https://files.pythonhosted.org/packages/03/69/9870368326db26f20c946205fb2d0008988aea552dbaec35fbacbb46efaa/ruff-0.12.11-py3-none-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:6a2c0a2e1a450f387bf2c6237c727dd22191ae8c00e448e0672d624b2bbd7fb0", size = 11799848, upload-time = "2025-08-28T13:58:38.051Z" }, + { url = "https://files.pythonhosted.org/packages/25/8c/dd2c7f990e9b3a8a55eee09d4e675027d31727ce33cdb29eab32d025bdc9/ruff-0.12.11-py3-none-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:8ca4c3a7f937725fd2413c0e884b5248a19369ab9bdd850b5781348ba283f644", size = 13536288, upload-time = "2025-08-28T13:58:40.046Z" }, + { url = "https://files.pythonhosted.org/packages/7a/30/d5496fa09aba59b5e01ea76775a4c8897b13055884f56f1c35a4194c2297/ruff-0.12.11-py3-none-manylinux_2_17_ppc64.manylinux2014_ppc64.whl", hash = "sha256:4d1df0098124006f6a66ecf3581a7f7e754c4df7644b2e6704cd7ca80ff95211", size = 14490633, upload-time = "2025-08-28T13:58:42.285Z" }, + { url = "https://files.pythonhosted.org/packages/9b/2f/81f998180ad53445d403c386549d6946d0748e536d58fce5b5e173511183/ruff-0.12.11-py3-none-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:5a8dd5f230efc99a24ace3b77e3555d3fbc0343aeed3fc84c8d89e75ab2ff793", size = 13888430, upload-time = "2025-08-28T13:58:44.641Z" }, + { url = "https://files.pythonhosted.org/packages/87/71/23a0d1d5892a377478c61dbbcffe82a3476b050f38b5162171942a029ef3/ruff-0.12.11-py3-none-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:4dc75533039d0ed04cd33fb8ca9ac9620b99672fe7ff1533b6402206901c34ee", size = 12913133, upload-time = "2025-08-28T13:58:47.039Z" }, + { url = "https://files.pythonhosted.org/packages/80/22/3c6cef96627f89b344c933781ed38329bfb87737aa438f15da95907cbfd5/ruff-0.12.11-py3-none-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4fc58f9266d62c6eccc75261a665f26b4ef64840887fc6cbc552ce5b29f96cc8", size = 13169082, upload-time = "2025-08-28T13:58:49.157Z" }, + { url = "https://files.pythonhosted.org/packages/05/b5/68b3ff96160d8b49e8dd10785ff3186be18fd650d356036a3770386e6c7f/ruff-0.12.11-py3-none-manylinux_2_31_riscv64.whl", hash = "sha256:5a0113bd6eafd545146440225fe60b4e9489f59eb5f5f107acd715ba5f0b3d2f", size = 13139490, upload-time = "2025-08-28T13:58:51.593Z" }, + { url = "https://files.pythonhosted.org/packages/59/b9/050a3278ecd558f74f7ee016fbdf10591d50119df8d5f5da45a22c6afafc/ruff-0.12.11-py3-none-musllinux_1_2_aarch64.whl", hash = "sha256:0d737b4059d66295c3ea5720e6efc152623bb83fde5444209b69cd33a53e2000", size = 11958928, upload-time = "2025-08-28T13:58:53.943Z" }, + { url = "https://files.pythonhosted.org/packages/f9/bc/93be37347db854806904a43b0493af8d6873472dfb4b4b8cbb27786eb651/ruff-0.12.11-py3-none-musllinux_1_2_armv7l.whl", hash = "sha256:916fc5defee32dbc1fc1650b576a8fed68f5e8256e2180d4d9855aea43d6aab2", size = 11764513, upload-time = "2025-08-28T13:58:55.976Z" }, + { url = "https://files.pythonhosted.org/packages/7a/a1/1471751e2015a81fd8e166cd311456c11df74c7e8769d4aabfbc7584c7ac/ruff-0.12.11-py3-none-musllinux_1_2_i686.whl", hash = "sha256:c984f07d7adb42d3ded5be894fb4007f30f82c87559438b4879fe7aa08c62b39", size = 12745154, upload-time = "2025-08-28T13:58:58.16Z" }, + { url = "https://files.pythonhosted.org/packages/68/ab/2542b14890d0f4872dd81b7b2a6aed3ac1786fae1ce9b17e11e6df9e31e3/ruff-0.12.11-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:e07fbb89f2e9249f219d88331c833860489b49cdf4b032b8e4432e9b13e8a4b9", size = 13227653, upload-time = "2025-08-28T13:59:00.276Z" }, + { url = "https://files.pythonhosted.org/packages/22/16/2fbfc61047dbfd009c58a28369a693a1484ad15441723be1cd7fe69bb679/ruff-0.12.11-py3-none-win32.whl", hash = "sha256:c792e8f597c9c756e9bcd4d87cf407a00b60af77078c96f7b6366ea2ce9ba9d3", size = 11944270, upload-time = "2025-08-28T13:59:02.347Z" }, + { url = "https://files.pythonhosted.org/packages/08/a5/34276984705bfe069cd383101c45077ee029c3fe3b28225bf67aa35f0647/ruff-0.12.11-py3-none-win_amd64.whl", hash = "sha256:a3283325960307915b6deb3576b96919ee89432ebd9c48771ca12ee8afe4a0fd", size = 13046600, upload-time = "2025-08-28T13:59:04.751Z" }, + { url = "https://files.pythonhosted.org/packages/84/a8/001d4a7c2b37623a3fd7463208267fb906df40ff31db496157549cfd6e72/ruff-0.12.11-py3-none-win_arm64.whl", hash = "sha256:bae4d6e6a2676f8fb0f98b74594a048bae1b944aab17e9f5d504062303c6dbea", size = 12135290, upload-time = "2025-08-28T13:59:06.933Z" }, ] [[package]] @@ -4629,15 +4690,15 @@ wheels = [ [[package]] name = "sentry-sdk" -version = "2.35.0" +version = "2.35.2" source = { registry = "https://pypi.org/simple" } dependencies = [ { name = "certifi" }, { name = "urllib3" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/31/83/055dc157b719651ef13db569bb8cf2103df11174478649735c1b2bf3f6bc/sentry_sdk-2.35.0.tar.gz", hash = "sha256:5ea58d352779ce45d17bc2fa71ec7185205295b83a9dbb5707273deb64720092", size = 343014, upload-time = "2025-08-14T17:11:20.223Z" } +sdist = { url = "https://files.pythonhosted.org/packages/bd/79/0ecb942f3f1ad26c40c27f81ff82392d85c01d26a45e3c72c2b37807e680/sentry_sdk-2.35.2.tar.gz", hash = "sha256:e9e8f3c795044beb59f2c8f4c6b9b0f9779e5e604099882df05eec525e782cc6", size = 343377, upload-time = "2025-09-01T11:00:58.633Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/36/3d/742617a7c644deb0c1628dcf6bb2d2165ab7c6aab56fe5222758994007f8/sentry_sdk-2.35.0-py2.py3-none-any.whl", hash = "sha256:6e0c29b9a5d34de8575ffb04d289a987ff3053cf2c98ede445bea995e3830263", size = 363806, upload-time = "2025-08-14T17:11:18.29Z" }, + { url = "https://files.pythonhosted.org/packages/c0/91/a43308dc82a0e32d80cd0dfdcfca401ecbd0f431ab45f24e48bb97b7800d/sentry_sdk-2.35.2-py2.py3-none-any.whl", hash = "sha256:38c98e3cbb620dd3dd80a8d6e39c753d453dd41f8a9df581b0584c19a52bc926", size = 363975, upload-time = "2025-09-01T11:00:56.574Z" }, ] [[package]] @@ -4686,7 +4747,7 @@ name = "shapely" version = "2.1.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy" }, + { name = "numpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ca/3c/2da625233f4e605155926566c0e7ea8dda361877f48e8b1655e53456f252/shapely-2.1.1.tar.gz", hash = "sha256:500621967f2ffe9642454808009044c21e5b35db89ce69f8a2042c2ffd0e2772", size = 315422, upload-time = "2025-05-19T11:04:41.265Z" } wheels = [ @@ -4915,7 +4976,7 @@ name = "yapf" version = "0.43.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "platformdirs" }, + { name = "platformdirs", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/23/97/b6f296d1e9cc1ec25c7604178b48532fa5901f721bcf1b8d8148b13e5588/yapf-0.43.0.tar.gz", hash = "sha256:00d3aa24bfedff9420b2e0d5d9f5ab6d9d4268e72afbf59bb3fa542781d5218e", size = 254907, upload-time = "2024-11-14T00:11:41.584Z" } wheels = [